-
Notifications
You must be signed in to change notification settings - Fork 0
/
PathFinderRead.java
231 lines (188 loc) · 9.25 KB
/
PathFinderRead.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
package org.usfirst.frc.team4201.robot.commands.autonomous;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import jaci.pathfinder.Pathfinder;
import jaci.pathfinder.Trajectory;
import jaci.pathfinder.Waypoint;
import jaci.pathfinder.followers.EncoderFollower;
import org.usfirst.frc.team4201.robot.Robot;
import org.usfirst.frc.team4201.robot.interfaces.Shuffleboard;
import java.io.*;
/**
*
*/
public class PathFinderRead extends Command {
double max_vel = 2;
double kP = 1;
double kD = 0.2;
Trajectory leftTrajectory;
Trajectory rightTrajectory;
EncoderFollower left;
EncoderFollower right;
boolean end = false;
Timer stopwatch;
Waypoint[] points;
String filename;
boolean first = false;
Notifier periodicRunnable;
public PathFinderRead(String filename, boolean first,
double maxVel, double kP, double kD) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.filename = filename;
this.first = first;
this.max_vel = maxVel;
this.kP = kP;
this.kD = kD;
}
public PathFinderRead(String filename, boolean first,
double maxVel, double kP) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.filename = filename;
this.first = first;
this.max_vel = maxVel;
this.kP = kP;
}
public PathFinderRead(String filename, boolean first, double maxVel) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.filename = filename;
this.first = first;
this.max_vel = maxVel;
}
public PathFinderRead(String filename, boolean first) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.filename = filename;
this.first = first;
}
public PathFinderRead(String filename) {
// Use requires() here to declare subsystem dependencies
requires(Robot.driveTrain);
this.filename = filename;
}
// Called just before this Command runs the first time
protected void initialize() {
// Used to print status to dashboard for debugging
Shuffleboard.putString("Pathfinder", "PathFinder Status", "Initializing...");
try {
// Try to read the .csv files
File leftFile = new File("/media/sda1/Pathfinder/" + filename + "_Left.csv");
Trajectory lT = Pathfinder.readFromCSV(leftFile);
leftTrajectory = lT;
File rightFile = new File("/media/sda1/Pathfinder/" + filename + "_Right.csv");
Trajectory rT = Pathfinder.readFromCSV(rightFile);
rightTrajectory = rT;
Shuffleboard.putString("Pathfinder", "PathFinder Read", "Trajectory Read Success!");
this.setTimeout(lT.segments.length * 0.05 * 1.1);
} catch (Exception e) {
// Handle it how you want
/*
if(first){
File leftFile = new File("/media/sda1/Pathfinder/straightCalibration_Left.csv");
Trajectory lT = Pathfinder.readFromCSV(leftFile);
leftTrajectory = lT;
File rightFile = new File("/media/sda1/Pathfinder/straightCalibration_Right.csv");
Trajectory rT = Pathfinder.readFromCSV(rightFile);
rightTrajectory = rT;
Shuffleboard.putString("Pathfinder", "PathFinder Read" , "Trajectory Read Failed!");
}
*/
}
// This resets the trajectory counts so you can run autos successively without redeploying the robot code.
// This is in a try/catch statement because the first path loaded will always be null
try {
left.reset();
right.reset();
} catch (Exception e) {
}
// Configure encoder classes to follow the trajectories
left = new EncoderFollower(leftTrajectory);
right = new EncoderFollower(rightTrajectory);
Shuffleboard.putString("Pathfinder", "PathFinder Status", "Enabling...");
left.configureEncoder(Robot.driveTrain.getLeftEncoderValue(), 1440, 0.1667); //0.1823 // 360 enc ticks per rev * 4x quad enc ? 0.1016
right.configureEncoder(Robot.driveTrain.getRightEncoderValue(), 1440, 0.1667); //0.1823 // 0.1016 4 inches in meters - undershoot
// The A value here != max_accel. A here is an acceleration gain (adjusting acceleration to go faster/slower), while max_accel is the max acceleration of the robot.
// Leave A here alone until robot is reaching its target, then adjust to get it to go faster/slower (typically a small value like ~0.03 is used).
// Usually, you wont have to adjust this though.
left.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);
right.configurePIDVA(kP, 0, kD, 1 / max_vel, 0);
// Initialize the timer & Notifier
stopwatch = new Timer();
periodicRunnable = new Notifier(new PeriodicRunnable());
// Start the stopwatch and the notifier (notifier will be called every 0.05 second to ensure trajectory is read properly)
stopwatch.start();
periodicRunnable.startPeriodic(0.05);
}
// Called repeatedly when this Command is scheduled to run
// Not used. run() from the Runnable is used instead
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return end || isTimedOut();
}
// Called once after isFinished returns true
protected void end() {
periodicRunnable.stop();
stopwatch.stop();
Robot.driveTrain.setDriveOutput(0, 0);
Shuffleboard.putString("Pathfinder", "PathFinder Status", "Command Exited");
Shuffleboard.putNumber("Pathfinder", "Path Time", stopwatch.get());
if (first) {
try {
FileWriter writer = new FileWriter("/media/sda1/Pathfinder/calibrationFile.txt", true);
BufferedWriter bufferedWriter = new BufferedWriter(writer);
bufferedWriter.write("Left Enc. Count: " + Robot.driveTrain.getLeftEncoderValue());
bufferedWriter.newLine();
bufferedWriter.write("Right Enc. Count: " + Robot.driveTrain.getRightEncoderValue());
bufferedWriter.close();
} catch (Exception e) {
DriverStation.reportError("Error: Could not write to calibration file", false);
}
}
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
this.end();
}
class PeriodicRunnable implements Runnable {
int segment = 0;
@Override
public void run() {
// TODO Auto-generated method stub
Shuffleboard.putString("Pathfinder", "PathFinder Status", "Running...");
Shuffleboard.putNumber("Pathfinder", "Segment", segment++);
// Calculate the current motor outputs based on the trajectory values + encoder positions
double l = left.calculate(Robot.driveTrain.getLeftEncoderValue());
double r = right.calculate(Robot.driveTrain.getRightEncoderValue());
Shuffleboard.putNumber("Pathfinder", "PathFinder L", l);
Shuffleboard.putNumber("Pathfinder", "PathFinder R", r);
Shuffleboard.putNumber("Pathfinder", "PathFinder H", Pathfinder.r2d(left.getHeading()));
// Adjust a turn value based on the gyro's heading + the trajectory's heading. Note that we only use the left's heading, but left/right would be the same since they're following the same path, but separated by wheelbase distance.
double angleDifference = 0;
double turn = 0;
try {
angleDifference = Pathfinder.boundHalfDegrees(Pathfinder.r2d(left.getHeading()) + Robot.driveTrain.spartanGyro.getAngle());
turn = 2 * (-1.0 / 80.0) * angleDifference;
} catch (Exception e) {
angleDifference = Pathfinder.boundHalfDegrees(Pathfinder.r2d(left.getHeading()));
turn = 2 * (-1.0 / 80.0) * angleDifference;
}
Shuffleboard.putNumber("Pathfinder", "PathFinder T", turn);
Shuffleboard.putNumber("Pathfinder", "PathFinder L output", l + turn);
Shuffleboard.putNumber("Pathfinder", "PathFinder R output", r - turn);
Shuffleboard.putNumber("Pathfinder", "Timer", stopwatch.get());
// Set the output to the motors
Robot.driveTrain.setDirectDriveOutput(l + turn, r - turn);
// Continue sending output values until the path has been completely followed.
if (left.isFinished() && right.isFinished() && (Math.abs(angleDifference) < 4)) {
end = true;
}
}
}
}