├── .classpath
├── .project
├── .settings
    └── org.eclipse.jdt.core.prefs
├── Images
    ├── Image1.png
    ├── Image10.png
    ├── Image2.png
    ├── Image3.png
    ├── Image4.png
    ├── Image5.png
    ├── Image6.png
    ├── Image7.png
    ├── Image8.png
    └── Image9.png
├── README.md
└── src
    └── usfirst
        └── frc
            └── team2168
                └── robot
                    ├── FalconLinePlot.java
                    └── FalconPathPlanner.java


/.classpath:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <classpath>
3 | 	<classpathentry kind="src" path="src"/>
4 | 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.7"/>
5 | 	<classpathentry kind="output" path="bin"/>
6 | </classpath>
7 | 


--------------------------------------------------------------------------------
/.project:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <projectDescription>
 3 | 	<name>PathPlanner</name>
 4 | 	<comment></comment>
 5 | 	<projects>
 6 | 	</projects>
 7 | 	<buildSpec>
 8 | 		<buildCommand>
 9 | 			<name>org.eclipse.jdt.core.javabuilder</name>
10 | 			<arguments>
11 | 			</arguments>
12 | 		</buildCommand>
13 | 	</buildSpec>
14 | 	<natures>
15 | 		<nature>org.eclipse.jdt.core.javanature</nature>
16 | 	</natures>
17 | </projectDescription>
18 | 


--------------------------------------------------------------------------------
/.settings/org.eclipse.jdt.core.prefs:
--------------------------------------------------------------------------------
 1 | eclipse.preferences.version=1
 2 | org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
 3 | org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.7
 4 | org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
 5 | org.eclipse.jdt.core.compiler.compliance=1.7
 6 | org.eclipse.jdt.core.compiler.debug.lineNumber=generate
 7 | org.eclipse.jdt.core.compiler.debug.localVariable=generate
 8 | org.eclipse.jdt.core.compiler.debug.sourceFile=generate
 9 | org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
10 | org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
11 | org.eclipse.jdt.core.compiler.source=1.7
12 | 


--------------------------------------------------------------------------------
/Images/Image1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image1.png


--------------------------------------------------------------------------------
/Images/Image10.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image10.png


--------------------------------------------------------------------------------
/Images/Image2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image2.png


--------------------------------------------------------------------------------
/Images/Image3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image3.png


--------------------------------------------------------------------------------
/Images/Image4.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image4.png


--------------------------------------------------------------------------------
/Images/Image5.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image5.png


--------------------------------------------------------------------------------
/Images/Image6.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image6.png


--------------------------------------------------------------------------------
/Images/Image7.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image7.png


--------------------------------------------------------------------------------
/Images/Image8.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image8.png


--------------------------------------------------------------------------------
/Images/Image9.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/KHEngineering/SmoothPathPlanner/11059aa2ec314ba20b364aeea3c968aca2672b49/Images/Image9.png


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | SmoothPathPlanner
  2 | =================
  3 | 
  4 | 
  5 | After the Cheesy Poofs (FRC 254) put on an amazing display of robot path following during the 2014 FRC championship finals, a lot of buzz has been generated by other teams wishing to implement a similar feature on their robot. The poofs released their code and the community learned they used 5th order polynomial splines in order to find the best fit smooth curve from POint A to Point B.
  6 | 
  7 | Path planning has been the primary focus in many other fields outside of FRC. For example in Game AI, generating smooths paths for the main character to follow when going around obstacles needs to be calculated in real-time. In other robotics fields such as autonomous car navigation, many smooth paths need to be generated simultaneously, and the "best" path choosen.
  8 | 
  9 | These fields have spent many years adopting and optimizing the field of motion planning for their specific application. However, what they have in common is that the calculations for generating the path needs to be performed in real-time, and typically are performed on inexpensive embedded devices, such as gaming consoles or car computers. As part of my PhD thesis (developing a controller for an Autonomous Car) I had the opportunity to create a simplified path planner for real-time mobile robot applications.
 10 | 
 11 | This class is a port of the code I developed for my PhD thesis, to perform the function of path calculation on a mobile, skid-steer robot very quickly. The algorithms provided here rely on algorithms used in optimal control theory to converge to a solution very quickly. The code here uses the "gradient descent" optimization technique to coerce a simple waypoint path, into a smooth trajectory. 
 12 | 
 13 | Once the smooth trajectory is determined, parallel paths for the left and right wheels are calculated based on the robots trackwidth distance, and corresponding velocities for each of the wheels are determined. As long as the robot has independant speed controllers for the left and right side of the drivetrain, the user can "play" through the velocity profile as if they are setpoints and the robot should drive the corresponding path.
 14 | 
 15 | The function takes in just 3 paramters to get started and boost a very simple user interface.
 16 | 
 17 | 1. `TotalTime` - is the time you wish the robot to complete the path, this is used to calculate the velocity
 18 | 2. `Time Step` - this is the rate at which the controller on your robot runs. Typically for FRC we run our controllers at a 100ms rate in a separate thread from the main Robot.
 19 | 3. `TrackWidth` - the distance between the left and right wheels of the robot. This is used to calculate the left and right wheel paths.
 20 | 	
 21 | The waypoints are simply global positions you would like your robot to drive, specified as a 2D array. The order of the points specify the path which the user wishes to drive, and the smooth path is generated accordingly. The smooth path generated also uses the Max Time, and controller periodic rate to automatically create the proper length velocity array so the user simply steps through each element once every time step. The end result is that the robot drives along the path, depending how well your speed controllers are tuned. So for example, if you have a controller running at 100ms, and you want a path that will take no more than 5 seconds to travsers, the algorithm will provide the left and right velocities as arrays with no more than 5/0.1 = 50 points. Each point will contain the velocity the robot should be at the time to make the end position by the end of travel.
 22 | 
 23 | Here is an an example to generate a path using the default paramters. 
 24 | 
 25 | ```java
 26 | //create waypoint path
 27 | double[][] waypoints = new double[][]{
 28 | 	{1, 1},
 29 | 	{5, 1},
 30 | 	{9, 12},
 31 | 	{12, 9},
 32 | 	{15, 6},
 33 | 	{19, 12}
 34 | }; 
 35 | 
 36 | double totalTime = 8; //max seconds we want to drive the path
 37 | double timeStep = 0.1; //period of control loop on Rio, seconds
 38 | double robotTrackWidth = 2; //distance between left and right wheels, feet
 39 | 
 40 | FalconPathPlanner path = new FalconPathPlanner(waypoints);
 41 | path.calculate(totalTime, timeStep, robotTrackWidth);
 42 | ```
 43 | 
 44 | After the `calculate` method is called, the smooth path and left and right velocities are all calculated and stored into the class data members. Below is the list of each class path variable. Note, all of the data members of this class are public. While this is usually not a good idea, I think for this case, and how we use these classes in FRC, it makes sense to leave these as public so the user can access them and modify them as needed. Note `path` is the class instantiated from the example above.
 45 | 
 46 | ```Java
 47 | path.origPath; // 2D array of doubles which contains the original waypoint path provided
 48 | path.nodeOnlyPath; // 2D array of doubles which filters the original path and only keeps points which
 49 | 		   //cause the path to change direction (i.e. all intermediate points on a straight line
 50 | 		   //are removed)
 51 | path.smoothPath; //2D array of doubles represents the smooth path for the center of the robot.
 52 | path.leftPath; //2D array of doubles represents the left wheel path (based on track width of robot) 
 53 | path.rightPath; //2D array of doubles represents the right wheel path (based on track width of robot)
 54 | 	
 55 | path.smoothCenterVelocity; //2D array of doubles which contains a smooth velocity profile of the smoothPath above
 56 | path.smoothRightVelocity; //2D array of doubles which contains a smooth velocity profile of the rightPath above
 57 | path.smoothLeftVelocity; //2D array of doubles which contains a smooth velocity profile of the leftPath above
 58 | ```
 59 | 
 60 | At this point all you need to do is pass `path.smoothRightVelocity` and `path.smoothLeftVelocity` to the Robots left and right wheel speed controllers. "Play" through the array updating the setpoint of the controller with the next element of the array each time step.
 61 | 
 62 | >Note: ALL PATH 2 DIMENSIONAL ARRAYS CONTAIN THE X-COORDINATE IN THE FIRST DIMENSION, AND THE Y COODINATE IN THE SECOND DIMENSION. SO `path.rightPath[i][0]` would be the ith X- coodinate, and `path.rightPath[i][1]` would be the ith Y- coordinate of the right wheel path for example. THE UNITS OF THESE ARE THE GLOBAL UNITS ASSUMED BY THE USER.
 63 | 
 64 | >Note: ALL VELOCITY 2 DIMENSIONAL ARRAYS CONTAIN THE TIME VECOTOR IN THE FIRST DIMENSION, AND THE VELOCITY MAGNITUDE IN THE SECOND DIMENSION. So `path.smoothLeftVelocity[i][0]` would be the ith time step value in seconds, and `path.smoothLeftVelocity[i][1]` would be the ith velocity magnitude in units per second. **When attemping to command the speed controller, the user only needs to pass the `path.smoothLeftVelocity[i][1] array, which is a single dimension array of doubles to the speed controller.** 
 65 | 
 66 | Furthermore, the algorithm also implements a smooth start/stop routine and automatically ramps up and down to a zero velocity so at the end of travel the robot slows down to zero. The coerceion to zero velocity takes in to account the total distance needed to travel and modifies the velocity accordingly. 
 67 | 
 68 | Typically when I write complex algorithms, I tend to put the Big O notation in the JavaDoc comments of each method and I also write down a pseudocode to help guide me as I program the algorithm. Typically I remove these comments in the final product, but I decided to leave them in to help people follow my logic, and see which Algorithms I have optimized, and which algorithms could use more optimization. Most algorithms have been optimized to linear Big O notation.
 69 | 
 70 | I have tested this algorithm on a 2015 RoboRio and for a path containing  
 71 | 
 72 | <Include timing assessment>
 73 | 
 74 | ## Using the Included Grapher
 75 | 
 76 | I have also included a very crude graphing class to help visualize the output. The graphing class is also written using native Java SE classes and will run on any desktop. 
 77 | 
 78 | >Note: Because the RoboRio is headless (meaning no display), the Graph functions can not run on the RoboRio.  Check `if(!GraphicsEnvironment.isHeadless())` before graphing this way the code is completely portable and will run on both desktop and RoboRio.
 79 | 
 80 | So let’s work through an example of how to use this class:
 81 | 
 82 | If we want to graph the output of the path above we can do the following and produce the following plot:
 83 | ```Java
 84 | FalconLinePlot fig1 = new FalconLinePlot(path.nodeOnlyPath,Color.blue,Color.green);
 85 | ```
 86 | 
 87 | ![](Images/Image1.png)
 88 | 
 89 | The plot above isn’t that helpful. However we can configure it to be more useful to us. Since this is the plot of the global path of the robot. Let’s change the X- and Y- axis to reflect the dimensions of half of the FRC field (24 x 27ft) and lets add some labels and grid lines. 
 90 | 
 91 | ```Java
 92 | fig1.yGridOn();
 93 | fig1.xGridOn();
 94 | fig1.setYLabel("Y (feet)");
 95 | fig1.setXLabel("X (feet)");
 96 | fig1.setTitle("Top Down View of FRC Field (24ft x 27ft) \n shows global position of robot path, along with left and right wheel trajectories");
 97 | 
 98 | //force graph to show 1/2 field dimensions of 24ft x 27 feet
 99 | fig1.setXTic(0, 27, 1);
100 | fig1.setYTic(0, 24, 1);
101 | ```
102 | ![](Images/Image2.png)
103 | 
104 | The above figure is more useful. It shows the general path we wish the robot to take. Starting at our waypoint {1,1} and ending at {19,12}. This plot is in feet. The real usefulness of the plotter function is the ability to add additional plots to one figure. You can add an unlimited amount of charts using the `addData` method. There are also options to specify line color, and marker color. If you do not wish to have lines or markers, set them to `null`.
105 | 
106 | Lets add the smooth center path the algorithm calculated to this plot. Which gives the following:
107 | ```Java
108 | fig1.addData(path.smoothPath, Color.red, Color.blue);
109 | ```
110 | ![Alt desc](Images/Image3.png)
111 | We can see the smooth path algorithm does two things. It creates many more data points and provides smooth transitions around the corners.  The number of data points created is based on the max Time you wish the robot to complete the path, and the time step of the controller. 
112 | 
113 | We can also add the left and right path trajectories to the plot to provide all the useful information we need in one chart. 
114 | 
115 | ```Java
116 | fig1.addData(path.leftPath, Color.magenta);
117 | fig1.addData(path.rightPath, Color.magenta);
118 | ```
119 | 
120 | ![](Images/Image4.png)
121 | 
122 | I choose this example for a reason, and we can see some tight turns on the robot in the example. (Maybe too tight) We will fix those later (todo: see section here), but first let’s see what the velocity looks like for this path. 
123 | 
124 | ```Java
125 | FalconLinePlot fig2 = new FalconLinePlot(path.smoothCenterVelocity,null,Color.blue);
126 | fig2.yGridOn();
127 | fig2.xGridOn();
128 | fig2.setYLabel("Velocity (ft/sec)");
129 | fig2.setXLabel("time (seconds)");
130 | fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Cyan, Right = Magenta");
131 | fig2.addData(path.smoothRightVelocity, Color.magenta);
132 | fig2.addData(path.smoothLeftVelocity, Color.cyan);
133 | ```
134 | ![](Images/Image5.png)
135 | 
136 | 
137 | This is where you need to determine, if the velocities shown can be accomplished by your robot. Looks like the max and min velocity is achievable for most drivetrains so let’s continue. We also note there is a relatively smooth transition between data points. If your velocity isn’t smooth, modify the way points so that it is smooth. The robot will not react well to instant changes in velocity.  Note the total time is 7.2 seconds which is less than the `8` second requirement we provided. If we increase or decrease the time requirement, the magnitude of the velocity will decrease or increase, respectively.
138 | 
139 | ## Let’s recreate the Cheezy Poof Path
140 | As another example we are going to try and replicate the smooth path used by FRC Team 254 during the 2014 FRC Einstein Finals, Match 3.
141 | 
142 | ```Java
143 | //Lets create a bank image
144 | FalconLinePlot fig3 = new FalconLinePlot(new double[][]{{0.0,0.0}});
145 | fig3.yGridOn();
146 | fig3.xGridOn();
147 | fig3.setYLabel("Y (feet)");
148 | fig3.setXLabel("X (feet)");
149 | fig3.setTitle("Top Down View of FRC Field (28ft x 27ft) \n shows global position of robot path, along with left and right wheel trajectories");
150 | 
151 | //force graph to show 1/2 field dimensions of 24.8ft x 27 feet
152 | double fieldWidth = 32.0;
153 | fig3.setXTic(0, 27, 1);
154 | fig3.setYTic(0, fieldWidth, 1);
155 | ```  
156 | 
157 | ![](Images/Image6.png)
158 | 
159 | 
160 | We can add the Goal wall, and low goals to the view.
161 | 
162 | ```Java
163 | //lets add field markers to help visual
164 | //http://www.usfirst.org/sites/default/files/uploadedFiles/Robotics_Programs/FRC/Game_and_Season__Info/2014/fe-00037_RevB.pdf
165 | //Goal line
166 | double[][] goalLine = new double[][] {{26.5,0}, {26.5, fieldWidth}};
167 | fig3.addData(goalLine, Color.black);
168 | 			
169 | //Low Goals roughly 33 inch x 33 inch and 24.6 ft apart (inside to inside)
170 | double[][] leftLowGoal = new double[][]{
171 | {26.5, fieldWidth/2 + 24.6/2},
172 | {26.5, (fieldWidth)/2 + 24.6/2 + 2.75},
173 | {26.5 - 2.75, fieldWidth/2 + 24.6/2 + 2.75},
174 | {26.5 - 2.75, fieldWidth/2 + 24.6/2},
175 | {26.5, fieldWidth/2 + 24.6/2},
176 | };
177 | 			
178 | double[][] rightLowGoal = new double[][]{
179 | {26.5, fieldWidth/2 - 24.6/2},
180 | {26.5, fieldWidth/2 - 24.6/2 - 2.75},
181 | {26.5 - 2.75, fieldWidth/2 - 24.6/2 - 2.75},
182 | {26.5 - 2.75, fieldWidth/2 - 24.6/2},
183 | {26.5, fieldWidth/2 - 24.6/2},
184 | };
185 | 			
186 | fig3.addData(leftLowGoal, Color.black);
187 | fig3.addData(rightLowGoal, Color.black);
188 | 
189 | //Auto Line
190 | double[][] autoLine = new double[][] {{26.5-18,0}, {26.5-18, fieldWidth}};
191 | fig3.addData(autoLine, Color.black);
192 | ```
193 | ![](Images/Image7.png)
194 | 
195 | Using the above plot, we can create the following waypoint trajectory to replicate the path taken by FRC 254 on Einstein Final Match 3;
196 | 
197 | ```Java
198 | 		double[][] CheesyPath = new double[][]{
199 | 			{7,16},
200 | 			{11,16},
201 | 			{17,28},
202 | 			{22,28},
203 | 	};
204 | 		
205 | 		double totalTime = 4.5; //seconds
206 | 		double timeStep = 0.1; //period of control loop on Rio, seconds
207 | 		double robotTrackWidth = 2; //distance between left and right wheels, feet
208 | 
209 | 		final FalconPathPlanner path = new FalconPathPlanner(CheesyPath);
210 | 		path.calculate(totalTime, timeStep, robotTrackWidth);
211 | 		
212 | 		fig3.addData(path.nodeOnlyPath,Color.blue,Color.green);
213 | ```
214 | ![](Images/Image8.png)
215 | 
216 | 
217 | Adding all of the other path parameters as well:
218 | 
219 | ```Java
220 | 		//add all other paths
221 | 		fig3.addData(path.smoothPath, Color.red, Color.blue);
222 | 		fig3.addData(path.leftPath, Color.magenta);
223 | 		fig3.addData(path.rightPath, Color.magenta);
224 | ```
225 | ![](Images/Image9.png)
226 | 
227 | 
228 | We also plot the Velocity of this chart:
229 | ```Java
230 | 		//Velocity
231 | 		FalconLinePlot fig4 = new FalconLinePlot(path.smoothCenterVelocity,null,Color.blue);
232 | 		fig4.yGridOn();
233 | 		fig4.xGridOn();
234 | 		fig4.setYLabel("Velocity (ft/sec)");
235 | 		fig4.setXLabel("time (seconds)");
236 | 		fig4.setTitle("Velocity Profile for Left and Right Wheels \n Left = Cyan, Right = Magenta");
237 | 		fig4.addData(path.smoothRightVelocity, Color.magenta);
238 | 		fig4.addData(path.smoothLeftVelocity, Color.cyan);
239 | ```
240 | 
241 | ![](Images/Image10.png)
242 | 
243 | 
244 | This is actually a really nice velocity curve. The transactions are very smooth for each wheel. Assuming our robot can meet these velocities we will traverse the smooth path in 4.2 seconds and stop! You will notice the left wheel slows down first to allow for the slight left turn in the beginning, and then the right wheel slows down to allow for the slight right turn.
245 | 
246 | At this point, all we need to do is pass `path.smoothLeftVelocity[i][1]` and ``path.smoothRightVelocity[i][1]` to our robot Left and right wheel speed controllers, and update the setpoint to the next value in the array each robot loop iteration. The robot will “play” through the array and drive the smooth path. 
247 | 
248 | The complete code for this example is below:
249 | 
250 | ```Java
251 | 		/***Poof Example***/
252 | 
253 | 		//Lets create a bank image
254 | 		FalconLinePlot fig3 = new FalconLinePlot(new double[][]{{0.0,0.0}});
255 | 		fig3.yGridOn();
256 | 		fig3.xGridOn();
257 | 		fig3.setYLabel("Y (feet)");
258 | 		fig3.setXLabel("X (feet)");
259 | 		fig3.setTitle("Top Down View of FRC Field (30ft x 27ft) \n shows global position of robot path, along with left and right wheel trajectories");
260 | 
261 | 
262 | 		//force graph to show 1/2 field dimensions of 24.8ft x 27 feet
263 | 		double fieldWidth = 32.0;
264 | 		fig3.setXTic(0, 27, 1);
265 | 		fig3.setYTic(0, fieldWidth, 1);
266 | 
267 | 
268 | 		//lets add field markers to help visual
269 | 		//http://www.usfirst.org/sites/default/files/uploadedFiles/Robotics_Programs/FRC/Game_and_Season__Info/2014/fe-00037_RevB.pdf
270 | 		//Goal line
271 | 		double[][] goalLine = new double[][] {{26.5,0}, {26.5, fieldWidth}};
272 | 		fig3.addData(goalLine, Color.black);
273 | 
274 | 		//Low Goals roughly 33 inch x 33 inch and 24.6 ft apart (inside to inside)
275 | 		double[][] leftLowGoal = new double[][]{
276 | 				{26.5, fieldWidth/2 + 24.6/2},
277 | 				{26.5, (fieldWidth)/2 + 24.6/2 + 2.75},
278 | 				{26.5 - 2.75, fieldWidth/2 + 24.6/2 + 2.75},
279 | 				{26.5 - 2.75, fieldWidth/2 + 24.6/2},
280 | 				{26.5, fieldWidth/2 + 24.6/2},
281 | 		};
282 | 
283 | 		double[][] rightLowGoal = new double[][]{
284 | 				{26.5, fieldWidth/2 - 24.6/2},
285 | 				{26.5, fieldWidth/2 - 24.6/2 - 2.75},
286 | 				{26.5 - 2.75, fieldWidth/2 - 24.6/2 - 2.75},
287 | 				{26.5 - 2.75, fieldWidth/2 - 24.6/2},
288 | 				{26.5, fieldWidth/2 - 24.6/2},
289 | 		};
290 | 
291 | 		fig3.addData(leftLowGoal, Color.black);
292 | 		fig3.addData(rightLowGoal, Color.black);
293 | 
294 | 		//Auto Line
295 | 		double[][] autoLine = new double[][] {{26.5-18,0}, {26.5-18, fieldWidth}};
296 | 		fig3.addData(autoLine, Color.black);
297 | 
298 | 
299 | 		double[][] CheesyPath = new double[][]{
300 | 				{7,16},
301 | 				{11,16},
302 | 				{17,28},
303 | 				{22,28},
304 | 		};
305 | 
306 | 		double totalTime = 4.5; //seconds
307 | 		double timeStep = 0.1; //period of control loop on Rio, seconds
308 | 		double robotTrackWidth = 2; //distance between left and right wheels, feet
309 | 
310 | 		final FalconPathPlanner path = new FalconPathPlanner(CheesyPath);
311 | 		path.calculate(totalTime, timeStep, robotTrackWidth);
312 | 
313 | 		//waypoint path
314 | 		fig3.addData(path.nodeOnlyPath,Color.blue,Color.green);
315 | 
316 | 		//add all other paths
317 | 		fig3.addData(path.smoothPath, Color.red, Color.blue);
318 | 		fig3.addData(path.leftPath, Color.magenta);
319 | 		fig3.addData(path.rightPath, Color.magenta);
320 | 
321 | 
322 | 		//Velocity
323 | 		FalconLinePlot fig4 = new FalconLinePlot(path.smoothCenterVelocity,null,Color.blue);
324 | 		fig4.yGridOn();
325 | 		fig4.xGridOn();
326 | 		fig4.setYLabel("Velocity (ft/sec)");
327 | 		fig4.setXLabel("time (seconds)");
328 | 		fig4.setTitle("Velocity Profile for Left and Right Wheels \n Left = Cyan, Right = Magenta");
329 | 		fig4.addData(path.smoothRightVelocity, Color.magenta);
330 | 		fig4.addData(path.smoothLeftVelocity, Color.cyan);
331 | 
332 | 	}
333 | ```
334 | 
335 | 
336 | 
337 | Modifying the calculation paramters
338 | =================
339 | The "smoothness" of the path is controlled by two paramters: Alpha and Beta. These variables take on default values which I believe will work in most cases and do not need to be modified. However, this section covers how to modify these paramters so that you can generate the exact path you wish to traverse.
340 | 
341 | You can change these paramters individually by calling the setter methods. 
342 | 
343 | ```Java
344 | path.setPathAlpha(0.5);
345 | path.setPathBeta(0.9);
346 | ```
347 | 
348 | After doing-so you will need to call the recalculate menthod to re-calculate the paths using the new paramters
349 | ```Java
350 | path.calculate(totalTime, timeStep, robotTrackWidth);
351 | ```
352 | 
353 | Alpha controls how close to the original line we wish the smooth points to be. Beta controls how curvy we wish our path be. The closer Alpha is to 1, the closer the smooth path will be to the original line, the closer Beta is to 1, the larger the path radii should be. Note, these are conflicting requirements, and thus why it is an optimization algorithm. You can think of it this way, as Alpha increases it wants to push the points toward the original path, as beta increses it want to push the points away from the original line and there is a constant tug of war between them. The algorithm converges when both are happy with where the points get pushed. 
354 | 
355 | Alpha and Beta should be a decimal between 0 and 1. If Alpha is zero regarless of Beta, the output will be a straight line from the starting waypoint to the ending waypoint. 
356 | 
357 | TODO: add image of different params
358 | 
359 | Caution: Depending on the combination of the variables, it is very possible to set the parameters such that the algorithm never convergers! So be careful. In general I have found the following settings to be acceptable for converging. However, they are not guranteed and you should test many datasets to be sure, the algorithm will convege to a solution in all your scenarios. If you decide to change the paramters from their defaults. 
360 | 
361 | If the algorithm does not converge, the program will simply never finish, so it is pretty easy to identify. 
362 | 
363 | In genral if you must change the parameters Set Beta between 0.1 and 0.4 (Do not set to zero), and set Alpha between 0 and 1 to get desired path output, but here is a complete list of recommended paramters.
364 | 
365 | | Alpha                | Beta           | 
366 | | -------------        |-------------:  | 
367 | | Any val from 0 to 1     | 0.0            | 
368 | | Any val from 0 to 1     | 0.1            | 
369 | | Any val from 0 to 1     | 0.2            | 
370 | | Any val from 0 to 1     | 0.3            | 
371 | | Any val from 0 to 1     | 0.4            |
372 | | Any val from 0 to 0.9   | 0.5            | 
373 | | Any val from 0 to 0.7     | 0.6            | 
374 | | Any val from 0 to 0.5     | 0.7            |
375 | | Any val from 0 to 0.3     | 0.8            | 
376 | | Any val from 0 to 0.1     | 0.9            | 
377 | | None                      | 1.0            | 
378 | 
379 | 
380 | <Example of Paths with different Paramters>
381 | 
382 | Real-Time Course Correction
383 | ============================
384 | The algorithm also provides what the robots heading should be at each time stamp, based on the global coordinate system. The user can use these headings to correct the robots course. As long as the robot maintains the appropriate headings and velocities of each wheels, the robot will drive the course outlined.
385 | 
386 | The current implementation of the heading is accumulated, not absolute. This is so that users can use the Gyro class already included with WPI lib (which is also accumulated) as a sensor to detect the robots actual heading and course correct.
387 | 
388 | the heading information is stored in member variable `heading`, so to access it one could call
389 | 
390 | ```java
391 | path.heading[i][0]; //ith time vector for path heading
392 | path.heading[i][1]; //ith point heading in degrees
393 | ```
394 | 
395 | > Note: This algorithm doesn't consider pose of the robot. What that means is it doesn't require an initial or final orientation of the robot, and does not gurantee the final orientation. Once you reach the desintation, you may need to then command your chassis to rotate to the desired final heading.
396 | 
397 | 
398 | Assumptions
399 | =========================
400 | 1. Robot Chassis is a skid-steer platform
401 | 2. Ground robot is to travel on is considered to be an even plane, with no hills, ramps, bumbs, or other obstacles
402 | 3. Robot has independant speed controllers for both sides of the Robot Chassis, tuned to meet the velocity profile changes
403 | 4. Sufficient time is given for the robot to complete the path. 
404 | 


--------------------------------------------------------------------------------
/src/usfirst/frc/team2168/robot/FalconLinePlot.java:
--------------------------------------------------------------------------------
  1 | package usfirst.frc.team2168.robot;
  2 | 
  3 | import java.awt.*;
  4 | import java.awt.datatransfer.Clipboard;
  5 | import java.awt.datatransfer.ClipboardOwner;
  6 | import java.awt.datatransfer.DataFlavor;
  7 | import java.awt.datatransfer.Transferable;
  8 | import java.awt.datatransfer.UnsupportedFlavorException;
  9 | import java.awt.event.ActionEvent;
 10 | import java.awt.event.ActionListener;
 11 | import java.awt.event.MouseAdapter;
 12 | import java.awt.event.MouseEvent;
 13 | import java.awt.geom.*;
 14 | import java.awt.image.BufferedImage;
 15 | import java.io.IOException;
 16 | import java.text.DecimalFormat;
 17 | import java.util.LinkedList;
 18 | 
 19 | import javax.swing.*;
 20 | 
 21 | /**
 22 |  * This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user 
 23 |  * to plot multiple graphs on one figure, control axis dimensions, and specify colors.
 24 |  * 
 25 |  * This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If
 26 |  * a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this
 27 |  * class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user
 28 |  * control over as much as possible, so they can generate the perfect chart.
 29 |  * 
 30 |  * The plotter also features the ability to capture screen shots directly from the right-click menu, this allows
 31 |  * the user to copy and paste plots into reports or other documents rather quickly.
 32 |  * 
 33 |  * This class holds an interface similar to that of Matlab. 
 34 |  * 
 35 |  * This class currently only supports scatterd line charts.
 36 |  * 
 37 |  * @author Kevin Harrilal
 38 |  * @email kevin@team2168.org
 39 |  * @version 1
 40 |  * @date 9 Sept 2014
 41 |  *
 42 |  */
 43 |  
 44 | class FalconLinePlot extends JPanel implements ClipboardOwner{
 45 |   
 46 |     
 47 | 	private static final long serialVersionUID = 3205256608145459434L;
 48 | 	private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge
 49 |     private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge
 50 |     
 51 |     private double upperXtic;
 52 |     private double lowerXtic;
 53 |     private double upperYtic;
 54 |     private double lowerYtic;
 55 |     private boolean yGrid;
 56 |     private boolean xGrid;
 57 |     
 58 |     private double yMax;
 59 |     private double yMin;
 60 |     private double xMax;
 61 |     private double xMin;
 62 |     
 63 |     private int yticCount;
 64 |     private int xticCount;
 65 |     private double xTicStepSize;
 66 |     private double yTicStepSize;
 67 |     
 68 |     boolean userSetYTic;
 69 |     boolean userSetXTic;
 70 |     
 71 |     private String xLabel;
 72 |     private String yLabel;
 73 |     private String titleLabel;
 74 |     protected static int count = 0;
 75 |     
 76 |     JPopupMenu menu = new JPopupMenu("Popup");
 77 |     
 78 |     //Link List to hold all different plots on one graph.
 79 |     private LinkedList<xyNode> link; 
 80 |     
 81 | 
 82 |     /**
 83 |      * Constructor which Plots only Y-axis data.
 84 |      * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
 85 |      */
 86 |     public FalconLinePlot(double[] yData)
 87 |     {
 88 |     	this(null,yData,Color.red);
 89 |     }
 90 |     
 91 |     public FalconLinePlot(double[] yData,Color lineColor, Color marker)
 92 |     {
 93 |     	this(null,yData,lineColor,marker);
 94 |     }
 95 |     
 96 |     /**
 97 |      * Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length.
 98 |      * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
 99 |      * @param yData is an array of double representing the Y-axis values of the data to be plotted.
100 |      */
101 |     public FalconLinePlot(double[] xData, double[] yData)
102 |     {
103 |     	this(xData,yData,Color.red,null);
104 |     }
105 |     
106 |     /**
107 |      * Constructor which Plots chart based on provided x and y axis data. 
108 |      * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
109 |      * is the second dimension.
110 |      */
111 |     public FalconLinePlot(double[][] data)
112 |     {
113 |     	this(getXVector(data),getYVector(data),Color.red,null);
114 |     }
115 |     
116 | /**
117 |  * Constructor which plots charts based on provided x and y axis data in a single two dimensional array.
118 |  * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
119 |  * is the second dimension.
120 |  * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
121 |  * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
122 |  * not have datapoint markers.
123 |  */
124 |     public FalconLinePlot(double[][] data, Color lineColor, Color markerColor)
125 |     {
126 |     	this(getXVector(data),getYVector(data),lineColor,markerColor);
127 |     }
128 |     
129 |     /**
130 |      * Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line.
131 |      * Data markers are not displayed.
132 |      * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
133 |      * @param yData is an array of double representing the Y-axis values of the data to be plotted.
134 |      * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
135 |      */
136 |     public FalconLinePlot(double[] xData, double[] yData,Color lineColor)
137 |     {
138 |     	this(xData,yData,lineColor,null);
139 |     }
140 |     
141 | 
142 |     
143 |     /**
144 |      * Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user 
145 |      * can also specify the color of the adjoining line and the color of the datapoint maker.
146 |      * @param xData is an array of doubles representing the X-axis values of the data to be plotted.
147 |      * @param yData is an array of double representing the Y-axis values of the data to be plotted.
148 |      * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
149 |      * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
150 |      * not have datapoint markers.
151 |      */
152 |     public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor)
153 |     {
154 |     	xLabel = "X axis";
155 |     	yLabel = "Y axis";
156 |     	titleLabel = "Title";
157 |     	
158 |        upperXtic = -Double.MAX_VALUE;
159 |        lowerXtic = Double.MAX_VALUE;
160 |        upperYtic = -Double.MAX_VALUE;
161 |        lowerYtic = Double.MAX_VALUE;
162 |        xticCount = -Integer.MAX_VALUE;
163 |        
164 |        this.userSetXTic = false;
165 |        this.userSetYTic = false;
166 |     	
167 |     	link = new LinkedList<xyNode>();
168 |     	
169 |     	addData(xData, yData, lineColor,markerColor);
170 |     	
171 |     	count ++;
172 |     	JFrame g = new JFrame("Figure " + count);
173 |         g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
174 |         g.add(this);
175 |         g.setSize(600,400);
176 |         g.setLocationByPlatform(true);
177 |         g.setVisible(true);
178 |          
179 |         menu(g,this);	
180 |     }
181 |     
182 |     /**
183 |      * Adds a plot to an existing figure.  
184 |      * @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
185 |      * @param color is the color the user wishes to be displayed for the line connecting each datapoint
186 |      */
187 |     
188 |     public void addData(double[] y, Color lineColor)
189 |     {
190 |     	addData(y, lineColor, null);
191 |     }
192 |     
193 |     public void addData(double[] y, Color lineColor, Color marker)
194 |     {
195 |     	//cant add y only data unless all other data is y only data
196 |     	for(xyNode data: link)
197 |     		if(data.x != null)
198 |     			throw new Error ("All previous chart series need to have only Y data arrays");
199 |     	
200 |     	addData(null,y,lineColor, marker);
201 |     }
202 |     
203 |     public void addData(double[] x, double[] y, Color lineColor)
204 |     {
205 |     	addData(x,y,lineColor,null);
206 |     }
207 |     
208 |     
209 |     public void addData(double[][] data, Color lineColor)
210 |     {
211 |     	addData(getXVector(data),getYVector(data),lineColor,null);
212 |     }
213 |     
214 |     public void addData(double[][] data, Color lineColor, Color marker)
215 |     {
216 |     	addData(getXVector(data),getYVector(data),lineColor,marker);
217 |     }
218 |     
219 |     public void addData(double[] x, double[] y, Color lineColor, Color marker)
220 |     { 	
221 |     	xyNode Data = new xyNode();
222 |     	    	
223 |     	//copy y array into node
224 |     	Data.y = new double[y.length];
225 |     	Data.lineColor = lineColor;
226 |     	
227 |     	if(marker == null)
228 |     		Data.lineMarker = false;
229 |     	else
230 |     	{
231 |     		Data.lineMarker = true;
232 |     		Data.markerColor = marker;
233 |     	}
234 |     	for(int i=0; i<y.length; i++)
235 |     		Data.y[i] = y[i];
236 |     	
237 |     	//if X is not null, copy x
238 |     	if(x != null)
239 |     	{
240 |         	//cant add x, and y data unless all other data has x and y data
241 |         	
242 |         	for(xyNode data: link)
243 |         		if(data.x == null)
244 |         			throw new Error ("All previous chart series need to have both X and Y data arrays");
245 |     		
246 |     		if(x.length != y.length)
247 |     			throw new Error("X dimension must match Y dimension");
248 |     		
249 |     		Data.x = new double[x.length];
250 |     		
251 |     		for(int i=0; i<x.length; i++)
252 |         		Data.x[i] = x[i];
253 |     		
254 |     	}
255 |     	link.add(Data);
256 |     }
257 |  
258 |     /**
259 |      * Main method which paints the panel and shows the figure.
260 |      * 
261 |      */
262 |     @Override
263 |     protected void paintComponent(Graphics g) {
264 |         super.paintComponent(g);
265 |         Graphics2D g2 =  (Graphics2D)g;
266 |         g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
267 |         
268 |         int w = getWidth();
269 |         int h = getHeight();
270 |     
271 |         // Draw X and Y lines axis.
272 |         Line2D yaxis = new Line2D.Double(xPAD, yPAD, xPAD, h-yPAD);
273 |         Line2D.Double xaxis = new Line2D.Double(xPAD, h-yPAD, w-xPAD, h-yPAD);
274 |         g2.draw(yaxis); 
275 |         g2.draw(xaxis);
276 |         
277 |         //find Max Y limits
278 |         getMinMax(link);
279 |         
280 |         //draw ticks
281 |         drawYTickRange(g2, yaxis, 15, yMax, yMin);
282 |         drawXTickRange(g2, xaxis, 15, xMax, xMin);
283 |         
284 |         //plot all data
285 |         plot(g2);
286 |         
287 |         //draw x and y labels
288 |         setXLabel(g2, xLabel);
289 |         setYLabel(g2, yLabel);
290 |         setTitle(g2, titleLabel);
291 |         
292 |     }
293 |     
294 |     void setXTic(double lowerBound, double upperBound, double stepSize)
295 |     {
296 |     	this.userSetXTic = true;
297 |     	
298 |     	this.upperXtic=upperBound;
299 |     	this.lowerXtic=lowerBound;
300 |     	this.xTicStepSize = stepSize;
301 |     }
302 |     
303 |     public void setYTic(double lowerBound, double upperBound, double stepSize)
304 |     {
305 |     	this.userSetYTic = true;
306 |     	
307 |     	this.upperYtic=upperBound;
308 |     	this.lowerYtic=lowerBound;
309 |     	this.yTicStepSize = stepSize;
310 |     }
311 |     
312 |     private void plot(Graphics2D g2)
313 |     {
314 |     	
315 |     	int w = super.getWidth();
316 |     	int h = super.getHeight();
317 |     	
318 |     	Color tempC = g2.getColor();
319 |     	
320 |     	//loop through list and plot each
321 |     	for(int i=0; i<link.size(); i++)
322 |     	{
323 | 	        // Draw lines.
324 | 	        double xScale = (double)(w - 2*xPAD)/(upperXtic-lowerXtic);
325 | 	        double yScale = (double)(h - 2*yPAD)/(upperYtic-lowerYtic);
326 | 	        
327 | 	        for(int j = 0; j < link.get(i).y.length-1; j++) 
328 | 	        {
329 | 	        	double x1;
330 | 	        	double x2;
331 | 	        	
332 | 	            if(link.get(i).x==null)
333 | 	            {
334 | 	            	x1 = xPAD + j*xScale;
335 | 	            	x2 = xPAD + (j+1)*xScale;
336 | 	            }
337 | 	            else
338 | 	            {
339 | 	            	x1 = xPAD + xScale*link.get(i).x[j] + lowerXtic*xScale;
340 | 	            	x2 = xPAD + xScale*link.get(i).x[j+1] + lowerXtic*xScale;
341 | 	            }
342 | 	            
343 | 	            double y1 = h - yPAD - yScale*link.get(i).y[j] + lowerYtic*yScale;
344 | 	            
345 | 	            double y2 = h - yPAD - yScale*link.get(i).y[j+1] + lowerYtic*yScale;
346 | 	            g2.setPaint(link.get(i).lineColor);
347 | 	            g2.draw(new Line2D.Double(x1, y1, x2, y2));
348 | 	            
349 | 	            if(link.get(i).lineMarker)
350 | 	            {
351 | 	            	g2.setPaint(link.get(i).markerColor);
352 | 	            	g2.fill(new Ellipse2D.Double(x1-2, y1-2, 4, 4));
353 | 	            	g2.fill(new Ellipse2D.Double(x2-2, y2-2, 4, 4));
354 | 	            }
355 | 	      
356 | 	        }
357 | 	        	        	
358 |     	}
359 |     	
360 |     	g2.setColor(tempC);
361 |     }
362 |     
363 |     /**
364 |      * need to optimize for loops
365 |      * @param list
366 |      */
367 |     private void getMinMax(LinkedList<xyNode> list)
368 |     {
369 |     	for(xyNode node: list)
370 |     	{
371 |     		double tempYMax = getMax(node.y);
372 |     		double tempYMin = getMin(node.y);
373 |     		
374 |     		if(tempYMin<yMin)
375 |     			yMin = tempYMin;
376 |     		
377 |     		if(tempYMax>yMax)
378 |     			yMax=tempYMax;
379 |     		
380 |     		if(xticCount < node.y.length)
381 | 				xticCount = node.y.length;
382 |     		
383 |     		
384 |     		if(node.x != null)
385 |     		{
386 |         		double tempXMax = getMax(node.x);
387 |         		double tempXMin = getMin(node.x);
388 |         		
389 |         		if(tempXMin<xMin)
390 |         			xMin = tempXMin;
391 |         		
392 |         		if(tempXMax>xMax)
393 |         			xMax=tempXMax;
394 |         		
395 |     		}
396 |     		else
397 |     		{
398 |     			xMax=node.y.length-1;
399 |     			xMin=0;
400 |     			
401 |     		}
402 |     		
403 |     	}
404 |     	
405 |     }
406 |  
407 |     private double getMax(double[] data) {
408 |         double max = -Double.MAX_VALUE;
409 |         for(int i = 0; i < data.length; i++) {
410 |             if(data[i] > max)
411 |                 max = data[i];
412 |         }
413 |         return max;
414 |     }
415 |     
416 |     private double getMin(double[] data) {
417 |         double min = Double.MAX_VALUE;
418 |         for(int i = 0; i < data.length; i++) {
419 |             if(data[i] < min)
420 |                 min = data[i];
421 |         }
422 |         return min;
423 |     }
424 |     
425 |     public void setYLabel(String s)
426 |     {
427 |     	yLabel = s;
428 |     }
429 |     
430 |     public void setXLabel(String s)
431 |     {
432 |     	xLabel = s;
433 |     }
434 |     
435 |     public void setTitle(String s)
436 |     {
437 |     	titleLabel = s;
438 |     }
439 |     
440 |     private void setYLabel(Graphics2D g2, String s)
441 |     {
442 |     	FontMetrics fm = getFontMetrics(getFont());
443 |     	int width = fm.stringWidth(s);
444 |     	
445 |     	AffineTransform temp = g2.getTransform();
446 |     	
447 |     	    AffineTransform at = new AffineTransform();
448 |     	    at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2);
449 |     	    g2.setTransform(at);
450 |     	    
451 |     	    //draw string in center of y axis
452 |     	    g2.drawString(s, 10, 7+getHeight()/2+width/2);
453 | 
454 |         g2.setTransform(temp);
455 |     	
456 |     }
457 |     
458 |     private void setXLabel(Graphics2D g2, String s)
459 |     {
460 |     	FontMetrics fm = getFontMetrics(getFont());
461 |     	int width = fm.stringWidth(s);
462 |     	
463 |     	g2.drawString(s, getWidth()/2-(width/2), getHeight()-10);
464 |     }
465 |     
466 |     private void setTitle(Graphics2D g2, String s)
467 |     {
468 |     	FontMetrics fm = getFontMetrics(getFont());
469 |     	
470 |     	String[] line = s.split("\n");
471 |     	
472 |     	int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2);
473 |     	
474 |     	for (int i=0; i<line.length; i++)
475 |     	{
476 |     		
477 |     		int width = fm.stringWidth(line[i]);
478 |             g2.drawString(line[i], getWidth()/2-(width/2),  height);
479 |             height +=fm.getHeight();
480 |             
481 |     	}
482 |    
483 |     }
484 | 
485 |     
486 |     public void yGridOn()
487 |     {
488 |     	yGrid=true;
489 |     	//super.repaint();
490 |     }
491 |     
492 |     public void yGridOff()
493 |     {
494 |     	yGrid=false;
495 |     	//super.repaint();
496 |     }
497 |     
498 |     public void xGridOn()
499 |     {
500 |     	xGrid=true;
501 |     	//super.repaint();
502 |     }
503 |     
504 |     public void xGridOff()
505 |     {
506 |     	xGrid=false;
507 |     	//super.repaint();
508 |     }
509 |     
510 |     private void drawYTickRange(Graphics2D g2, Line2D yaxis, int tickCount, double Max, double Min)
511 |     {
512 |     	if(!userSetYTic)
513 |     	{
514 |     	double range = Max - Min;
515 |     	
516 |     	
517 |     	//calculate max Y and min Y tic Range
518 |     	double unroundedTickSize = range/(tickCount-1);
519 |     	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
520 |     	double pow10x = Math.pow(10, x);
521 |     	yTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
522 |     	
523 |     	//determine min and max tick label 
524 |         if(Min<0)
525 |         	lowerYtic = yTicStepSize * Math.floor(Min/yTicStepSize);
526 |         else
527 |         	lowerYtic = yTicStepSize * Math.ceil(Min/yTicStepSize);
528 |         
529 |         if(Max<0)
530 |         	upperYtic = yTicStepSize * Math.floor(1+Max/yTicStepSize);
531 |         else
532 |         	upperYtic = yTicStepSize * Math.ceil(1+Max/yTicStepSize);
533 |         
534 |     	}
535 |     	
536 |         
537 |         double x0 = yaxis.getX1();
538 |         double y0 = yaxis.getY1();
539 |         double xf = yaxis.getX2();
540 |         double yf = yaxis.getY2();
541 |         
542 |         //calculate stepsize between ticks and length of Y axis using distance formula
543 |         int roundedTicks = (int) ((upperYtic - lowerYtic) / yTicStepSize);
544 |         double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
545 |         
546 |         double upper = upperYtic;
547 |         for (int i = 0; i<=roundedTicks; i++)
548 |         {
549 |         	double newY = y0;
550 |         	
551 |         	//calculate width of number for proper drawing
552 |         	String number = new DecimalFormat("#.#").format(upper);
553 |         	FontMetrics fm = getFontMetrics(getFont());
554 |         	int width = fm.stringWidth(number);
555 |    
556 |         	g2.draw(new Line2D.Double(x0,newY, x0-10,newY));
557 |         	g2.drawString(number, (float)x0-15-width, (float)newY+5); 
558 |         	 
559 |         	//add grid lines to chart
560 |         	if(yGrid && i!=roundedTicks)
561 |         	{
562 | 
563 |         		Stroke tempS = g2.getStroke();
564 |         		Color tempC = g2.getColor();
565 |         		
566 |         		g2.setColor (Color.lightGray);
567 |           	    g2.setStroke (new BasicStroke(
568 |           	      1f, 
569 |           	      BasicStroke.CAP_ROUND, 
570 |           	      BasicStroke.JOIN_ROUND, 
571 |           	      1f, 
572 |           	      new float[] {5f}, 
573 |           	      0f));
574 |           	    
575 |           	    g2.draw(new Line2D.Double(xPAD, newY, getWidth()-xPAD, newY));
576 |         		
577 |           	    g2.setColor(tempC);
578 |           	    g2.setStroke(tempS);
579 |         		
580 |         	}
581 |         	
582 |         	  upper = upper - yTicStepSize;
583 |         	y0 = newY + distance;
584 |         	
585 |         }
586 |     }
587 |     
588 |     private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min)
589 |     {
590 |     	drawXTickRange(g2, xaxis, tickCount, Max, Min, 1);
591 |     }
592 |     
593 |     private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min, double skip)
594 |     {
595 |     	if(!userSetXTic)
596 |     	{
597 |     	double range = Max - Min;
598 |     	
599 |     	//calculate max Y and min Y tic Range
600 |     	double unroundedTickSize = range/(tickCount-1);
601 |     	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
602 |     	double pow10x = Math.pow(10, x);
603 |     	xTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
604 |     	
605 |     	//determine min and max tick label 
606 |         if(Min<0)
607 |         	lowerXtic = xTicStepSize * Math.floor(Min/xTicStepSize);
608 |         else
609 |         	lowerXtic = xTicStepSize * Math.ceil(Min/xTicStepSize);
610 |         
611 |         if(Max<0)
612 |         	upperXtic = xTicStepSize * Math.floor(1+Max/xTicStepSize);
613 |         else
614 |         	upperXtic = xTicStepSize * Math.ceil(1+Max/xTicStepSize);
615 |     	}
616 |         
617 |         
618 |         double x0 = xaxis.getX1();
619 |         double y0 = xaxis.getY1();
620 |         double xf = xaxis.getX2();
621 |         double yf = xaxis.getY2();
622 |         
623 |         //calculate stepsize between ticks and length of Y axis using distance formula
624 |         int roundedTicks = (int) ((upperXtic - lowerXtic) / xTicStepSize);
625 |         
626 |         
627 |         double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
628 |         
629 |         double lower = lowerXtic;
630 |         for (int i = 0; i<=roundedTicks; i++)
631 |         {
632 |         	double newX = x0;
633 |         	
634 |         	//calculate width of number for proper drawing
635 |         	String number = new DecimalFormat("#.#").format(lower);
636 |         	FontMetrics fm = getFontMetrics( getFont() );
637 |         	int width = fm.stringWidth(number);
638 |         	
639 |    
640 |         	g2.draw(new Line2D.Double(newX,yf, newX,yf+10));
641 |         	
642 |         	//dont label every x tic to prevent clutter
643 |         	if(i%skip==0)
644 |        		g2.drawString(number, (float)(newX-(width/2.0)), (float)yf+25); 
645 |         	
646 |         	//add grid lines to chart
647 |         	if(xGrid && i!=0)
648 |         	{
649 |         		Stroke tempS = g2.getStroke();
650 |         		Color tempC = g2.getColor();
651 |         		
652 |         		g2.setColor (Color.lightGray);
653 |           	    g2.setStroke (new BasicStroke(
654 |           	      1f, 
655 |           	      BasicStroke.CAP_ROUND, 
656 |           	      BasicStroke.JOIN_ROUND, 
657 |           	      1f, 
658 |           	      new float[] {5f}, 
659 |           	      0f));
660 |           	    
661 |           	    g2.draw(new Line2D.Double(newX, yPAD, newX, getHeight()-yPAD));
662 |         		
663 |           	    g2.setColor(tempC);
664 |           	    g2.setStroke(tempS);
665 |         		
666 |         	}
667 |         	
668 |         	 
669 |         	lower = lower + xTicStepSize;
670 |         	x0 = newX + distance;
671 |         }
672 |     }
673 |     
674 |     public void updateData(int series, double[][] data)
675 |     {
676 |     	//add Data to link list
677 |     	addData(data,null,null);
678 |     	
679 |     	//copy data from new to old and line styles from list to new list.
680 |     	
681 |     	link.get(series).x = link.getLast().x.clone();
682 |     	link.get(series).y = link.getLast().y.clone();
683 |     	
684 |     	//remove last data
685 |     	link.removeLast();
686 |     
687 |     	
688 |     	
689 |     }
690 |     
691 | 	public static double[] getXVector(double[][] arr)
692 | 	{
693 | 		double[] temp = new double[arr.length];
694 | 
695 | 		for(int i=0; i<temp.length; i++)
696 | 			temp[i] = arr[i][0];
697 | 
698 | 		return temp;		
699 | 	}
700 | 
701 | 	public static double[] getYVector(double[][] arr)
702 | 	{
703 | 		double[] temp = new double[arr.length];
704 | 
705 | 		for(int i=0; i<temp.length; i++)
706 | 			temp[i] = arr[i][1];
707 | 
708 | 		return temp;		
709 | 	}
710 | 	
711 | 	/**********Class for Linked List************/
712 |     private class xyNode
713 |     {
714 |     	double[] x;
715 |     	double[] y;
716 |     	Color lineColor;
717 |     	
718 |     	boolean lineMarker;
719 |     	Color markerColor;
720 |     	
721 |     	public xyNode()
722 |     	{
723 |     		x=null;
724 |     		y=null;
725 |     		
726 |     		lineMarker = false;
727 |     	}
728 |     }
729 | 	
730 |     /****Methods to Support Right Click Menu****/
731 |     @Override
732 | 	public void lostOwnership(Clipboard    clip, Transferable transferable) 
733 | 	{
734 | 		//We must keep the object we placed on the system clipboard
735 | 		//until this method is called.
736 | 	}
737 | 
738 |     
739 |     
740 |     private void menu(JFrame g, final FalconLinePlot p )
741 |     {
742 | 
743 |     	g.addMouseListener(new PopupTriggerListener());
744 |     	
745 |     	JMenuItem item = new JMenuItem("Copy Figure");
746 | 
747 |         item.addActionListener(new ActionListener() 
748 |         {
749 |           public void actionPerformed(ActionEvent e) 
750 |           {
751 |             
752 |             BufferedImage i = new BufferedImage(p.getSize().width, p.getSize().height,BufferedImage.TRANSLUCENT);
753 |             p.setOpaque(false);
754 |             p.paint(i.createGraphics()); 
755 |             TransferableImage trans = new TransferableImage( i );
756 | 			Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
757 | 			c.setContents( trans, p);
758 |           }
759 |         });
760 |         
761 |         menu.add(item);
762 |     	
763 |         item = new JMenuItem("Desktop ScreenShot");
764 |         item.addActionListener(new ActionListener() 
765 |         {
766 |           public void actionPerformed(ActionEvent e) 
767 |           {
768 |             System.out.println("Copy files to clipboard");
769 |             
770 |             try {
771 |                 Robot robot = new Robot();
772 |                 Dimension screenSize  = Toolkit.getDefaultToolkit().getScreenSize();
773 |                 Rectangle screen = new Rectangle( screenSize );
774 |                 BufferedImage i = robot.createScreenCapture( screen );
775 |                 TransferableImage trans = new TransferableImage( i );
776 |                 Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
777 |                 c.setContents( trans, p);
778 |             }
779 |             catch ( AWTException x ) {
780 |                 x.printStackTrace();
781 |                 System.exit( 1 );
782 |             }
783 |             
784 |           }
785 |         });
786 |         
787 |         menu.add(item);
788 |     }
789 | 
790 |     class PopupTriggerListener extends MouseAdapter {
791 |     	public void mousePressed(MouseEvent ev) {
792 |     		if (ev.isPopupTrigger()) {
793 |     			menu.show(ev.getComponent(), ev.getX(), ev.getY());
794 |     		}
795 |     	}
796 | 
797 |     	public void mouseReleased(MouseEvent ev) {
798 |     		if (ev.isPopupTrigger()) {
799 |     			menu.show(ev.getComponent(), ev.getX(), ev.getY());
800 |     		}
801 |     	}
802 | 
803 |     	public void mouseClicked(MouseEvent ev) {
804 | 
805 |     	}
806 |     }
807 | 
808 |     private class TransferableImage implements Transferable {
809 | 
810 |         Image i;
811 | 
812 |         public TransferableImage( Image i ) {
813 |             this.i = i;
814 |         }
815 | 
816 |         public Object getTransferData( DataFlavor flavor )
817 |         throws UnsupportedFlavorException, IOException {
818 |             if ( flavor.equals( DataFlavor.imageFlavor ) && i != null ) {
819 |                 return i;
820 |             }
821 |             else {
822 |                 throw new UnsupportedFlavorException( flavor );
823 |             }
824 |         }
825 | 
826 |         public DataFlavor[] getTransferDataFlavors() {
827 |             DataFlavor[] flavors = new DataFlavor[ 1 ];
828 |             flavors[ 0 ] = DataFlavor.imageFlavor;
829 |             return flavors;
830 |         }
831 | 
832 |         public boolean isDataFlavorSupported( DataFlavor flavor ) {
833 |             DataFlavor[] flavors = getTransferDataFlavors();
834 |             for ( int i = 0; i < flavors.length; i++ ) {
835 |                 if ( flavor.equals( flavors[ i ] ) ) {
836 |                     return true;
837 |                 }
838 |             }
839 | 
840 |             return false;
841 |         }
842 |     }
843 |     
844 |     /******TEST MAIN METHOD*******/
845 |     public static void main(String[] args) {
846 |     	
847 |   	  double[] data = {
848 |   		        -235, 14, 18, 03, 60, 150, 74, 87, 54, 77,
849 |   		        61, 55, 48, 60, 49, 36, 38, 27, 20, 18,5
850 |   		    };
851 |   	  
852 |   	  double[] data2 = {
853 | 		        -4, 124, 128, 33, -1, 1, 74, 87, 54, 77,
854 | 		        61, 55, 48, 60, 40, 36, 38, 27, 20, 18,5
855 | 		    };
856 |   	  
857 |   	  double[] test = {0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 2.0,
858 |   			  2.2,2.4,2.6,2.8,3.0,3.2,3.4,3.6,3.8,4.0,4.2
859 |   	  };
860 |   	
861 |   	
862 |   	
863 |   	FalconLinePlot fig2 = new FalconLinePlot(data,Color.red, Color.blue);
864 |   	
865 |   	fig2.yGridOn();
866 |   	fig2.xGridOn();
867 |   	fig2.setYLabel("This is a new");
868 |   	
869 |   	fig2.addData(data2, Color.blue);
870 |   	
871 |   	FalconLinePlot fig1 =  new FalconLinePlot(test,data);
872 |   	
873 |   }
874 |     
875 |     
876 | }
877 |    
878 | 


--------------------------------------------------------------------------------
/src/usfirst/frc/team2168/robot/FalconPathPlanner.java:
--------------------------------------------------------------------------------
  1 | package usfirst.frc.team2168.robot;
  2 | 
  3 | import java.awt.Color;
  4 | import java.awt.GraphicsEnvironment;
  5 | import java.util.LinkedList;
  6 | import java.util.List;
  7 | 
  8 | 
  9 | 
 10 | /**
 11 |  * This Class provides many useful algorithms for Robot Path Planning. It uses optimization techniques and knowledge
 12 |  * of Robot Motion in order to calculate smooth path trajectories, if given only discrete waypoints. The Benefit of these optimization
 13 |  * algorithms are very efficient path planning that can be used to Navigate in Real-time.
 14 |  * 
 15 |  * This Class uses a method of Gradient Decent, and other optimization techniques to produce smooth Velocity profiles
 16 |  * for both left and right wheels of a differential drive robot.
 17 |  * 
 18 |  * This Class does not attempt to calculate quintic or cubic splines for best fitting a curve. It is for this reason, the algorithm can be ran
 19 |  * on embedded devices with very quick computation times.
 20 |  * 
 21 |  * The output of this function are independent velocity profiles for the left and right wheels of a differential drive chassis. The velocity
 22 |  * profiles start and end with 0 velocity and maintain smooth transitions throughout the path. 
 23 |  * 
 24 |  * This algorithm is a port from a similar algorithm running on a Robot used for my PhD thesis. I have not fully optimized
 25 |  * these functions, so there is room for some improvement. 
 26 |  * 
 27 |  * Initial tests on the 2015 FRC NI RoboRio, the complete algorithm finishes in under 15ms using the Java System Timer for paths with less than 50 nodes. 
 28 |  * 
 29 |  * @author Kevin Harrilal
 30 |  * @email kevin@team2168.org
 31 |  * @version 1.0
 32 |  * @date 2014-Aug-11
 33 |  *
 34 |  */
 35 | public class FalconPathPlanner
 36 | {
 37 | 
 38 | 	//Path Variables
 39 | 	public double[][] origPath;
 40 | 	public double[][] nodeOnlyPath;
 41 | 	public double[][] smoothPath;
 42 | 	public double[][] leftPath;
 43 | 	public double[][] rightPath;
 44 | 
 45 | 	//Orig Velocity
 46 | 	public double[][] origCenterVelocity;
 47 | 	public double[][] origRightVelocity;
 48 | 	public double[][] origLeftVelocity;
 49 | 
 50 | 	//smooth velocity
 51 | 	public double[][] smoothCenterVelocity;
 52 | 	public double[][] smoothRightVelocity;
 53 | 	public double[][] smoothLeftVelocity;
 54 | 
 55 | 	//accumulated heading
 56 | 	public double[][] heading;
 57 | 
 58 | 	double totalTime;
 59 | 	double totalDistance;
 60 | 	double numFinalPoints;
 61 | 
 62 | 	double pathAlpha;
 63 | 	double pathBeta;
 64 | 	double pathTolerance;
 65 | 
 66 | 	double velocityAlpha;
 67 | 	double velocityBeta;
 68 | 	double velocityTolerance;
 69 | 
 70 | 
 71 | 	/**
 72 | 	 * Constructor, takes a Path of Way Points defined as a double array of column vectors representing the global
 73 | 	 * cartesian points of the path in {x,y} coordinates. The waypoint are traveled from one point to the next in sequence.
 74 | 	 * 
 75 | 	 * For example: here is a properly formated waypoint array
 76 | 	 * 
 77 | 	 * double[][] waypointPath = new double[][]{
 78 | 				{1, 1},
 79 | 				{5, 1},
 80 | 				{9, 12},
 81 | 				{12, 9},
 82 | 				{15,6},
 83 | 				{15, 4}
 84 | 		};
 85 | 
 86 | 		This path goes from {1,1} -> {5,1} -> {9,12} -> {12, 9} -> {15,6} -> {15,4}
 87 | 
 88 | 		The units of these coordinates are position units assumed by the user (i.e inch, foot, meters) 
 89 | 	 * @param path
 90 | 	 */
 91 | 	public FalconPathPlanner(double[][] path)
 92 | 	{
 93 | 		this.origPath = doubleArrayCopy(path);
 94 | 
 95 | 		//default values DO NOT MODIFY;
 96 | 		pathAlpha = 0.7;
 97 | 		pathBeta = 0.3;
 98 | 		pathTolerance = 0.0000001;
 99 | 
100 | 		velocityAlpha = 0.1;
101 | 		velocityBeta = 0.3;
102 | 		velocityTolerance = 0.0000001;
103 | 	}
104 | 
105 | 	public static void print(double[] path)
106 | 	{
107 | 		System.out.println("X: \t Y:");
108 | 
109 | 		for(double u: path)
110 | 			System.out.println(u);
111 | 	}
112 | 
113 | 
114 | 
115 | 	/**
116 | 	 * Prints Cartesian Coordinates to the System Output as Column Vectors in the Form X	Y
117 | 	 * @param path
118 | 	 */
119 | 	public static void print(double[][] path)
120 | 	{
121 | 		System.out.println("X: \t Y:");
122 | 
123 | 		for(double[] u: path)
124 | 			System.out.println(u[0]+ "\t" +u[1]);
125 | 	}
126 | 
127 | 	/**
128 | 	 * Performs a deep copy of a 2 Dimensional Array looping thorough each element in the 2D array
129 | 	 * 
130 | 	 * BigO: Order N x M
131 | 	 * @param arr
132 | 	 * @return
133 | 	 */
134 | 	public static double[][] doubleArrayCopy(double[][] arr)
135 | 	{
136 | 
137 | 		//size first dimension of array
138 | 		double[][] temp = new double[arr.length][arr[0].length];
139 | 
140 | 		for(int i=0; i<arr.length; i++)
141 | 		{
142 | 			//Resize second dimension of array
143 | 			temp[i] = new double[arr[i].length];
144 | 
145 | 			//Copy Contents
146 | 			for(int j=0; j<arr[i].length; j++)
147 | 				temp[i][j] = arr[i][j];
148 | 		}
149 | 
150 | 		return temp;
151 | 
152 | 	}
153 | 
154 | 	/**
155 | 	 * Method upsamples the Path by linear injection. The result providing more waypoints along the path.
156 | 	 * 
157 | 	 * BigO: Order N * injection#
158 | 	 * 
159 | 	 * @param orig
160 | 	 * @param numToInject
161 | 	 * @return
162 | 	 */
163 | 	public double[][] inject(double[][] orig, int numToInject)
164 | 	{
165 | 		double morePoints[][];
166 | 
167 | 		//create extended 2 Dimensional array to hold additional points
168 | 		morePoints = new double[orig.length + ((numToInject)*(orig.length-1))][2];
169 | 
170 | 		int index=0;
171 | 
172 | 		//loop through original array
173 | 		for(int i=0; i<orig.length-1; i++)
174 | 		{
175 | 			//copy first
176 | 			morePoints[index][0] = orig[i][0];
177 | 			morePoints[index][1] = orig[i][1];
178 | 			index++;
179 | 
180 | 			for(int j=1; j<numToInject+1; j++)
181 | 			{
182 | 				//calculate intermediate x points between j and j+1 original points
183 | 				morePoints[index][0] = j*((orig[i+1][0]-orig[i][0])/(numToInject+1))+orig[i][0];
184 | 
185 | 				//calculate intermediate y points  between j and j+1 original points
186 | 				morePoints[index][1] = j*((orig[i+1][1]-orig[i][1])/(numToInject+1))+orig[i][1];
187 | 
188 | 				index++;
189 | 			}
190 | 		}
191 | 
192 | 		//copy last
193 | 		morePoints[index][0] =orig[orig.length-1][0];
194 | 		morePoints[index][1] =orig[orig.length-1][1];
195 | 		index++;
196 | 
197 | 		return morePoints;
198 | 	}
199 | 
200 | 
201 | 	/**
202 | 	 * Optimization algorithm, which optimizes the data points in path to create a smooth trajectory.
203 | 	 * This optimization uses gradient descent. While unlikely, it is possible for this algorithm to never
204 | 	 * converge. If this happens, try increasing the tolerance level.
205 | 	 * 
206 | 	 * BigO: N^x, where X is the number of of times the while loop iterates before tolerance is met. 
207 | 	 * 
208 | 	 * @param path
209 | 	 * @param weight_data
210 | 	 * @param weight_smooth
211 | 	 * @param tolerance
212 | 	 * @return
213 | 	 */
214 | 	public double[][] smoother(double[][] path, double weight_data, double weight_smooth, double tolerance)
215 | 	{
216 | 
217 | 		//copy array
218 | 		double[][] newPath = doubleArrayCopy(path);
219 | 
220 | 		double change = tolerance;
221 | 		while(change >= tolerance)
222 | 		{
223 | 			change = 0.0;
224 | 			for(int i=1; i<path.length-1; i++)
225 | 				for(int j=0; j<path[i].length; j++)
226 | 				{
227 | 					double aux = newPath[i][j];
228 | 					newPath[i][j] += weight_data * (path[i][j] - newPath[i][j]) + weight_smooth * (newPath[i-1][j] + newPath[i+1][j] - (2.0 * newPath[i][j]));
229 | 					change += Math.abs(aux - newPath[i][j]);	
230 | 				}					
231 | 		}
232 | 
233 | 		return newPath;
234 | 
235 | 	}
236 | 
237 | 	/**
238 | 	 * reduces the path into only nodes which change direction. This allows the algorithm to know at what points
239 | 	 * the original WayPoint vector changes. 
240 | 	 * 
241 | 	 * BigO: Order N + Order M, Where N is length of original Path, and M is length of Nodes found in Path
242 | 	 * @param path
243 | 	 * @return
244 | 	 */
245 | 	public static double[][] nodeOnlyWayPoints(double[][] path)
246 | 	{
247 | 
248 | 		List<double[]> li = new LinkedList<double[]>();
249 | 
250 | 		//save first value
251 | 		li.add(path[0]);
252 | 
253 | 		//find intermediate nodes
254 | 		for(int i=1; i<path.length-1; i++)
255 | 		{
256 | 			//calculate direction
257 | 			double vector1 = Math.atan2((path[i][1]-path[i-1][1]),path[i][0]-path[i-1][0]);
258 | 			double vector2 = Math.atan2((path[i+1][1]-path[i][1]),path[i+1][0]-path[i][0]);
259 | 
260 | 			//determine if both vectors have a change in direction
261 | 			if(Math.abs(vector2-vector1)>=0.01)
262 | 				li.add(path[i]);					
263 | 		}
264 | 
265 | 		//save last
266 | 		li.add(path[path.length-1]);
267 | 
268 | 		//re-write nodes into new 2D Array
269 | 		double[][] temp = new double[li.size()][2];
270 | 
271 | 		for (int i = 0; i < li.size(); i++)
272 | 		{
273 | 			temp[i][0] = li.get(i)[0];
274 | 			temp[i][1] = li.get(i)[1];
275 | 		}	
276 | 
277 | 		return temp;
278 | 	}
279 | 
280 | 
281 | 	/**
282 | 	 * Returns Velocity as a double array. The First Column vector is time, based on the time step, the second vector 
283 | 	 * is the velocity magnitude.
284 | 	 * 
285 | 	 * BigO: order N
286 | 	 * @param smoothPath
287 | 	 * @param timeStep
288 | 	 * @return
289 | 	 */
290 | 	double[][] velocity(double[][] smoothPath, double timeStep)
291 | 	{
292 | 		double[] dxdt = new double[smoothPath.length];
293 | 		double[] dydt = new double[smoothPath.length];
294 | 		double[][] velocity = new double[smoothPath.length][2];
295 | 
296 | 		//set first instance to zero
297 | 		dxdt[0]=0;
298 | 		dydt[0]=0;
299 | 		velocity[0][0]=0;
300 | 		velocity[0][1]=0;
301 | 		heading[0][1]=0;
302 | 
303 | 		for(int i=1; i<smoothPath.length; i++)
304 | 		{
305 | 			dxdt[i] = (smoothPath[i][0]-smoothPath[i-1][0])/timeStep;
306 | 			dydt[i] = (smoothPath[i][1]-smoothPath[i-1][1])/timeStep;
307 | 
308 | 			//create time vector
309 | 			velocity[i][0]=velocity[i-1][0]+timeStep;
310 | 			heading[i][0]=heading[i-1][0]+timeStep;
311 | 
312 | 			//calculate velocity
313 | 			velocity[i][1] = Math.sqrt(Math.pow(dxdt[i],2) + Math.pow(dydt[i],2));
314 | 		}
315 | 
316 | 
317 | 		return velocity;
318 | 
319 | 	}
320 | 	/**
321 | 	 * optimize velocity by minimizing the error distance at the end of travel
322 | 	 * when this function converges, the fixed velocity vector will be smooth, start
323 | 	 * and end with 0 velocity, and travel the same final distance as the original
324 | 	 * un-smoothed velocity profile
325 | 	 * 
326 | 	 * This Algorithm may never converge. If this happens, reduce tolerance. 
327 | 	 * 
328 | 	 * @param smoothVelocity
329 | 	 * @param origVelocity
330 | 	 * @param tolerance
331 | 	 * @return
332 | 	 */
333 | 	double[][] velocityFix(double[][] smoothVelocity, double[][] origVelocity, double tolerance)
334 | 	{
335 | 
336 | 		/*pseudo
337 | 		 * 1. Find Error Between Original Velocity and Smooth Velocity
338 | 		 * 2. Keep increasing the velocity between the first and last node of the smooth Velocity by a small amount
339 | 		 * 3. Recalculate the difference, stop if threshold is met or repeat step 2 until the final threshold is met.
340 | 		 * 3. Return the updated smoothVelocity
341 | 		 */
342 | 
343 | 		//calculate error difference
344 | 		double[] difference = errorSum(origVelocity,smoothVelocity);
345 | 
346 | 
347 | 		//copy smooth velocity into new Vector
348 | 		double[][] fixVel = new double[smoothVelocity.length][2];
349 | 
350 | 		for (int i=0; i<smoothVelocity.length; i++)
351 | 		{
352 | 			fixVel[i][0] = smoothVelocity[i][0];
353 | 			fixVel[i][1] = smoothVelocity[i][1];
354 | 		}
355 | 
356 | 		//optimize velocity by minimizing the error distance at the end of travel
357 | 		//when this converges, the fixed velocity vector will be smooth, start
358 | 		//and end with 0 velocity, and travel the same final distance as the original
359 | 		//un-smoothed velocity profile
360 | 		double increase = 0.0;
361 | 		while (Math.abs(difference[difference.length-1]) > tolerance)
362 | 		{
363 | 			increase = difference[difference.length-1]/1/50;
364 | 
365 | 			for(int i=1;i<fixVel.length-1; i++)
366 | 				fixVel[i][1] = fixVel[i][1] - increase;
367 | 
368 | 			difference = errorSum(origVelocity,fixVel);
369 | 		}
370 | 
371 | 		//fixVel =  smoother(fixVel, 0.001, 0.001, 0.0000001);
372 | 		return fixVel;
373 | 
374 | 	}
375 | 
376 | 
377 | 	/**
378 | 	 * This method calculates the integral of the Smooth Velocity term and compares it to the Integral of the 
379 | 	 * original velocity term. In essence we are comparing the total distance by the original velocity path and 
380 | 	 * the smooth velocity path to ensure that as we modify the smooth Velocity it still covers the same distance 
381 | 	 * as was intended by the original velocity path.
382 | 	 * 
383 | 	 * BigO: Order N
384 | 	 * @param origVelocity
385 | 	 * @param smoothVelocity
386 | 	 * @return
387 | 	 */
388 | 	private double[] errorSum(double[][] origVelocity, double[][] smoothVelocity)
389 | 	{
390 | 		//copy vectors
391 | 		double[] tempOrigDist = new double[origVelocity.length];
392 | 		double[] tempSmoothDist = new double[smoothVelocity.length];
393 | 		double[] difference = new double[smoothVelocity.length];
394 | 
395 | 
396 | 		double timeStep = origVelocity[1][0]-origVelocity[0][0];
397 | 
398 | 		//copy first elements
399 | 		tempOrigDist[0] = origVelocity[0][1];
400 | 		tempSmoothDist[0] = smoothVelocity[0][1];
401 | 
402 | 
403 | 		//calculate difference
404 | 		for (int i=1; i<origVelocity.length; i++)
405 | 		{
406 | 			tempOrigDist[i] = origVelocity[i][1]*timeStep + tempOrigDist[i-1];
407 | 			tempSmoothDist[i] = smoothVelocity[i][1]*timeStep + tempSmoothDist[i-1];
408 | 
409 | 			difference[i] = tempSmoothDist[i]-tempOrigDist[i];
410 | 
411 | 		}
412 | 
413 | 		return difference;
414 | 	}
415 | 	/**
416 | 	 * This method calculates the optimal parameters for determining what amount of nodes to inject into the path
417 | 	 * to meet the time restraint. This approach uses an iterative process to inject and smooth, yielding more desirable
418 | 	 * results for the final smooth path.
419 | 	 * 
420 | 	 * Big O: Constant Time
421 | 	 * 	
422 | 	 * @param numNodeOnlyPoints
423 | 	 * @param maxTimeToComplete
424 | 	 * @param timeStep
425 | 	 */
426 | 	public int[] injectionCounter2Steps(double numNodeOnlyPoints, double maxTimeToComplete, double timeStep)
427 | 	{
428 | 		int first = 0;
429 | 		int second = 0;
430 | 		int third = 0;
431 | 
432 | 		double oldPointsTotal = 0;
433 | 
434 | 		numFinalPoints  = 0;
435 | 
436 | 		int[] ret = null;
437 | 
438 | 		double totalPoints = maxTimeToComplete/timeStep;
439 | 
440 | 		if (totalPoints < 100)
441 | 		{
442 | 			double pointsFirst = 0;
443 | 			double pointsTotal = 0;
444 | 
445 | 
446 | 			for (int i=4; i<=6; i++)
447 | 				for (int j=1; j<=8; j++)
448 | 				{
449 | 					pointsFirst = i *(numNodeOnlyPoints-1) + numNodeOnlyPoints;
450 | 					pointsTotal = (j*(pointsFirst-1)+pointsFirst);
451 | 
452 | 					if(pointsTotal<=totalPoints && pointsTotal>oldPointsTotal)
453 | 					{
454 | 						first=i;
455 | 						second=j;
456 | 						numFinalPoints=pointsTotal;
457 | 						oldPointsTotal=pointsTotal;
458 | 					}
459 | 				}
460 | 
461 | 			ret = new int[] {first, second, third};
462 | 		}
463 | 		else
464 | 		{
465 | 
466 | 			double pointsFirst = 0;
467 | 			double pointsSecond = 0;
468 | 			double pointsTotal = 0;
469 | 
470 | 			for (int i=1; i<=5; i++)
471 | 				for (int j=1; j<=8; j++)
472 | 					for (int k=1; k<8; k++)
473 | 					{
474 | 						pointsFirst = i *(numNodeOnlyPoints-1) + numNodeOnlyPoints;
475 | 						pointsSecond = (j*(pointsFirst-1)+pointsFirst);
476 | 						pointsTotal =  (k*(pointsSecond-1)+pointsSecond);
477 | 
478 | 						if(pointsTotal<=totalPoints)
479 | 						{
480 | 							first=i;
481 | 							second=j;
482 | 							third=k;
483 | 							numFinalPoints=pointsTotal;
484 | 						}
485 | 					}
486 | 
487 | 			ret = new int[] {first, second, third};
488 | 		}
489 | 
490 | 
491 | 		return ret;
492 | 	}
493 | /**
494 |  * Calculates the left and right wheel paths based on robot track width
495 |  * 
496 |  * Big O: 2N
497 |  * 
498 |  * @param smoothPath - center smooth path of robot
499 |  * @param robotTrackWidth - width between left and right wheels of robot of skid steer chassis. 
500 |  */
501 | 	public void leftRight(double[][] smoothPath, double robotTrackWidth)
502 | 	{
503 | 
504 | 		double[][] leftPath = new double[smoothPath.length][2];
505 | 		double[][] rightPath = new double[smoothPath.length][2];
506 | 
507 | 		double[][] gradient = new double[smoothPath.length][2];
508 | 
509 | 		for(int i = 0; i<smoothPath.length-1; i++)
510 | 			gradient[i][1] = Math.atan2(smoothPath[i+1][1] - smoothPath[i][1],smoothPath[i+1][0] - smoothPath[i][0]);
511 | 
512 | 		gradient[gradient.length-1][1] = gradient[gradient.length-2][1];
513 | 
514 | 
515 | 		for (int i=0; i<gradient.length; i++)
516 | 		{
517 | 			leftPath[i][0] = (robotTrackWidth/2 * Math.cos(gradient[i][1] + Math.PI/2)) + smoothPath[i][0];
518 | 			leftPath[i][1] = (robotTrackWidth/2 * Math.sin(gradient[i][1] + Math.PI/2)) + smoothPath[i][1];
519 | 
520 | 			rightPath[i][0] = robotTrackWidth/2 * Math.cos(gradient[i][1] - Math.PI/2) + smoothPath[i][0];
521 | 			rightPath[i][1] = robotTrackWidth/2 * Math.sin(gradient[i][1] - Math.PI/2) + smoothPath[i][1];
522 | 
523 | 			//convert to degrees 0 to 360 where 0 degrees is +X - axis, accumulated to aline with WPI sensor
524 | 			double deg = Math.toDegrees(gradient[i][1]);
525 | 
526 | 			gradient[i][1] = deg;
527 | 
528 | 			if(i>0)
529 | 			{
530 | 				if((deg-gradient[i-1][1])>180)
531 | 					gradient[i][1] = -360+deg;
532 | 
533 | 				if((deg-gradient[i-1][1])<-180)
534 | 					gradient[i][1] = 360+deg;
535 | 			}
536 | 
537 | 
538 | 
539 | 		}
540 | 
541 | 		this.heading = gradient;
542 | 		this.leftPath = leftPath;
543 | 		this.rightPath = rightPath;
544 | 	}
545 | 	
546 | 	/**
547 | 	 * Returns the first column of a 2D array of doubles
548 | 	 * @param arr 2D array of doubles
549 | 	 * @return array of doubles representing the 1st column of the initial parameter
550 | 	 */
551 | 
552 | 	public static double[] getXVector(double[][] arr)
553 | 	{
554 | 		double[] temp = new double[arr.length];
555 | 
556 | 		for(int i=0; i<temp.length; i++)
557 | 			temp[i] = arr[i][0];
558 | 
559 | 		return temp;		
560 | 	}
561 | 
562 | 	/**
563 | 	 * Returns the second column of a 2D array of doubles
564 | 	 * 
565 | 	 * @param arr 2D array of doubles
566 | 	 * @return array of doubles representing the 1st column of the initial parameter
567 | 	 */
568 | 	public static double[] getYVector(double[][] arr)
569 | 	{
570 | 		double[] temp = new double[arr.length];
571 | 
572 | 		for(int i=0; i<temp.length; i++)
573 | 			temp[i] = arr[i][1];
574 | 
575 | 		return temp;		
576 | 	}
577 | 
578 | 	public static double[][] transposeVector(double[][] arr)
579 | 	{
580 | 		double[][] temp = new double[arr[0].length][arr.length];
581 | 
582 | 		for(int i=0; i<temp.length; i++)
583 | 			for(int j=0; j<temp[i].length; j++)
584 | 				temp[i][j] = arr[j][i];
585 | 
586 | 		return temp;		
587 | 	}
588 | 
589 | 	public void setPathAlpha(double alpha)
590 | 	{
591 | 		pathAlpha = alpha;
592 | 	}
593 | 
594 | 	public void setPathBeta(double beta)
595 | 	{
596 | 		pathAlpha = beta;
597 | 	}
598 | 
599 | 	public void setPathTolerance(double tolerance)
600 | 	{
601 | 		pathAlpha = tolerance;
602 | 	}
603 | 
604 | 	/**
605 | 	 * This code will calculate a smooth path based on the program parameters. If the user doesn't set any parameters, the will use the defaults optimized for most cases. The results will be saved into the corresponding
606 | 	 * class members. The user can then access .smoothPath, .leftPath, .rightPath, .smoothCenterVelocity, .smoothRightVelocity, .smoothLeftVelocity as needed.
607 | 	 * 
608 | 	 * After calling this method, the user only needs to pass .smoothRightVelocity[1], .smoothLeftVelocity[1] to the corresponding speed controllers on the Robot, and step through each setPoint.
609 | 	 * 
610 | 	 * @param totalTime - time the user wishes to complete the path in seconds. (this is the maximum amount of time the robot is allowed to take to traverse the path.)
611 | 	 * @param timeStep - the frequency at which the robot controller is running on the robot. 
612 | 	 * @param robotTrackWidth - distance between left and right side wheels of a skid steer chassis. Known as the track width.
613 | 	 */
614 | 	public void calculate(double totalTime, double timeStep, double robotTrackWidth)
615 | 	{
616 | 		/**
617 | 		 * pseudo code
618 | 		 * 
619 | 		 * 1. Reduce input waypoints to only essential (direction changing) node points
620 | 		 * 2. Calculate how many total datapoints we need to satisfy the controller for "playback"
621 | 		 * 3. Simultaneously inject and smooth the path until we end up with a smooth path with required number 
622 | 		 *    of datapoints, and which follows the waypoint path.
623 | 		 * 4. Calculate left and right wheel paths by calculating parallel points at each datapoint 
624 | 		 */
625 | 
626 | 
627 | 		//first find only direction changing nodes
628 | 		nodeOnlyPath = nodeOnlyWayPoints(origPath);
629 | 
630 | 		//Figure out how many nodes to inject
631 | 		int[] inject = injectionCounter2Steps(nodeOnlyPath.length, totalTime, timeStep);
632 | 
633 | 		//iteratively inject and smooth the path
634 | 		for(int i=0; i<inject.length; i++)
635 | 		{
636 | 			if(i==0)
637 | 			{
638 | 				smoothPath = inject(nodeOnlyPath,inject[0]);
639 | 				smoothPath = smoother(smoothPath, pathAlpha, pathBeta, pathTolerance);	
640 | 			}
641 | 			else
642 | 			{
643 | 				smoothPath = inject(smoothPath,inject[i]);
644 | 				smoothPath = smoother(smoothPath, 0.1, 0.3, 0.0000001);	
645 | 			}
646 | 		}
647 | 
648 | 		//calculate left and right path based on center path
649 | 		leftRight(smoothPath, robotTrackWidth);
650 | 
651 | 		origCenterVelocity = velocity(smoothPath, timeStep);
652 | 		origLeftVelocity = velocity(leftPath, timeStep);
653 | 		origRightVelocity = velocity(rightPath, timeStep);
654 | 
655 | 		//copy smooth velocities into fix Velocities
656 | 		smoothCenterVelocity =  doubleArrayCopy(origCenterVelocity);
657 | 		smoothLeftVelocity =  doubleArrayCopy(origLeftVelocity);
658 | 		smoothRightVelocity =  doubleArrayCopy(origRightVelocity);
659 | 
660 | 		//set final vel to zero
661 | 		smoothCenterVelocity[smoothCenterVelocity.length-1][1] = 0.0;
662 | 		smoothLeftVelocity[smoothLeftVelocity.length-1][1] = 0.0;
663 | 		smoothRightVelocity[smoothRightVelocity.length-1][1] = 0.0;
664 | 
665 | 		//Smooth velocity with zero final V
666 | 		smoothCenterVelocity = smoother(smoothCenterVelocity, velocityAlpha, velocityBeta, velocityTolerance);
667 | 		smoothLeftVelocity = smoother(smoothLeftVelocity, velocityAlpha, velocityBeta, velocityTolerance);
668 | 		smoothRightVelocity = smoother(smoothRightVelocity,velocityAlpha, velocityBeta, velocityTolerance);
669 | 
670 | 		//fix velocity distance error
671 | 		smoothCenterVelocity = velocityFix(smoothCenterVelocity, origCenterVelocity, 0.0000001);
672 | 		smoothLeftVelocity = velocityFix(smoothLeftVelocity, origLeftVelocity, 0.0000001);
673 | 		smoothRightVelocity = velocityFix(smoothRightVelocity, origRightVelocity, 0.0000001);
674 | 	}
675 | 
676 | 	//main program
677 | 	public static void main(String[] args)
678 | 	{
679 | 		long start = System.currentTimeMillis();
680 | 		//System.setProperty("java.awt.headless", "true"); //enable this to true to emulate roboRio environment
681 | 
682 | 
683 | 		//create waypoint path
684 | 		double[][] waypoints = new double[][]{
685 | 				{1, 1},
686 | 				{5, 1},
687 | 				{9, 12},
688 | 				{12, 9},
689 | 				{15, 6},
690 | 				{19, 12}
691 | 		}; 
692 | 
693 | 		double totalTime = 8; //seconds
694 | 		double timeStep = 0.1; //period of control loop on Rio, seconds
695 | 		double robotTrackWidth = 2; //distance between left and right wheels, feet
696 | 
697 | 		final FalconPathPlanner path = new FalconPathPlanner(waypoints);
698 | 		path.calculate(totalTime, timeStep, robotTrackWidth);
699 | 
700 | 		System.out.println("Time in ms: " + (System.currentTimeMillis()-start));
701 | 
702 | 		if(!GraphicsEnvironment.isHeadless())
703 | 		{
704 | 
705 | 			FalconLinePlot fig2 = new FalconLinePlot(path.smoothCenterVelocity,null,Color.blue);
706 | 			fig2.yGridOn();
707 | 			fig2.xGridOn();
708 | 			fig2.setYLabel("Velocity (ft/sec)");
709 | 			fig2.setXLabel("time (seconds)");
710 | 			fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Cyan, Right = Magenta");
711 | 			fig2.addData(path.smoothRightVelocity, Color.magenta);
712 | 			fig2.addData(path.smoothLeftVelocity, Color.cyan);
713 | 
714 | 			FalconLinePlot fig1 = new FalconLinePlot(path.nodeOnlyPath,Color.blue,Color.green);
715 | 			fig1.yGridOn();
716 | 			fig1.xGridOn();
717 | 			fig1.setYLabel("Y (feet)");
718 | 			fig1.setXLabel("X (feet)");
719 | 			fig1.setTitle("Top Down View of FRC Field (24ft x 27ft) \n shows global position of robot path, along with left and right wheel trajectories");
720 | 
721 | 			//force graph to show 1/2 field dimensions of 24ft x 27 feet
722 | 			fig1.setXTic(0, 27, 1);
723 | 			fig1.setYTic(0, 24, 1);
724 | 			fig1.addData(path.smoothPath, Color.red, Color.blue);
725 | 
726 | 
727 | 			fig1.addData(path.leftPath, Color.magenta);
728 | 			fig1.addData(path.rightPath, Color.magenta);
729 | 
730 | 
731 | 			//generate poof path used in 2014 Einstein
732 | 			path.poofExample();
733 | 
734 | 		}
735 | 
736 | 
737 | 		//example on printing useful path information
738 | 		//System.out.println(path.numFinalPoints);
739 | 		//System.out.println(path.pathAlpha);
740 | 
741 | 
742 | 
743 | 
744 | 	}
745 | 
746 | 	public void poofExample()
747 | 	{
748 | 		/***Poof Example***/
749 | 
750 | 		//Lets create a bank image
751 | 		FalconLinePlot fig3 = new FalconLinePlot(new double[][]{{0.0,0.0}});
752 | 		fig3.yGridOn();
753 | 		fig3.xGridOn();
754 | 		fig3.setYLabel("Y (feet)");
755 | 		fig3.setXLabel("X (feet)");
756 | 		fig3.setTitle("Top Down View of FRC Field (30ft x 27ft) \n shows global position of robot path, along with left and right wheel trajectories");
757 | 
758 | 
759 | 		//force graph to show 1/2 field dimensions of 24.8ft x 27 feet
760 | 		double fieldWidth = 32.0;
761 | 		fig3.setXTic(0, 27, 1);
762 | 		fig3.setYTic(0, fieldWidth, 1);
763 | 
764 | 
765 | 		//lets add field markers to help visual
766 | 		//http://www.usfirst.org/sites/default/files/uploadedFiles/Robotics_Programs/FRC/Game_and_Season__Info/2014/fe-00037_RevB.pdf
767 | 		//Goal line
768 | 		double[][] goalLine = new double[][] {{26.5,0}, {26.5, fieldWidth}};
769 | 		fig3.addData(goalLine, Color.black);
770 | 
771 | 		//Low Goals roughly 33 inch x 33 inch and 24.6 ft apart (inside to inside)
772 | 		double[][] leftLowGoal = new double[][]{
773 | 				{26.5, fieldWidth/2 + 24.6/2},
774 | 				{26.5, (fieldWidth)/2 + 24.6/2 + 2.75},
775 | 				{26.5 - 2.75, fieldWidth/2 + 24.6/2 + 2.75},
776 | 				{26.5 - 2.75, fieldWidth/2 + 24.6/2},
777 | 				{26.5, fieldWidth/2 + 24.6/2},
778 | 		};
779 | 
780 | 		double[][] rightLowGoal = new double[][]{
781 | 				{26.5, fieldWidth/2 - 24.6/2},
782 | 				{26.5, fieldWidth/2 - 24.6/2 - 2.75},
783 | 				{26.5 - 2.75, fieldWidth/2 - 24.6/2 - 2.75},
784 | 				{26.5 - 2.75, fieldWidth/2 - 24.6/2},
785 | 				{26.5, fieldWidth/2 - 24.6/2},
786 | 		};
787 | 
788 | 		fig3.addData(leftLowGoal, Color.black);
789 | 		fig3.addData(rightLowGoal, Color.black);
790 | 
791 | 		//Auto Line
792 | 		double[][] autoLine = new double[][] {{26.5-18,0}, {26.5-18, fieldWidth}};
793 | 		fig3.addData(autoLine, Color.black);
794 | 
795 | 
796 | 		double[][] CheesyPath = new double[][]{
797 | 				{7,16},
798 | 				{11,16},
799 | 				{17,28},
800 | 				{23,28},
801 | 		};
802 | 
803 | 		long start = System.currentTimeMillis();
804 | 
805 | 		double totalTime = 5; //seconds
806 | 		double timeStep = 0.1; //period of control loop on Rio, seconds
807 | 		double robotTrackWidth = 2; //distance between left and right wheels, feet
808 | 
809 | 		final FalconPathPlanner path = new FalconPathPlanner(CheesyPath);
810 | 		path.calculate(totalTime, timeStep, robotTrackWidth);
811 | 		
812 | 		System.out.println("Time in ms: " + (System.currentTimeMillis()-start));
813 | 
814 | 		//waypoint path
815 | 		fig3.addData(path.nodeOnlyPath,Color.blue,Color.green);
816 | 
817 | 		//add all other paths
818 | 		fig3.addData(path.smoothPath, Color.red, Color.blue);
819 | 		fig3.addData(path.leftPath, Color.magenta);
820 | 		fig3.addData(path.rightPath, Color.magenta);
821 | 
822 | 
823 | 		//Velocity
824 | 		FalconLinePlot fig4 = new FalconLinePlot(path.smoothCenterVelocity,null,Color.blue);
825 | 		fig4.yGridOn();
826 | 		fig4.xGridOn();
827 | 		fig4.setYLabel("Velocity (ft/sec)");
828 | 		fig4.setXLabel("time (seconds)");
829 | 		fig4.setTitle("Velocity Profile for Left and Right Wheels \n Left = Cyan, Right = Magenta");
830 | 		fig4.addData(path.smoothRightVelocity, Color.magenta);
831 | 		fig4.addData(path.smoothLeftVelocity, Color.cyan);
832 | 
833 | 		//path heading accumulated in degrees
834 | 		//FalconPathPlanner.print(path.heading);
835 | 
836 | 
837 | 	};
838 | }	
839 | 
840 | 
841 | 
842 | 
843 | 


--------------------------------------------------------------------------------