├── CircularBuffer ├── CircularBuffer.cpp └── CircularBuffer.h ├── Images ├── Figure 3.png └── math checks out.png ├── LICENSE ├── README.md └── S4_CurvePlanner ├── S4_CurvePlanner.cpp └── S4_CurvePlanner.h /CircularBuffer/CircularBuffer.cpp: -------------------------------------------------------------------------------- 1 | #include "CircularBuffer.h" 2 | 3 | float mm_per_rev = 17.357f; 4 | 5 | CircularBuffer::CircularBuffer(int size) : size(size), head(0), tail(0) { 6 | buffer = new char*[size]; 7 | } 8 | 9 | void CircularBuffer::push(char* item) { 10 | buffer[head] = item; 11 | head = (head + 1) % size; 12 | } 13 | 14 | void CircularBuffer::pop(char* &tailItem, char* &nextItem) { 15 | if (isEmpty()) { 16 | tailItem = nullptr; 17 | nextItem = nullptr; 18 | return; 19 | } 20 | 21 | tailItem = buffer[tail]; 22 | 23 | // Calculate the next tail position 24 | int nextTail = (tail + 1) % size; 25 | 26 | // Check if it's the last command in the buffer 27 | if (nextTail == head) { 28 | nextItem = nullptr; // Set nextItem to nullptr or 0 29 | } else { 30 | nextItem = buffer[nextTail]; 31 | } 32 | 33 | // Move the tail to the next position in the circular buffer 34 | tail = nextTail; 35 | } 36 | 37 | bool CircularBuffer::isEmpty() { 38 | return head == tail; 39 | } -------------------------------------------------------------------------------- /CircularBuffer/CircularBuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef CIRCULARBUFFER_H 2 | #define CIRCULARBUFFER_H 3 | 4 | class CircularBuffer { 5 | public: 6 | CircularBuffer(int size); 7 | void push(char* item); 8 | void pop(char* &tailItem, char* &nextItem); // Modified pop function 9 | bool isEmpty(); 10 | 11 | //Set the mm per revolution of the drivetrain. This is used to calculate the distance traveled in mm 12 | float mm_per_rev = 17.3474f; 13 | 14 | private: 15 | char** buffer; 16 | int head; 17 | int tail; 18 | int size; 19 | }; 20 | 21 | #endif // CIRCULARBUFFER_H -------------------------------------------------------------------------------- /Images/Figure 3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Juanduino/S4_CurvePlanner/0d3d134268e204e9fe90b186941bc176cbe3beb7/Images/Figure 3.png -------------------------------------------------------------------------------- /Images/math checks out.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Juanduino/S4_CurvePlanner/0d3d134268e204e9fe90b186941bc176cbe3beb7/Images/math checks out.png -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Juan-Antonio Søren E. Pedersen 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | IF THOU ART-CONTENT WITH THE SOFTWARE, THOU SHALL BUY ME A COFFEE https://www.buymeacoffee.com/jasep10101 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # S4 CurvePlanner C++ 4th order S-curve motion-planner 2 | 3 | In development - not testet at this moment. Code compiles. 4 | 5 | This libery calculates and executes 4th order S-curve motion. 6 | 7 | 4th order S-curve is divided into 15 time segments. 8 | It is only when a constant-jerk time exist, that the algorithm uses all 15 time-segments. When max jerk is not reached, only 8/9 time-segments is required, depending on if a constant velocity phase exist. 9 | 10 | The S-curve is a smoother motion profile compared to the trapez, since acceleration and deceleration are curved. 11 | 12 | All the math for the 4th order planner was inspired by: 13 | 14 | 15 | [Kinematically Constrained Jerk–Continuous S-Curve Trajectory Planning in Joint Space for Industrial Robots](https://www.mdpi.com/2079-9292/12/5/1135) 16 | 17 | 18 | 19 | Furthermore, the goal is to implement the planner with a circular buffer, so that the planner can be fed commands from a serial port or USB connection, 20 | and execute the commands in the order they were received. This will make the S4_CurvePlanner capable of handeling varius G and M commands. 21 | 22 | In order to use it with the SimpleFOC library, the planner will need to be created and linked in the main.cpp file, and the runPlannerOnTick() function will need to be called in the main loop. 23 | The commander is obviosly also needed. 24 | 25 | We also have to create a circularBuffer class in the same lib folder as the planner is #include "../CircularBuffer/CircularBuffer.h" 26 | The CircularBuffer folder is in this repo. 27 | 28 | This figure illustrates how the motion-profile is divided into 15 time segments. 29 | 30 | ![alt text](https://github.com/Juanduino/S4_CurvePlanner/blob/main/Images/Figure%203.png) 31 | 32 | 33 | Here is a move to 200. Pos_target is calculated using deltaTime, velocity_now, last_velocity, acceleration_now, last_acceleration. 34 | 35 | 36 | ![alt text](https://github.com/Juanduino/S4_CurvePlanner/blob/main/Images/math%20checks%20out.png) 37 | 38 | 39 | [FOR USE WITH SIMPLEFOC ](https://community.simplefoc.com/) 40 | 41 | ```cpp 42 | S4_CurvePlanner planner(5); 43 | 44 | void doPlanner(char *cmd){ 45 | planner.doGcommandBuffer(cmd); 46 | } 47 | 48 | void MPlanner(char *cmd){ 49 | planner.doMCommand(cmd); 50 | } 51 | ``` 52 | 53 | 54 | //And in the void setup() 55 | 56 | ```cpp 57 | planner.linkMotor(&motor); 58 | 59 | commander.add('G', doPlanner, "Motion Planner"); 60 | 61 | commander.add('M', MPlanner, "Motion Planner"); 62 | ``` 63 | 64 | //Here is a example of a loop that will run the planner on every tick. 65 | 66 | ```cpp 67 | void loop() { 68 | 69 | // main FOC algorithm function 70 | 71 | motor.loopFOC(); 72 | 73 | // Motion control function 74 | 75 | motor.move(); 76 | 77 | //motor.monitor(); 78 | 79 | commander.run(); 80 | 81 | planner.runPlannerOnTick(); 82 | 83 | } 84 | ``` 85 | 86 | 87 | -------------------------------------------------------------------------------- /S4_CurvePlanner/S4_CurvePlanner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define __debug 5 | 6 | S4_CurvePlanner::S4_CurvePlanner(int tickPeriod){ 7 | plannerPeriod = tickPeriod; 8 | isTrajectoryExecuting = false; 9 | return; 10 | } 11 | 12 | void S4_CurvePlanner::linkMotor(FOCMotor *motorReference){ 13 | motor = motorReference; 14 | } 15 | 16 | bool S4_CurvePlanner::isPlannerMoving(){ 17 | return isTrajectoryExecuting; 18 | } 19 | 20 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max){ 21 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 22 | } 23 | 24 | 25 | void S4_CurvePlanner::doGcommandBuffer(char *gCodeCommand){ 26 | 27 | buffer.push(gCodeCommand); 28 | } 29 | 30 | 31 | void S4_CurvePlanner::doMCommand(char *MCommand){ 32 | #ifdef __debug 33 | //SerialUSB.print("GGode command: M"); 34 | //SerialUSB.println(MCommand); 35 | #endif 36 | 37 | 38 | // Parse this string for vals to int 39 | String commandSrt = String(MCommand); 40 | int commandValue_Int = 0; 41 | commandValue_Int = commandSrt.toInt(); 42 | 43 | 44 | // The controller must be able to report its capabilities, typically with the M115 command. 45 | if (commandValue_Int == 115){ 46 | // M115 47 | // Send firmware version and capabilities 48 | // Note: Not final. Suggested by co-pilot. 49 | SerialUSB.println("FIRMWARE_NAME: ** CurvePlanner-S4 ** "); 50 | SerialUSB.println("FIRMWARE_VERSION: ** 1.0.1 ** "); 51 | SerialUSB.println("FIRMWARE_URL:"); 52 | SerialUSB.println("https://github.com/Juanduino/S4_CurvePlanner/tree/main/S4_CurvePlanner"); 53 | SerialUSB.println("PROTOCOL_VERSION: ** 1.0.1 **"); 54 | SerialUSB.println("AVAILABLE_COMMANDS:M, G"); 55 | SerialUSB.println("CAPABILITY:TRAJECTORY_CONTROL"); 56 | SerialUSB.println("POWER_SUPPLY:"); 57 | SerialUSB.println(voltage_power_supply); 58 | SerialUSB.println("MOTOR_VOLTAGE_LIMIT:"); 59 | SerialUSB.println(voltage_limit); 60 | SerialUSB.println("SENSE_VOLTAGE:"); 61 | SerialUSB.println(voltage_sense_divider); 62 | 63 | SerialUSB.println("LAST_POSITION:"); 64 | SerialUSB.println(Last_position); 65 | SerialUSB.println("LAST_VEL:"); 66 | SerialUSB.println(last_velocity); 67 | SerialUSB.println("LAST_ACCEL:"); 68 | SerialUSB.println(last_acceleratio); 69 | SerialUSB.println("deltaTIME:"); 70 | SerialUSB.println(deltaTime); 71 | 72 | 73 | } 74 | 75 | /*********************************************************************************** 76 | 77 | //Variables for calculating pos_target using deltaTime, velocity and acceleration. 78 | float Last_position; 79 | float last_velocity; 80 | float last_acceleratio; 81 | float now_; 82 | float deltaTime; 83 | 84 | *************************************************************************************/ 85 | 86 | 87 | //The controller can report axes positions, including extra axes (A, B, C etc.), typically with the M114 command. 88 | 89 | 90 | if (commandValue_Int == 114){ 91 | SerialUSB.println("ok"); 92 | // M114 93 | // Send current position 94 | 95 | //MM per revolution movement 96 | //int mm_per_rev = 17; 97 | 98 | //convert to mm from 2 x radians per revolution using the mm_per_rev variable as the mm per revolution 99 | 100 | float mm = mapfloat(motor->shaft_angle, 0, 2*PI, 0, buffer.mm_per_rev); 101 | SerialUSB.print("X:"); 102 | SerialUSB.println(mm, 4); } 103 | 104 | 105 | 106 | // The controller must be able to wait for motion completion, typically with the M400 command. Any further commands sent after the M400 must be suspended until motion completion. 107 | // The controller must only acknowledge the command, when motion is complete i.e. the "ok" (COMMAND_CONFIRM_REGEX) response must be suspended until then, providing blocking 108 | // synchronization to OpenPnP. 109 | 110 | if (commandValue_Int == 400){ 111 | // M400 112 | // Wait for current move to complete 113 | m400_flag = true; 114 | // SerialUSB.println("ok"); 115 | } 116 | 117 | //The controller must support dynamic acceleration and/or jerk limits, typically by the M204 command for acceleration or the M201.3 command for jerk. 118 | float commandValue2; 119 | 120 | if (commandValue_Int == 204){ 121 | // M204 122 | // Set acceleration 123 | 124 | commandSrt = commandSrt.substring(4); 125 | commandValue2 = commandSrt.toFloat(); 126 | vmax = commandValue2; 127 | // Try calling the planner to use this new velocity value 128 | // We have to use the current pos, vel, and accel 129 | // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_); 130 | //calculateTrapezoidalPathParameters(Xf_, motor->shaft_angle, motor->shaft_velocity, Vmax_, Amax_, Dmax_); 131 | #ifdef __debug 132 | SerialUSB.print("User wants velocity change. Vmax_: "); 133 | SerialUSB.println(vmax); 134 | #endif 135 | } 136 | 137 | 138 | if (commandValue_Int == 208){ 139 | //Note: Changed to 208 to avoid colision with 201(201.3) command´s 140 | // M201.3 141 | // Set jerk 142 | 143 | commandSrt = commandSrt.substring(4); 144 | commandValue2 = commandSrt.toFloat(); 145 | _Jmax_= commandValue2; 146 | 147 | 148 | // Try calling the planner to use this new acceleration value 149 | // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_); 150 | //calculateTrapezoidalPathParameters(Xf_, motor->shaft_angle, motor->shaft_velocity, Vmax_, Amax_, Dmax_); 151 | 152 | #ifdef __debug 153 | SerialUSB.print("JERK_MAX: "); 154 | SerialUSB.println(_Jmax_); 155 | #endif 156 | 157 | } 158 | 159 | // The controller must be able to home. 160 | //The homing procedure must be configurable, typically with the M206 command. 161 | //The controller must be able to home to a position other than 0. 162 | 163 | if (commandValue_Int == 206){ 164 | // M206 165 | // Set current position, for homing to 0. 166 | 167 | commandSrt = commandSrt.substring(4); 168 | commandValue2 = commandSrt.toFloat(); 169 | 170 | motor->shaft_angle = commandValue2; 171 | 172 | // NOTE: Probably need to set position in a better mannor. 173 | 174 | SerialUSB.print("HOMING_TO:"); 175 | SerialUSB.println(commandValue2); 176 | 177 | SerialUSB.print("POS:"); 178 | SerialUSB.println(motor->shaft_angle); } 179 | 180 | 181 | 182 | if (commandValue_Int == 207){ 183 | // M207 - NEW M-COMMAND 184 | // Set MAX SNAP 185 | 186 | commandSrt = commandSrt.substring(4); 187 | commandValue2 = commandSrt.toFloat(); 188 | _Smax_= commandValue2; 189 | 190 | #ifdef __debug 191 | SerialUSB.print("SNAP_MAX: "); 192 | SerialUSB.println(_Smax_); 193 | #endif 194 | 195 | } 196 | 197 | 198 | 199 | if (commandValue_Int == 203){ 200 | // M203 201 | // Set maximum feedrate/velocity 202 | commandSrt = commandSrt.substring(4); 203 | commandValue2 = commandSrt.toFloat(); 204 | _Vmax_= commandValue2; 205 | 206 | #ifdef __debug 207 | SerialUSB.print("VEL_MAX: "); 208 | SerialUSB.println(_Jmax_); 209 | #endif 210 | 211 | } 212 | 213 | 214 | if (commandValue_Int == 201){ 215 | // M201 216 | // Set maximum acceleration 217 | commandSrt = commandSrt.substring(4); 218 | commandValue2 = commandSrt.toFloat(); 219 | _Amax_= commandValue2; 220 | 221 | #ifdef __debug 222 | SerialUSB.print("ACCEL_MAX: "); 223 | SerialUSB.println(_Jmax_); 224 | #endif 225 | 226 | 227 | 228 | } 229 | 230 | 231 | if (commandValue_Int == 202){ 232 | // M202 233 | // Set maximum jerk 234 | commandSrt = commandSrt.substring(4); 235 | commandValue2 = commandSrt.toFloat(); 236 | _Jmax_= commandValue2; 237 | 238 | #ifdef __debug 239 | SerialUSB.print("JERK_MAX: "); 240 | SerialUSB.println(_Jmax_); 241 | #endif 242 | } 243 | 244 | if (commandValue_Int == 425){ 245 | // M425 246 | // Backlash compensation 247 | // Note: Should not be needed with sense on shaft...Perhaps usefull if using belts or spring loaded screw w. tiny amount of slag 248 | // This is outside the scope of this theoretical perfect move, since the machine constraints and physics may have some minute deviance. 249 | // This deviance, between perfect move and actual move, can of cource add up over time, not to slag! 250 | // Sure, with vision this acumulativ error get zeroed out, when homing to fiducals or end-stop actuator/sensor. Fixed machine head/camera/vision/sensor pointers. 251 | 252 | } 253 | 254 | // https://snapmaker.github.io/Documentation/gcode/M425 255 | 256 | 257 | if (commandValue_Int == 205){ 258 | // M205 259 | // Advanced settings 260 | 261 | 262 | } 263 | 264 | 265 | /* 266 | Note: TODO? 267 | M205 - Set Advanced Settings 268 | Description 269 | Set various motion settings. See parameters for details. 270 | 271 | Notes 272 | View the current setting with M503. 273 | 274 | .... 275 | 276 | https://snapmaker.github.io/Documentation/gcode/M425 277 | 278 | 279 | */ 280 | 281 | 282 | /* 283 | #ifdef __debug 284 | SerialUSB.print("M Command : "); 285 | SerialUSB.println(commandValue_Int); 286 | #endif 287 | 288 | */ 289 | 290 | } 291 | 292 | 293 | 294 | void S4_CurvePlanner::executeCommand(char *gCodeCommand, char *gCodeCommand2){ 295 | 296 | #ifdef __debug 297 | SerialUSB.print("GGode command: G"); 298 | SerialUSB.println(gCodeCommand); 299 | #endif 300 | 301 | //Serial.println("ok"); 302 | 303 | // Parse this string for vals 304 | String commandSrt = String(gCodeCommand); 305 | commandSrt = commandSrt.substring(1); 306 | float commandValue; 307 | switch (gCodeCommand[0]){ 308 | 309 | 310 | case 'V': 311 | // Remove V so can convert to a float 312 | commandSrt = commandSrt.substring(1); 313 | commandValue = commandSrt.toFloat(); 314 | _Vmax_ = commandValue; 315 | // Try calling the planner to use this new velocity value 316 | // We have to use the current pos, vel, and accel 317 | // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_); 318 | //calculateTrapezoidalPathParameters(Xf_, motor->shaft_angle, motor->shaft_velocity, Vmax_, Amax_, Dmax_); 319 | //This new value will be used when the gCommand is executed. 320 | #ifdef __debug 321 | SerialUSB.print("User wants velocity change. Vmax_: "); 322 | SerialUSB.println(vmax); 323 | #endif 324 | break; 325 | 326 | case 'A': 327 | // Remove A so can convert to a float 328 | commandSrt = commandSrt.substring(1); 329 | commandValue = commandSrt.toFloat(); 330 | _Amax_ = commandValue; 331 | //Dmax_ = Amax_; 332 | // Try calling the planner to use this new acceleration value 333 | // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_); 334 | //calculateTrapezoidalPathParameters(Xf_, motor->shaft_angle, motor->shaft_velocity, Vmax_, Amax_, Dmax_); 335 | //This new value will be used when the gCommand is executed. 336 | #ifdef __debug 337 | SerialUSB.print("User wants acceleration change. Amax_: "); 338 | SerialUSB.println(amax); 339 | 340 | #endif 341 | break; 342 | 343 | case 'M': 344 | commandSrt = commandSrt.substring(1); 345 | //commandValue = commandSrt.toFloat(); 346 | commandSrt.toCharArray(command_char, 10); 347 | doMCommand(command_char); 348 | 349 | break; 350 | 351 | default: 352 | // Remove G so can convert to a float 353 | commandSrt = commandSrt.substring(1); 354 | commandValue = commandSrt.toFloat(); 355 | 356 | //SerialUSB.print("Float: "); 357 | // SerialUSB.println(commandValue, 5); 358 | //= map(commandValue, 0, buffer.mm_per_rev, 0, 2*PI); 359 | 360 | float angle_command = mapfloat(commandValue, 0, buffer.mm_per_rev, 0, 2*PI); 361 | 362 | #ifdef __debug 363 | SerialUSB.print("Move to new position (rads): "); 364 | SerialUSB.println(angle_command, 5); 365 | #endif 366 | 367 | // We start moving to the new position 368 | Initiate_Move(angle_command); 369 | break; 370 | } 371 | 372 | } 373 | 374 | 375 | 376 | 377 | void S4_CurvePlanner::runPlannerOnTick(){ 378 | // This could run 10 000 times / second (10kHz) ? 379 | // https://www.youtube.com/watch?v=EYWEprqoLAQ 380 | 381 | if ((unsigned long)(millis() - plannerTimeStep) > plannerPeriod){ 382 | plannerTimeStep = millis(); 383 | 384 | 385 | 386 | if (!isTrajectoryExecuting){ 387 | // we are not in a move, let's see if we have a new move to start 388 | 389 | 390 | if (!buffer.isEmpty()){ 391 | // we have a new move to start 392 | buffer.pop(tailItem, nextItem); 393 | 394 | #ifdef __debug 395 | SerialUSB.println("tailItem: "); 396 | SerialUSB.println(tailItem); 397 | SerialUSB.println("nextItem: "); 398 | SerialUSB.println(nextItem); 399 | #endif 400 | 401 | executeCommand(tailItem, nextItem); 402 | } 403 | } 404 | 405 | // see if we are in a move or not 406 | if (isTrajectoryExecuting){ 407 | // we are in a move, let's calc the next position 408 | float timeSinceStartingTrajectoryInSeconds = (millis() - plannerStartingMovementTimeStamp) / 1000.0f; 409 | RuntimePlanner(timeSinceStartingTrajectoryInSeconds); 410 | motor->target = vel_target; 411 | 412 | float EventHorizon = 0.1f; // 100ms 413 | 414 | if (!buffer.isEmpty() && !m400_flag && timeSinceStartingTrajectoryInSeconds >= ((t1 + t2) - EventHorizon)){ 415 | 416 | buffer.pop(tailItem, nextItem); 417 | 418 | executeCommand(tailItem, nextItem); 419 | 420 | } 421 | 422 | 423 | 424 | // see if we are done with our move 425 | if (timeSinceStartingTrajectoryInSeconds >= Tf){ 426 | // we are done with move 427 | // motor.monitor_downsample = 0; // disable monitor 428 | #ifdef __debug 429 | SerialUSB.println("Done with move"); 430 | #endif 431 | 432 | SerialUSB.println("ok"); 433 | float map_pos = mapfloat(motor->shaft_angle, 0, 2*PI, 0, buffer.mm_per_rev); 434 | SerialUSB.print("X:"); 435 | SerialUSB.println(map_pos, 4); 436 | SerialUSB.print("Shaft angle:"); 437 | SerialUSB.println(motor->shaft_angle, 4); 438 | 439 | 440 | isTrajectoryExecuting = false; 441 | 442 | if (m400_flag){ 443 | Serial.println("ok"); 444 | m400_flag = false;} 445 | 446 | } 447 | } 448 | } 449 | } 450 | 451 | 452 | 453 | void S4_CurvePlanner::CalculateTs(float Ts_vmax, float Ts_amax, float Ts_jmax) { 454 | 455 | // Step 1: Calculate Ts from displacement constraint 456 | float ds_max = dmax / (8 * smax); 457 | Ts_d = pow(ds_max, 0.25f); 458 | 459 | //vmax = (2 * pow(Ts, 3)) * smax; 460 | // Step 2: Calculate Ts from velocity constraint 461 | Ts_v = cbrt(Ts_vmax / (2 * smax)); 462 | 463 | // Step 3: Calculate Ts from acceleration constraint 464 | //amax = pow(Ts, 2) * smax; 465 | Ts_a = sqrt(Ts_amax / smax); 466 | 467 | // Step 4: Calculate Ts from jerk constraint 468 | Ts_j = sqrt(Ts_jmax / smax); 469 | 470 | 471 | #ifdef __debug 472 | SerialUSB.println("**********************"); 473 | SerialUSB.println("CalculateTs variables:"); 474 | SerialUSB.print("Ts_d: "); 475 | SerialUSB.println(Ts_d, 8); 476 | SerialUSB.print("Ts_v: "); 477 | SerialUSB.println(Ts_v, 8); 478 | SerialUSB.print("Ts_a: "); 479 | SerialUSB.println(Ts_a, 8); 480 | SerialUSB.print("Ts_j: "); 481 | SerialUSB.println(Ts_j, 8); 482 | #endif 483 | 484 | } 485 | 486 | 487 | void S4_CurvePlanner::disp_void(){ 488 | 489 | jmax = smax * Ts_d; 490 | amax = jmax * Ts_d; 491 | vmax = (2 * amax) * Ts_d; 492 | 493 | #ifdef __debug 494 | SerialUSB.print("dmax: "); 495 | SerialUSB.println(dmax); 496 | SerialUSB.print("jmax: "); 497 | SerialUSB.println(jmax); 498 | SerialUSB.print("amax: "); 499 | SerialUSB.println(amax); 500 | SerialUSB.print("vmax: "); 501 | SerialUSB.println(vmax); 502 | #endif 503 | } 504 | 505 | void S4_CurvePlanner::disp_void_rampToCero(){ 506 | 507 | jmax_rampToCero = smax * Ts_d; 508 | amax_rampToCero = jmax_rampToCero * Ts_d; 509 | vmax_rampToCero = (2 * amax_rampToCero) * Ts_d; 510 | 511 | #ifdef __debug 512 | SerialUSB.print("dmax: "); 513 | SerialUSB.println(dmax); 514 | SerialUSB.print("jmax_rampToCero: "); 515 | SerialUSB.println(jmax_rampToCero); 516 | SerialUSB.print("amax_rampToCero: "); 517 | SerialUSB.println(amax_rampToCero); 518 | SerialUSB.print("vmax_rampToCero: "); 519 | SerialUSB.println(vmax_rampToCero); 520 | #endif 521 | } 522 | 523 | 524 | void S4_CurvePlanner::vel_void(){ 525 | 526 | jmax = smax * Ts_v; 527 | amax = pow(Ts, 2) * smax; 528 | #ifdef __debug 529 | SerialUSB.print("jmax: "); 530 | SerialUSB.println(jmax); 531 | SerialUSB.print("amax: "); 532 | SerialUSB.println(amax); 533 | SerialUSB.print("vmax: "); 534 | SerialUSB.println(vmax); 535 | SerialUSB.print("smax: "); 536 | SerialUSB.println(smax); 537 | #endif 538 | } 539 | 540 | 541 | 542 | void S4_CurvePlanner::vel_void_rampToCero(){ 543 | if (Ts_v == Ts_a) { 544 | Ts_a = 0; } 545 | 546 | jmax_rampToCero = smax * Ts_v; 547 | amax_rampToCero = pow(Ts_rampToCero, 2) * smax; 548 | #ifdef __debug 549 | SerialUSB.print("jmax_rampToCero: "); 550 | SerialUSB.println(jmax_rampToCero); 551 | SerialUSB.print("amax_rampToCero: "); 552 | SerialUSB.println(amax_rampToCero); 553 | SerialUSB.print("vmax_rampToCero: "); 554 | SerialUSB.println(vmax_rampToCero); 555 | SerialUSB.print("smax_rampToCero: "); 556 | SerialUSB.println(smax); 557 | #endif 558 | } 559 | 560 | 561 | 562 | void S4_CurvePlanner::acel_void(){ 563 | 564 | jmax = smax * Ts_v; 565 | 566 | #ifdef __debug 567 | SerialUSB.print("jmax: "); 568 | SerialUSB.println(jmax); 569 | SerialUSB.print("amax: "); 570 | SerialUSB.println(amax); 571 | SerialUSB.print("vmax: "); 572 | SerialUSB.println(vmax); 573 | SerialUSB.print("smax: "); 574 | SerialUSB.println(smax); 575 | #endif 576 | } 577 | 578 | 579 | void S4_CurvePlanner::acel_void_rampToCero(){ 580 | 581 | jmax_rampToCero = smax * Ts_v; 582 | 583 | #ifdef __debug 584 | SerialUSB.print("jmax_rampToCero: "); 585 | SerialUSB.println(jmax_rampToCero); 586 | SerialUSB.print("amax: "); 587 | SerialUSB.println(amax); 588 | SerialUSB.print("vmax: "); 589 | SerialUSB.println(vmax); 590 | SerialUSB.print("smax: "); 591 | SerialUSB.println(smax); 592 | #endif 593 | } 594 | 595 | 596 | void S4_CurvePlanner::jerk_void(){ 597 | 598 | #ifdef __debug 599 | SerialUSB.print("jmax: "); 600 | SerialUSB.println(jmax); 601 | SerialUSB.print("amax: "); 602 | SerialUSB.println(amax); 603 | SerialUSB.print("vmax: "); 604 | SerialUSB.println(vmax); 605 | SerialUSB.print("smax: "); 606 | SerialUSB.println(smax); 607 | #endif 608 | } 609 | 610 | 611 | 612 | float S4_CurvePlanner::CalculateTj(float Ts_jerk, float Tj_vmax, float Tj_jmax, float Tj_amax) { 613 | 614 | // Calculate Tdj based on the displacement constraint (Equation 16) 615 | float c = pow(Ts_jerk, 3); 616 | float d = (dmax * c) / (54.0f * Tj_jmax); 617 | float e = pow(dmax, 2) / (16.0f * pow(Tj_jmax, 2)); 618 | float a = (c / 27.0f) + (dmax / (4.0f * Tj_jmax)); 619 | float b = sqrt(d + e); 620 | float Tjd = cbrt(a + b) + cbrt(a - b) + ((5.0f * Ts_jerk) / 3.0f); 621 | 622 | // Calculate Tvj based on the velocity constraint (Equation 18) 623 | float Tjv = sqrt((pow(Ts_jerk, 2) / 4) + (Tj_vmax / Tj_jmax)) - ((3 * Ts_jerk) / 2); 624 | 625 | // Calculate Taj based on the acceleration constraint (Equation 20) 626 | float Tja = (Tj_amax / Tj_jmax) - Ts_jerk; 627 | 628 | #ifdef __debug 629 | SerialUSB.println("*******************************"); 630 | SerialUSB.println("CalculateTj variables: "); 631 | SerialUSB.print("Tjd: "); 632 | SerialUSB.println(Tjd); 633 | SerialUSB.print("Tjv: "); 634 | SerialUSB.println(Tjv); 635 | SerialUSB.print("Tja: "); 636 | SerialUSB.println(Tja); 637 | #endif 638 | 639 | float Tj_ = min({Tjd, Tjv, Tja}); 640 | 641 | return Tj_; 642 | } 643 | 644 | 645 | void S4_CurvePlanner::Tjd_void() { 646 | // Case 1: Only varying jerk and constant jerk 647 | amax = jmax * (Ts + Tjd); 648 | vmax = amax * ((2 * Ts) + Tj); 649 | 650 | #ifdef __debug 651 | SerialUSB.println("Case 1: Varying jerk and constant jerk"); 652 | SerialUSB.print("Acceleration: "); 653 | SerialUSB.println(amax); 654 | SerialUSB.print("Velocity: "); 655 | SerialUSB.println(vmax); 656 | #endif 657 | } 658 | 659 | void S4_CurvePlanner::Tjv_void(){ 660 | 661 | // Case 2: Maximum velocity reached without max acceleration 662 | amax = jmax * (Ts + Tjv); 663 | 664 | #ifdef __debug 665 | SerialUSB.println("Case 2: Maximum velocity reached"); 666 | SerialUSB.print("Acceleration: "); 667 | SerialUSB.println(amax); 668 | SerialUSB.print("dmax: "); 669 | SerialUSB.println(dmax); 670 | SerialUSB.print("jmax: "); 671 | SerialUSB.println(jmax); 672 | SerialUSB.print("vmax: "); 673 | SerialUSB.println(vmax); 674 | #endif 675 | } 676 | 677 | 678 | 679 | float S4_CurvePlanner::CalculateTa(float Ts_, float Tj_, float amax_, float vmax_) { 680 | 681 | //amax = (smax * Ts) * (Ts + Tj); 682 | float pre_calc = (2 * Ts_) + Tj_; 683 | // Calculate Tda based on the displacement constraint (Equation 23) 684 | Tad = ((3 * Tj_) / 2) - (3 * Ts_) + sqrt((pow(pre_calc, 2) / 4 ) + ((qe - qs) / amax_)); 685 | 686 | //amax = abs(dmax) / (8 * pow(Ta, 2) + ((3 * Ta) * Tj) + ((6 * Ts) * Ta) + (8 * pow(Ts, 2)) + (2 * pow(Tj, 2)) + ((8 * Ts) * Tj)); 687 | // Calculate Tva based on the velocity constraint (Equation 25) 688 | Tav = (vmax_ / amax_) - Tj_ + (2 * Ts_); 689 | 690 | // Choose the minimum Ta among the constraints 691 | // Tad = abs(Tad); 692 | float Ta_ = min({Tad, Tav}); 693 | 694 | #ifdef __debug 695 | SerialUSB.println("*******************************"); 696 | SerialUSB.println("CalculateTa variables: "); 697 | SerialUSB.print("Tad: "); 698 | SerialUSB.println(Tad); 699 | SerialUSB.print("Tav: "); 700 | SerialUSB.println(Tav); 701 | SerialUSB.print("amax_rampToCero: "); 702 | SerialUSB.println(amax_rampToCero); 703 | #endif 704 | 705 | return Ta_; 706 | } 707 | 708 | 709 | void S4_CurvePlanner::Tad_void(){ 710 | 711 | vmax = amax * ((2 * Ts) + Tad); 712 | #ifdef __debug 713 | SerialUSB.print("CASE 1 :"); 714 | SerialUSB.println("Trajectory segments with a constant velocity do not exist"); 715 | SerialUSB.print("real Vmax:"); 716 | SerialUSB.println(vmax); 717 | #endif 718 | } 719 | 720 | void S4_CurvePlanner::Tad_void_rampToCero(){ 721 | vmax_rampToCero = amax * ((2 * Ts_rampToCero) + Tad); 722 | #ifdef __debug 723 | SerialUSB.print("CASE 1 :"); 724 | SerialUSB.println("Trajectory segments with a constant velocity do not exist"); 725 | SerialUSB.print("real Vmax:"); 726 | SerialUSB.println(vmax); 727 | #endif 728 | } 729 | 730 | void S4_CurvePlanner::Tav_void(){ 731 | 732 | #ifdef __debug 733 | SerialUSB.print("CASE 2 :"); 734 | SerialUSB.println("both the maximum acceleration and velocity can reach their maximums"); 735 | #endif 736 | } 737 | 738 | 739 | float S4_CurvePlanner::CalculateTv() { 740 | //vmax = (smax * Ts) * (Ts + Tj) * ((2 * Ts) + Tj + Ta); 741 | // Calculate Tv based on the velocity constraint (Equation 27) 742 | Tv = ((qe - qs) / vmax) - ((4 * Ts) + (2 * Tj) + Ta); 743 | 744 | #ifdef __debug 745 | SerialUSB.print("vmax"); 746 | SerialUSB.println(vmax); 747 | SerialUSB.print("Tv"); 748 | SerialUSB.println(Tv); 749 | #endif 750 | 751 | return Tv; 752 | } 753 | 754 | 755 | float S4_CurvePlanner::CalculateTv_rampToCero(){ 756 | //vmax = (smax * Ts) * (Ts + Tj) * ((2 * Ts) + Tj + Ta); 757 | // Calculate Tv based on the velocity constraint (Equation 27) 758 | Tv_rampToCero = ((qe - qs) / vmax_rampToCero) - ((4 * Ts_rampToCero) + (2 * Tj_rampToCero) + Ta_rampToCero); 759 | 760 | #ifdef __debug 761 | SerialUSB.print("vmax_rampToCero"); 762 | SerialUSB.println(vmax_rampToCero); 763 | SerialUSB.print("Tv_rampToCero:"); 764 | SerialUSB.println(Tv_rampToCero); 765 | #endif 766 | 767 | return Tv_rampToCero; 768 | } 769 | 770 | 771 | 772 | /**************************************************************************************************************************/ 773 | bool S4_CurvePlanner::calculateVariables(float Xf, float Xi, float Vi, float Vmax_, float Amax_, float Jmax_, float Smax_) { 774 | 775 | 776 | //End position 777 | qe = Xf; 778 | 779 | //Start Position 780 | qs = Xi; 781 | 782 | //Initial velocity 783 | vs = Vi; 784 | 785 | // Calculate dmax from the displacement constraint 786 | dmax = abs(qs - qe); 787 | 788 | //Snap max 789 | smax = Smax_; 790 | 791 | //Acceleration max. 792 | amax = Amax_; 793 | 794 | amax_rampToCero = Amax_; 795 | 796 | // Jerk max. 797 | jmax = Jmax_; 798 | 799 | jmax_rampToCero = Jmax_; 800 | 801 | 802 | //Velocity max. minus initial velocity, for ramp up calculations. If initial velocity is > 0, calculate seperate ramp-down, within same timeframe 803 | vmax = Vmax_ - vs; 804 | 805 | //Secundary vmax in case ramp-down is diferent from ramp-up 806 | vmax_rampToCero = Vmax_; 807 | 808 | // Calculate the time intervals using snap 809 | //Ts = Jmax_ / Smax_; 810 | //Tj = Amax_ / Jmax_ - Ts; 811 | //Ta = Vmax_ / Amax_ - Tj - 2 * Ts; 812 | //Tv = (qe - qs) / Vmax_ - Ta - 2 * Tj - 4 * Ts; 813 | 814 | //Zero 815 | Ts = Ts_a = Ts_d = Ts_j = Ts_v = 0.0f; 816 | 817 | double_decel_move = false; 818 | Vi_is_positive = false; 819 | 820 | if (vmax < 0){ 821 | double_decel_move = true; 822 | vs = vs - (abs(vmax) * 2); 823 | vmax = abs(vmax); 824 | } 825 | 826 | //If Vi (Initial velocity) is more then 0 and we are not in a double-deceleration move. 827 | if (Vi > 0 && !double_decel_move){Vi_is_positive = true;} 828 | // Calculate the time intervals 829 | // NOTE: Follow flow chart 830 | CalculateTs(vmax, amax, jmax); 831 | Ts = min({Ts_d, Ts_v, Ts_a, Ts_j}); 832 | // Choose the minimum Ts among the constraints 833 | 834 | if (Ts == Ts_d){ 835 | // Move is constrained by distance only. 836 | disp_void();} 837 | 838 | if (Ts == Ts_v){ 839 | // Move is constrained by velocity 840 | Ts = abs(Ts); 841 | vel_void(); 842 | Tv = CalculateTv(); 843 | } 844 | 845 | if (Ts == Ts_a){ 846 | 847 | // Move is constrained by acceleration 848 | acel_void();Ta = CalculateTa(Ts, Tj, amax, vmax); 849 | 850 | if (Ta == Tad){Tad_void();} 851 | 852 | if (Ta == Tav){Tav_void(); 853 | 854 | CalculateTv();} 855 | 856 | } 857 | 858 | if (Ts == Ts_j){ 859 | //Move is constrained by jerk 860 | jerk_void(); 861 | 862 | Tj = CalculateTj(Ts, vmax, jmax, amax); 863 | // Choose the minimum Tj among the constraints 864 | if (Tj == Tjd){ 865 | Tjd_void(); 866 | }else if(Tj == Tjv){ 867 | Tjv_void(); 868 | CalculateTv(); 869 | }else{ 870 | // Case 3: Calculate other time intervals or motion parameters 871 | #ifdef __debug 872 | SerialUSB.println("Case 3: Calculate Ta"); 873 | #endif 874 | Ta = CalculateTa(Ts, Tj, amax, vmax); 875 | } 876 | } 877 | 878 | #ifdef __debug 879 | SerialUSB.println("****************************"); 880 | SerialUSB.println("S-curve Time-Segment Values:"); 881 | SerialUSB.print("Ts: "); 882 | SerialUSB.println(Ts); 883 | SerialUSB.print("Tj: "); 884 | SerialUSB.println(Tj); 885 | SerialUSB.print("Ta: "); 886 | SerialUSB.println(Ta); 887 | SerialUSB.print("Tv: "); 888 | SerialUSB.println(Tv); 889 | #endif 890 | 891 | 892 | //Zorro 893 | Ts_a = Ts_d = Ts_j = Ts_v = 0; 894 | //If the initial velocity is higher then (new)max velocity. Ramp down to max velocity and then ramp down to 0. 895 | //Note: This is also essential if initial velocity is larger then 0. 896 | //TODO: Handle cases where there are moves in buffer. 897 | if (double_decel_move || Vi_is_positive){ 898 | 899 | CalculateTs(Vmax_, amax_rampToCero, jmax_rampToCero); 900 | Ts_rampToCero = min({Ts_v, Ts_a}); 901 | 902 | if (Ts_rampToCero == Ts_d){ 903 | // Move is constrained by distance only. 904 | disp_void_rampToCero();} 905 | 906 | if (Ts_rampToCero == Ts_v){ 907 | // Move is constrained by velocity 908 | vel_void_rampToCero(); 909 | Tv_rampToCero = CalculateTv_rampToCero(); 910 | } 911 | 912 | if (Ts_rampToCero == Ts_a){ 913 | // Move is constrained by acceleration 914 | acel_void(); 915 | Ta_rampToCero = CalculateTa(Ts_rampToCero, Tj_rampToCero, amax_rampToCero, vmax_rampToCero); 916 | 917 | if (Ts_rampToCero == Tad){Tad_void_rampToCero();} 918 | 919 | if (Ts_rampToCero == Tav){Tav_void();CalculateTv_rampToCero();} 920 | } 921 | 922 | if (Ts_rampToCero == Ts_j){ 923 | //Move is constrained by jerk 924 | jerk_void(); 925 | 926 | Tj_rampToCero = CalculateTj(Ts_rampToCero, vmax_rampToCero, jmax_rampToCero, amax_rampToCero); 927 | // Choose the minimum Tj among the constraints 928 | if (Tj == Tjd){ 929 | Tjd_void(); 930 | }else if(Tj == Tjv){ 931 | Tjv_void(); 932 | CalculateTv_rampToCero(); 933 | }else{ 934 | // Case 3: Calculate other time intervals or motion parameters 935 | #ifdef __debug 936 | SerialUSB.println("Case 3: Calculate Ta"); 937 | #endif 938 | Ta = CalculateTa(Ts_rampToCero, Tj_rampToCero, amax_rampToCero, vmax_rampToCero); 939 | } 940 | } 941 | } 942 | 943 | #ifdef __debug 944 | SerialUSB.println("****************************"); 945 | SerialUSB.println("S-curve Time-Segment Values for rampToCero:"); 946 | SerialUSB.print("Ts_rampToCero: "); 947 | SerialUSB.println(Ts_rampToCero); 948 | SerialUSB.print("Tj_rampToCero: "); 949 | SerialUSB.println(Tj_rampToCero); 950 | SerialUSB.print("Ta_rampToCero: "); 951 | SerialUSB.println(Ta_rampToCero); 952 | SerialUSB.print("Tv_rampToCero: "); 953 | SerialUSB.println(Tv_rampToCero); 954 | #endif 955 | 956 | 957 | // Calculate the maximum motion parameters 958 | //jmax = smax * Ts; 959 | //amax = smax * Ts * (Ts + Tj); 960 | //vmax = smax * Ts * (Ts + Tj) * (2 * Ts + Tj + Ta); 961 | //dmax = smax * Ts * (Ts + Tj) * (2 * Ts + Tj + Ta) * (4 * Ts + 2 * Tj + Ta + Tv); 962 | 963 | // Calculate transition times for acceleration phase, using Ts (varying jerk), Tj (constant jerk), and Tv(constant velocity - aka cruice) 964 | t1 = Ts; 965 | t2 = t1 + Tj; 966 | t3 = t2 + Ts; 967 | t4 = t3 + Tj; 968 | t5 = t4 + Ts; 969 | t6 = t5 + Tj; 970 | t7 = t6 + Ts; 971 | 972 | //Constant velocity 973 | if (Tv <= 0){ t8 = t7; 974 | } else { 975 | t8 = t7 + Tv; 976 | } 977 | 978 | // Calculate transition times for deceleration phase (symetrical to acceleration). 979 | 980 | if (double_decel_move || Vi_is_positive){ 981 | t9 = t8 + Ts_rampToCero; 982 | t10 = t9 + Tj_rampToCero; 983 | t11 = t10 + Ts_rampToCero; 984 | t12 = t11 + Tj_rampToCero; 985 | t13 = t12 + Ts_rampToCero; 986 | t14 = t13 + Tj_rampToCero; 987 | t15 = t14 + Ts_rampToCero; 988 | }else{ 989 | t9 = t8 + Ts; 990 | t10 = t9 + Tj; 991 | t11 = t10 + Ts; 992 | t12 = t11 + Tj; 993 | t13 = t12 + Ts; 994 | t14 = t13 + Tj; 995 | t15 = t14 + Ts; 996 | } 997 | 998 | 999 | #ifdef __debug 1000 | SerialUSB.println("*************************"); 1001 | SerialUSB.println("S-curve Transition Times:"); 1002 | SerialUSB.print("t1: "); SerialUSB.println(t1); 1003 | SerialUSB.print("t2: "); SerialUSB.println(t2); 1004 | SerialUSB.print("t3: "); SerialUSB.println(t3); 1005 | SerialUSB.print("t4: "); SerialUSB.println(t4); 1006 | SerialUSB.print("t5: "); SerialUSB.println(t5); 1007 | SerialUSB.print("t6: "); SerialUSB.println(t6); 1008 | SerialUSB.print("t7: "); SerialUSB.println(t7); 1009 | SerialUSB.print("t8: "); SerialUSB.println(t8); 1010 | SerialUSB.print("t9: "); SerialUSB.println(t9); 1011 | SerialUSB.print("t10: "); SerialUSB.println(t10); 1012 | SerialUSB.print("t11: "); SerialUSB.println(t11); 1013 | SerialUSB.print("t12: "); SerialUSB.println(t12); 1014 | SerialUSB.print("t13: "); SerialUSB.println(t13); 1015 | SerialUSB.print("t14: "); SerialUSB.println(t14); 1016 | SerialUSB.print("t15: "); SerialUSB.println(t15); 1017 | #endif 1018 | 1019 | /*************************************/ 1020 | // Asign duration values for T1 to T15 1021 | T1 = Ts; 1022 | T2 = Tj; 1023 | T3 = Ts; 1024 | T4 = Tj; 1025 | T5 = Ts; 1026 | T6 = Tj; 1027 | T7 = Ts; 1028 | T8 = Tv; 1029 | 1030 | if (double_decel_move || Vi_is_positive){ 1031 | T9 = Ts_rampToCero; 1032 | T10 = Tj_rampToCero; 1033 | T11 = Ts_rampToCero; 1034 | T12 = Tj_rampToCero; 1035 | T13 = Ts_rampToCero; 1036 | T14 = Tj_rampToCero; 1037 | T15 = Ts_rampToCero; 1038 | 1039 | #ifdef __debug 1040 | SerialUSB.print("T9: "); SerialUSB.println(T9); 1041 | SerialUSB.print("T10: "); SerialUSB.println(T10); 1042 | SerialUSB.print("T11: "); SerialUSB.println(T11); 1043 | SerialUSB.print("T12: "); SerialUSB.println(T12); 1044 | SerialUSB.print("T13: "); SerialUSB.println(T13); 1045 | SerialUSB.print("T14: "); SerialUSB.println(T14); 1046 | SerialUSB.print("T15: "); SerialUSB.println(T15); 1047 | #endif 1048 | }else{ 1049 | T9 = Ts; 1050 | T10 = Tj; 1051 | T11 = Ts; 1052 | T12 = Tj; 1053 | T13 = Ts; 1054 | T14 = Tj; 1055 | T15 = Ts; 1056 | } 1057 | 1058 | // Print the time intervals 1059 | #ifdef __debug 1060 | SerialUSB.println("********************************"); 1061 | SerialUSB.println("Time Duration for each interval: "); 1062 | SerialUSB.print("T1: "); SerialUSB.println(T1); 1063 | SerialUSB.print("T2: "); SerialUSB.println(T2); 1064 | SerialUSB.print("T3: "); SerialUSB.println(T3); 1065 | SerialUSB.print("T4: "); SerialUSB.println(T4); 1066 | SerialUSB.print("T5: "); SerialUSB.println(T5); 1067 | SerialUSB.print("T6: "); SerialUSB.println(T6); 1068 | SerialUSB.print("T7: "); SerialUSB.println(T7); 1069 | SerialUSB.print("T8: "); SerialUSB.println(T8); 1070 | SerialUSB.print("T9: "); SerialUSB.println(T9); 1071 | SerialUSB.print("T10: "); SerialUSB.println(T10); 1072 | SerialUSB.print("T11: "); SerialUSB.println(T11); 1073 | SerialUSB.print("T12: "); SerialUSB.println(T12); 1074 | SerialUSB.print("T13: "); SerialUSB.println(T13); 1075 | SerialUSB.print("T14: "); SerialUSB.println(T14); 1076 | SerialUSB.print("T15: "); SerialUSB.println(T15); 1077 | #endif 1078 | 1079 | 1080 | 1081 | //Calculate Notations 1082 | v1 = vs + (jmax / 6.0) * pow(T1, 2); 1083 | 1084 | v2 = v1 + (jmax / 2.0) * (T2 * (T1 + T2)); 1085 | 1086 | v3 = v2 + (jmax / 6.0) * T3 * ((3 * T1) + (6 * T2) + (2 * T3)); 1087 | 1088 | v4 = v3 + (amax * T4); 1089 | 1090 | v5 = v4 - (jmax / 6.0) * pow(T5, 2) + (amax * T5); 1091 | 1092 | v6 = v5 - (jmax / 2.0) * pow(T6, 2) + (amax - (jmax / 2.0) * T5) * T6; 1093 | 1094 | v7 = v6 - (jmax / 3.0) * pow(T7, 2) + (amax - ((jmax / 2.0) * T5) - (jmax * T6)) * T7; 1095 | 1096 | 1097 | // If vs (initial velocity) > 0, calculate ramp down within same jerk/acceleration constrains. This may lead to position overshoot, but is only the outcome of missing commands. 1098 | // In essence this is just a failsafe if the algorithm recieves a new position command with a unrealistic ramp-dowm, while in motion. 1099 | // If Tv (time in cruice) exist, then we can subtract the time difference and hit the same target. 1100 | // If initial velocity is greater then cruice, do a dual-ramp-dowm. 1101 | 1102 | 1103 | //If dual-deceleration and if Vi (initial valocity) is positive, calculate new velocities for ramp-dowm 1104 | if (double_decel_move || Vi_is_positive){ 1105 | v1_rd = 0.0f + (jmax_rampToCero / 6.0) * pow(T9, 2); 1106 | v2_rd = v1_rd + (jmax_rampToCero / 2.0) * (T10 * (T9 + T10)); 1107 | v3_rd = v2_rd + (jmax_rampToCero / 6.0) * T11 * ((3 * T9) + (6 * T10) + (2 * T11)); 1108 | v4_rd = v3_rd + (amax_rampToCero * T12); 1109 | v5_rd = v4_rd - (jmax_rampToCero / 6.0) * pow(T13, 2) + (amax_rampToCero * T13); 1110 | v6_rd = v5_rd - (jmax_rampToCero / 2.0) * pow(T14, 2) + (amax_rampToCero - (jmax_rampToCero / 2.0) * T13) * T14; 1111 | v7_rd = v6_rd - (jmax_rampToCero / 3.0) * pow(T15, 2) + (amax_rampToCero - ((jmax_rampToCero / 2.0) * T13) - (jmax_rampToCero * T14)) * T15; 1112 | 1113 | #ifdef __debug 1114 | SerialUSB.print("v1_rd: "); SerialUSB.println(v1_rd); 1115 | SerialUSB.print("v2_rd: "); SerialUSB.println(v2_rd); 1116 | SerialUSB.print("v3_rd: "); SerialUSB.println(v3_rd); 1117 | SerialUSB.print("v4_rd: "); SerialUSB.println(v4_rd); 1118 | SerialUSB.print("v5_rd: "); SerialUSB.println(v5_rd); 1119 | SerialUSB.print("v6_rd: "); SerialUSB.println(v6_rd); 1120 | SerialUSB.print("v7_rd: "); SerialUSB.println(v7_rd); 1121 | #endif 1122 | } 1123 | 1124 | #ifdef __debug 1125 | SerialUSB.print("vs: "); SerialUSB.println(vs); 1126 | SerialUSB.print("v1: "); SerialUSB.println(v1); 1127 | SerialUSB.print("v2: "); SerialUSB.println(v2); 1128 | SerialUSB.print("v3: "); SerialUSB.println(v3); 1129 | SerialUSB.print("v4: "); SerialUSB.println(v4); 1130 | SerialUSB.print("v5: "); SerialUSB.println(v5); 1131 | SerialUSB.print("v6: "); SerialUSB.println(v6); 1132 | SerialUSB.print("v7: "); SerialUSB.println(v7); 1133 | #endif 1134 | 1135 | //last_time = 0.0f; 1136 | last_velocity = Vi; 1137 | Last_position = Xi; 1138 | 1139 | return true; 1140 | } 1141 | 1142 | 1143 | void S4_CurvePlanner::resetTimeVariables() { 1144 | t1 = t2 = t3 = t4 = t5 = t6 = t7 = t8 = t9 = t10 = t11 = t12 = t13 = t14 = t15 = 0.0f; 1145 | } 1146 | 1147 | void S4_CurvePlanner::Initiate_Move(float Pos){ 1148 | 1149 | // set our global of the new position 1150 | Xf_ = Pos; 1151 | 1152 | // At this poin we are atarting to move following the trapezoidal profile 1153 | plannerStartingMovementTimeStamp = millis(); 1154 | 1155 | // take the position from SimpleFOC and set it to our start position 1156 | Xi_ = motor->shaft_angle; 1157 | 1158 | // take the velocity from SimpleFOC and set it to our start velocity 1159 | Vi_ = motor->shaft_velocity; 1160 | 1161 | // TODO: If we are being asked to move but are already in motion, we should go with the current velocity, position, and acceleration 1162 | // and keep moving. 1163 | 1164 | // Now we need to do our calcs before we start moving 1165 | calculateVariables( Xf_, Xi_, Vi_, _Vmax_, _Amax_, _Jmax_, _Smax_); 1166 | motor->target = vel_target; // could possibly put this in the method above 1167 | // Tell user how long this move will take 1168 | #ifdef __debug 1169 | SerialUSB.println("Starting to move to a new position"); 1170 | SerialUSB.print("Time to complete move (secs):"); 1171 | SerialUSB.println(Tf); 1172 | // Velocity and Accel of move 1173 | SerialUSB.print("Velocity for move: "); 1174 | SerialUSB.println(_Vmax_); 1175 | SerialUSB.print("Acceleration: "); 1176 | SerialUSB.println(_Amax_); 1177 | SerialUSB.print("Jerk: "); 1178 | SerialUSB.println(_Jmax_); 1179 | SerialUSB.print("Snap: "); 1180 | SerialUSB.println(_Smax_); 1181 | 1182 | #endif 1183 | // set our global bool so the tick method knows we have a move in motion 1184 | isTrajectoryExecuting = true; 1185 | } 1186 | 1187 | 1188 | float S4_CurvePlanner::findTf(float Ts, float Tj, float Ta, float Tv, float Td) { 1189 | float Tf = 0.0f; 1190 | 1191 | // Perform the summation using a loop 1192 | for (int i = 1; i <= 15; ++i) { 1193 | Tf += i * Ts + i * Tj + i * Ta + i * Tv; 1194 | } 1195 | } 1196 | 1197 | 1198 | 1199 | void S4_CurvePlanner::RuntimePlanner(float t) { 1200 | //float currentTrajectoryTime = (millis() - plannerStartingMov 1201 | 1202 | if (t >= t0 && t < t1) { 1203 | // Code for the [t0, t1] time range 1204 | 1205 | 1206 | now_ = micros(); 1207 | deltaTime = now_ - last_time; 1208 | //convert to seconds 1209 | //deltaTime = deltaTime / 1e6f; 1210 | deltaTime = 0.025f; 1211 | 1212 | tau1 = t - t0; 1213 | 1214 | 1215 | if (!double_decel_move){ 1216 | jerk_now = jmax / T1 * tau1; 1217 | acel_now = (jmax / (2.0f * T1)) * pow(tau1, 2); 1218 | vel_target = (jmax / (6.0f * T1)) * pow(tau1, 3) + vs; 1219 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1220 | }else{ 1221 | float v7_2 = v7 + vmax; 1222 | jerk_now = - (jmax / T1) * tau1; 1223 | acel_now = - (jmax / (2.0f * T1)) * pow(tau1, 2); 1224 | vel_target = - (jmax / (6.0f * T1)) * pow(tau1, 3) + v7_2; 1225 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1226 | } 1227 | 1228 | last_time = now_; 1229 | last_velocity = vel_target; 1230 | Last_position = pos_target; 1231 | last_acceleratio = acel_now; 1232 | 1233 | #ifdef __debug 1234 | print_debug_variables(); 1235 | #endif 1236 | 1237 | } 1238 | 1239 | 1240 | if (t >= t1 && t < t2) { 1241 | // Code for the [t1, t2] time range 1242 | 1243 | now_ = micros(); 1244 | deltaTime = now_ - last_time; 1245 | //convert to seconds 1246 | //deltaTime = deltaTime / 1e6f; 1247 | deltaTime = 0.025f; 1248 | 1249 | tau2 = t - t1; 1250 | 1251 | if (!double_decel_move){ 1252 | jerk_now = jmax; 1253 | acel_now = jmax * tau2 + (jmax / 2.0) * T1; 1254 | vel_target = (jmax / 2.0) * pow(tau2, 2) + (jmax / 2.0) * T1 * tau2 + v1; 1255 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1256 | }else{ 1257 | float v6_2 = v6 + vmax; 1258 | jerk_now = -jmax; 1259 | acel_now = -jmax * tau2 - (jmax / 2.0) * T1; 1260 | vel_target = -(jmax / 2.0) * pow(tau2, 2) - (jmax / 2.0) * T1 * tau2 + v6_2; 1261 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1262 | } 1263 | 1264 | last_time = now_; 1265 | last_velocity = vel_target; 1266 | Last_position = pos_target; 1267 | last_acceleratio = acel_now; 1268 | 1269 | #ifdef __debug 1270 | print_debug_variables(); 1271 | #endif 1272 | 1273 | } 1274 | 1275 | 1276 | if (t >= t2 && t < t3) { 1277 | // Code for the [t2, t3] time range 1278 | 1279 | now_ = micros(); 1280 | deltaTime = now_ - last_time; 1281 | //convert to seconds 1282 | //deltaTime = deltaTime / 1e6f; 1283 | deltaTime = 0.025f; 1284 | 1285 | tau3 = t - t2; 1286 | 1287 | if (!double_decel_move){ 1288 | jerk_now = jmax - (jmax / T3) * tau3; 1289 | acel_now = (jmax * tau3) - (jmax / (2 * T3)) * pow(tau3, 2) + (jmax / 2) * (T1 + (2 * T2)); 1290 | vel_target = (jmax / 2.0f) * pow(tau3, 2) - (jmax / (6.0f * T3)) * pow(tau3, 3) + (jmax / 2.0f) * (T1 + (2 * T2)) * tau3 + v2; 1291 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1292 | 1293 | }else{ 1294 | float v5_2 = v5 + vmax; 1295 | jerk_now = - jmax + ((jmax / T3) * tau3); 1296 | acel_now = - (jmax * tau3) + (jmax / (2 * T3)) * pow(tau3, 2) - (jmax / 2) * (T3 + (2 * T4)); 1297 | vel_target = - (jmax / 2.0) * pow(tau3, 2) + ((jmax / (6.0 * T3)) * pow(tau3, 3)) - ((jmax / 2.0) * (T3 + (2 * T4))) * tau3 + v5_2; 1298 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1299 | } 1300 | 1301 | last_time = now_; 1302 | last_velocity = vel_target; 1303 | Last_position = pos_target; 1304 | last_acceleratio = acel_now; 1305 | 1306 | #ifdef __debug 1307 | print_debug_variables(); 1308 | #endif 1309 | } 1310 | 1311 | if (t >= t3 && t < t4) { 1312 | // Code for the [t3, t4] time range 1313 | 1314 | now_ = micros(); 1315 | deltaTime = now_ - last_time; 1316 | //convert to seconds 1317 | //deltaTime = deltaTime / 1e6f; 1318 | deltaTime = 0.025f; 1319 | 1320 | tau4 = t - t3; 1321 | 1322 | if (!double_decel_move){ 1323 | jerk_now = 0.0; 1324 | acel_now = amax; 1325 | vel_target = (amax * tau4) + v3; 1326 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1327 | }else{ 1328 | 1329 | float v3_3 = v3 + vmax; 1330 | jerk_now = 0.0; 1331 | acel_now = -amax; 1332 | vel_target = -(amax * tau4) + v3_3; 1333 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1334 | } 1335 | 1336 | last_time = now_; 1337 | last_velocity = vel_target; 1338 | Last_position = pos_target; 1339 | last_acceleratio = acel_now; 1340 | 1341 | #ifdef __debug 1342 | print_debug_variables(); 1343 | #endif 1344 | } 1345 | 1346 | 1347 | if (t >= t4 && t < t5) { 1348 | // Code for the [t4, t5] time range 1349 | 1350 | now_ = micros(); 1351 | deltaTime = now_ - last_time; 1352 | //convert to seconds 1353 | //deltaTime = deltaTime / 1e6f; 1354 | deltaTime = 0.025f; 1355 | 1356 | tau5 = t - t4; 1357 | 1358 | if (!double_decel_move){ 1359 | jerk_now = - (jmax / T5) * tau5; 1360 | acel_now = - (jmax / (2.0 * T5)) * pow(tau5, 2) + amax; 1361 | vel_target = -jmax / (6.0 * T5) * pow(tau5, 3) + amax * tau5 + v4; 1362 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1363 | } else { 1364 | float v3_2 = v3 + vmax; 1365 | jerk_now = jmax / T5 * tau5; 1366 | acel_now = (jmax / (2.0 * T5)) * pow(tau5, 2) - amax; 1367 | vel_target = jmax / (6.0 * T5) * pow(tau5, 3) - amax * tau5 + v3_2; 1368 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1369 | } 1370 | 1371 | 1372 | last_time = now_; 1373 | last_velocity = vel_target; 1374 | Last_position = pos_target; 1375 | last_acceleratio = acel_now; 1376 | 1377 | 1378 | #ifdef __debug 1379 | print_debug_variables(); 1380 | #endif 1381 | } 1382 | 1383 | 1384 | if (t >= t5 && t < t6) { 1385 | // Code for the [t5, t6] time range 1386 | 1387 | now_ = micros(); 1388 | deltaTime = now_ - last_time; 1389 | //convert to seconds 1390 | //deltaTime = deltaTime / 1e6f; 1391 | deltaTime = 0.025f; 1392 | 1393 | tau6 = t - t5; 1394 | 1395 | if (!double_decel_move){ 1396 | jerk_now = -jmax; 1397 | acel_now = -(jmax * tau6) + amax - (jmax / 2.0) * T5; 1398 | float tempt_cal = amax - (jmax / 2.0) * T5; 1399 | vel_target = - (jmax / 2.0) * pow(tau6, 2) + tempt_cal * tau6 + v5; 1400 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1401 | } else { 1402 | float v2_2 = v2 + vmax; 1403 | jerk_now = jmax; 1404 | acel_now = (jmax * tau6) - amax + (jmax / 2.0) * T5; 1405 | float tempt_cal = amax - (jmax / 2.0) * T5; 1406 | vel_target = (jmax / 2.0) * pow(tau6, 2) - tempt_cal * tau6 + v2_2; 1407 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1408 | } 1409 | 1410 | last_time = now_; 1411 | last_velocity = vel_target; 1412 | Last_position = pos_target; 1413 | last_acceleratio = acel_now; 1414 | 1415 | #ifdef __debug 1416 | print_debug_variables(); 1417 | #endif 1418 | } 1419 | 1420 | 1421 | if (t >= t6 && t < t7) { 1422 | // Code for the [t6, t7] time range 1423 | 1424 | now_ = micros(); 1425 | deltaTime = now_ - last_time; 1426 | //convert to seconds 1427 | //deltaTime = deltaTime / 1e6f; 1428 | deltaTime = 0.025f; 1429 | 1430 | tau7 = t - t6; 1431 | 1432 | if (!double_decel_move){ 1433 | jerk_now = - jmax + ((jmax / T7) * tau7); 1434 | acel_now = - (jmax * tau7) + ((jmax / (2.0 * T7)) * pow(tau7, 2)) + amax - ((jmax / 2.0) * T5) - (jmax * T6); 1435 | vel_target = - (jmax / 2.0) * pow(tau7, 2) + (jmax / (6.0 * T7)) * pow(tau7, 3) + (amax - (jmax / 2.0) * T5 - (jmax * T6)) * tau7 + v6; 1436 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1437 | }else{ 1438 | float v1_2 = v1 + vmax; 1439 | jerk_now = jmax - (jmax / T7) * tau7; 1440 | acel_now = (jmax * tau7) - ((jmax / (2.0 * T7)) * pow(tau7, 2)) - amax + ((jmax / 2.0) * T5) + (jmax * T6); 1441 | vel_target = (jmax / 2.0) * pow(tau7, 2) - (jmax / (6.0 * T7)) * pow(tau7, 3) - (amax - (jmax / 2.0) * T5 - (jmax * T6)) * tau7 + v1_2; 1442 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1443 | } 1444 | 1445 | 1446 | last_time = now_; 1447 | last_velocity = vel_target; 1448 | Last_position = pos_target; 1449 | last_acceleratio = acel_now; 1450 | 1451 | #ifdef __debug 1452 | print_debug_variables(); 1453 | #endif 1454 | } 1455 | 1456 | 1457 | if (t >= t7 && t < t8) { 1458 | // Code for the [t7, t8] time range 1459 | 1460 | now_ = micros(); 1461 | deltaTime = now_ - last_time; 1462 | //convert to seconds 1463 | //deltaTime = deltaTime / 1e6f; 1464 | deltaTime = 0.025f; 1465 | 1466 | tau8 = t - t7; 1467 | 1468 | // Use v7 and q7 as needed for further calculations 1469 | // j(t) and a(t) are both 0 within this range 1470 | jerk_now = 0.0f; 1471 | acel_now = 0.0f; 1472 | vel_target = v7; 1473 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1474 | 1475 | last_time = now_; 1476 | last_velocity = vel_target; 1477 | Last_position = pos_target; 1478 | last_acceleratio = acel_now; 1479 | 1480 | #ifdef __debug 1481 | print_debug_variables(); 1482 | #endif 1483 | } 1484 | 1485 | 1486 | if (t >= t8 && t < t9) { 1487 | // Code for the [t8, t9] time range 1488 | 1489 | now_ = micros(); 1490 | //deltaTime = now_ - last_time; 1491 | //convert to seconds 1492 | //deltaTime = deltaTime / 1e6f; 1493 | deltaTime = 0.025f; 1494 | 1495 | tau9 = t - t8; 1496 | 1497 | if (!double_decel_move && !Vi_is_positive){ 1498 | jerk_now = - (jmax / T9) * tau9; 1499 | acel_now = - (jmax / (2.0f * T9)) * pow(tau9, 2); 1500 | vel_target = - (jmax / (6.0f * T9)) * pow(tau9, 3) + v7; 1501 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1502 | }else{ 1503 | //Execute ramp down to 0 velocity 1504 | jerk_now = - (jmax_rampToCero / T9) * tau9; 1505 | acel_now = - (jmax_rampToCero / (2.0f * T9)) * pow(tau9, 2); 1506 | vel_target = - (jmax_rampToCero / (6.0f * T9)) * pow(tau9, 3) + v7_rd; 1507 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1508 | } 1509 | 1510 | last_time = now_; 1511 | last_velocity = vel_target; 1512 | Last_position = pos_target; 1513 | last_acceleratio = acel_now; 1514 | 1515 | 1516 | #ifdef __debug 1517 | print_debug_variables(); 1518 | #endif 1519 | } 1520 | 1521 | 1522 | if (t >= t9 && t < t10) { 1523 | 1524 | now_ = micros(); 1525 | deltaTime = now_ - last_time; 1526 | //convert to seconds 1527 | //deltaTime = deltaTime / 1e6f; 1528 | deltaTime = 0.025f; 1529 | 1530 | tau10 = t - t9; 1531 | 1532 | if (!double_decel_move && !Vi_is_positive){ 1533 | jerk_now = -jmax; 1534 | acel_now = -jmax * tau10 - (jmax / 2.0) * T9; 1535 | vel_target = -(jmax / 2.0) * pow(tau10, 2) - (jmax / 2.0) * T9 * tau10 + v6; 1536 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1537 | }else{ 1538 | jerk_now = -jmax_rampToCero; 1539 | acel_now = -jmax_rampToCero * tau10 - (jmax_rampToCero / 2.0) * T9; 1540 | vel_target = -(jmax_rampToCero / 2.0) * pow(tau10, 2) - (jmax_rampToCero / 2.0) * T9 * tau10 + v6_rd; 1541 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1542 | } 1543 | 1544 | last_time = now_; 1545 | last_velocity = vel_target; 1546 | Last_position = pos_target; 1547 | last_acceleratio = acel_now; 1548 | 1549 | #ifdef __debug 1550 | print_debug_variables(); 1551 | #endif 1552 | } 1553 | 1554 | 1555 | if (t >= t10 && t < t11) { 1556 | // Code for the [t10, t11] time range 1557 | now_ = micros(); 1558 | deltaTime = now_ - last_time; 1559 | //convert to seconds 1560 | //deltaTime = deltaTime / 1e6f; 1561 | deltaTime = 0.025f; 1562 | 1563 | tau11 = t - t10; 1564 | 1565 | if (!double_decel_move && !Vi_is_positive){ 1566 | jerk_now = - jmax + ((jmax / T11) * tau11); 1567 | acel_now = - (jmax * tau11) + (jmax / (2 * T11)) * pow(tau11, 2) - (jmax / 2) * (T9 + (2 * T10)); 1568 | vel_target = - (jmax / 2.0) * pow(tau11, 2) + (jmax / (6.0 * T11)) * pow(tau11, 3) - (jmax / 2.0) * (T9 + (2 * T10)) * tau11 + v5; 1569 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1570 | 1571 | }else{ 1572 | jerk_now = - jmax_rampToCero + ((jmax_rampToCero / T11) * tau11); 1573 | acel_now = - (jmax_rampToCero * tau11) + (jmax_rampToCero / (2 * T11)) * pow(tau11, 2) - (jmax_rampToCero / 2) * (T9 + (2 * T10)); 1574 | vel_target = - (jmax_rampToCero / 2.0) * pow(tau11, 2) + (jmax_rampToCero / (6.0 * T11)) * pow(tau11, 3) - (jmax_rampToCero / 2.0) * (T9 + (2 * T10)) * tau11 + v5_rd; 1575 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1576 | } 1577 | 1578 | last_time = now_; 1579 | last_velocity = vel_target; 1580 | Last_position = pos_target; 1581 | last_acceleratio = acel_now; 1582 | 1583 | 1584 | #ifdef __debug 1585 | print_debug_variables(); 1586 | #endif 1587 | } 1588 | 1589 | 1590 | if (t >= t11 && t < t12) { 1591 | // Code for the [t11, t12] time range 1592 | 1593 | now_ = micros(); 1594 | deltaTime = now_ - last_time; 1595 | //convert to seconds 1596 | //deltaTime = deltaTime / 1e6f; 1597 | deltaTime = 0.025f; 1598 | 1599 | tau12 = t - t12; 1600 | 1601 | if (!double_decel_move && !Vi_is_positive){ 1602 | jerk_now = 0.0; 1603 | acel_now = -amax; 1604 | vel_target = -(amax * tau12) + v3; 1605 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1606 | }else{ 1607 | jerk_now = 0.0; 1608 | acel_now = -amax_rampToCero; 1609 | vel_target = -(amax_rampToCero * tau12) + v3_rd; 1610 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1611 | } 1612 | 1613 | last_time = now_; 1614 | last_velocity = vel_target; 1615 | Last_position = pos_target; 1616 | last_acceleratio = acel_now; 1617 | 1618 | 1619 | #ifdef __debug 1620 | print_debug_variables(); 1621 | #endif 1622 | } 1623 | 1624 | 1625 | if (t >= t12 && t < t13) { 1626 | // Code for the [t12, t13] time range 1627 | 1628 | now_ = micros(); 1629 | //deltaTime = now_ - last_time; 1630 | 1631 | //convert to seconds 1632 | //deltaTime = deltaTime / 1e6f; 1633 | deltaTime = 0.025f; 1634 | 1635 | tau13 = t - t12; 1636 | 1637 | if (!double_decel_move && !Vi_is_positive){ 1638 | jerk_now = jmax / T13 * tau13; 1639 | acel_now = (jmax / (2.0 * T13)) * pow(tau13, 2) - amax; 1640 | vel_target = jmax / (6.0 * T13) * pow(tau13, 3) - amax * tau13 + v3; 1641 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1642 | }else{ 1643 | jerk_now = jmax_rampToCero / T13 * tau13; 1644 | acel_now = (jmax_rampToCero / (2.0 * T13)) * pow(tau13, 2) - amax_rampToCero; 1645 | vel_target = jmax_rampToCero / (6.0 * T13) * pow(tau13, 3) - amax_rampToCero * tau13 + v3_rd; 1646 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1647 | } 1648 | 1649 | last_time = now_; 1650 | last_velocity = vel_target; 1651 | Last_position = pos_target; 1652 | last_acceleratio = acel_now; 1653 | 1654 | #ifdef __debug 1655 | print_debug_variables(); 1656 | #endif 1657 | } 1658 | 1659 | 1660 | if (t >= t13 && t < t14) { 1661 | // Code for the [t13, t14] time range 1662 | 1663 | now_ = micros(); 1664 | deltaTime = now_ - last_time; 1665 | //convert to seconds 1666 | //deltaTime = deltaTime / 1e6f; 1667 | deltaTime = 0.025f; 1668 | 1669 | tau14 = t - t13; 1670 | 1671 | if (!double_decel_move && !Vi_is_positive){ 1672 | jerk_now = jmax; 1673 | acel_now = (jmax * tau14) - amax + (jmax / 2.0) * T9; 1674 | float tempt_cal = amax - (jmax / 2.0) * T9; 1675 | vel_target = (jmax / 2.0) * pow(tau14, 2) - tempt_cal * tau14 + v2; 1676 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1677 | 1678 | }else{ 1679 | jerk_now = jmax_rampToCero; 1680 | acel_now = (jmax_rampToCero * tau14) - amax_rampToCero + (jmax_rampToCero / 2.0) * T9; 1681 | float tempt_cal_rampToCero = amax_rampToCero - (jmax_rampToCero / 2.0) * T9; 1682 | vel_target = (jmax_rampToCero / 2.0) * pow(tau14, 2) - tempt_cal_rampToCero * tau14 + v2_rd; 1683 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1684 | } 1685 | 1686 | last_time = now_; 1687 | last_velocity = vel_target; 1688 | Last_position = pos_target; 1689 | last_acceleratio = acel_now; 1690 | 1691 | #ifdef __debug 1692 | print_debug_variables(); 1693 | #endif 1694 | } 1695 | 1696 | 1697 | if (t >= t14 && t < t15) { 1698 | // Code for the [t14, t15] time range 1699 | 1700 | 1701 | now_ = micros(); 1702 | //deltaTime = now_ - last_time; 1703 | 1704 | //convert to seconds 1705 | //deltaTime = deltaTime / 1e6f; 1706 | deltaTime = 0.025f; 1707 | 1708 | tau15 = t - t14; 1709 | 1710 | if (!double_decel_move && !Vi_is_positive){ 1711 | jerk_now = jmax - (jmax / T15) * tau15; 1712 | acel_now = (jmax * tau15) - ((jmax / (2.0 * T15)) * pow(tau15, 2)) - amax + ((jmax / 2.0) * T9) + (jmax * T10); 1713 | vel_target = (jmax / 2.0) * pow(tau15, 2) - (jmax / (6.0 * T15)) * pow(tau15, 3) - (amax - (jmax / 2.0) * T9 - (jmax * T10)) * tau15 + v1; 1714 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1715 | }else{ 1716 | jerk_now = jmax_rampToCero - (jmax_rampToCero / T15) * tau15; 1717 | acel_now = (jmax_rampToCero * tau15) - ((jmax_rampToCero / (2.0 * T15)) * pow(tau15, 2)) - amax_rampToCero + ((jmax_rampToCero / 2.0) * T9) + (jmax_rampToCero * T10); 1718 | pos_target = Last_position + ((last_velocity + vel_target) / 2.0f + 0.5f * (last_acceleratio + acel_now) * deltaTime) * deltaTime; 1719 | } 1720 | 1721 | last_time = now_; 1722 | last_velocity = vel_target; 1723 | Last_position = pos_target; 1724 | last_acceleratio = acel_now; 1725 | 1726 | #ifdef __debug 1727 | print_debug_variables(); 1728 | #endif 1729 | 1730 | } 1731 | } 1732 | 1733 | void S4_CurvePlanner::print_debug_variables(){ 1734 | 1735 | SerialUSB.print("jerk_now:"); 1736 | SerialUSB.print(jerk_now, 5); 1737 | SerialUSB.print(","); 1738 | SerialUSB.print("acel_now:"); 1739 | SerialUSB.print(acel_now, 5); 1740 | SerialUSB.print(","); 1741 | SerialUSB.print("vel_target:"); 1742 | SerialUSB.print(vel_target, 5); 1743 | SerialUSB.print(","); 1744 | SerialUSB.print("pos_target:"); 1745 | SerialUSB.print(pos_target, 5); 1746 | SerialUSB.println(","); 1747 | } 1748 | 1749 | 1750 | 1751 | -------------------------------------------------------------------------------- /S4_CurvePlanner/S4_CurvePlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef __S4_CurvePlanner__H 2 | #define __S4_CurvePlanner__H 3 | #include 4 | #include "../CircularBuffer/CircularBuffer.h" 5 | 6 | class S4_CurvePlanner 7 | { 8 | public: 9 | 10 | S4_CurvePlanner(int); 11 | void executeCommand(char *command, char *command2); 12 | void doGcommandBuffer(char *command); 13 | void doMCommand(char *command); 14 | void linkMotor(FOCMotor*); 15 | void runPlannerOnTick(); 16 | bool isPlannerMoving(); 17 | void resetTimeVariables(); 18 | 19 | //For testing purposes 20 | float t0, t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15; 21 | bool calculateVariables(float Xf, float Xi, float Vi, float Vmax_, float Amax_, float Jmax_, float Smax_); 22 | void RuntimePlanner(float currentTrajectoryTime); 23 | bool double_decel_move = false; 24 | bool Vi_is_positive = false; 25 | float last_time; 26 | 27 | private: 28 | 29 | //SETUP 30 | FOCMotor * motor; 31 | unsigned long plannerTimeStep; 32 | int plannerPeriod = 0.5; // 1000 / this number = Hz, i.e. 1000 / 100 = 10Hz, 1000 / 10 = 100Hz, 1000 / 5 = 200Hz, 1000 / 1 = 1000hZ 33 | float _Vmax_ = 30.0f; // # Velocity max (rads/s) 34 | float _Amax_ = 20.0f; // # Acceleration max (rads/s/s) 35 | float _Jmax_ = 20.0f; // # Jerk max (rads/s/s/s) 36 | float _Smax_ = 20.0f; // # Snap max (rads/s/s/s/s) 37 | float qs; // Start position 38 | float qe; // Final position 39 | 40 | //Variables for calculating pos_target using deltaTime, velocity and acceleration. 41 | float Last_position; 42 | float last_velocity; 43 | float last_acceleratio; 44 | float now_; 45 | float deltaTime; 46 | 47 | //Variables related to trajectory/time 48 | //Tau == DeltaTime 49 | float tau1, tau2, tau3, tau4, tau5, tau6, tau7, tau8, tau9, tau10, tau11, tau12, tau13, tau14, tau15; 50 | 51 | //T(x) == TimeSegment duration 52 | float T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15; 53 | 54 | //v(x) == Zero crossing time for each time-segment. 55 | float vs, v0, v1, v2, v3, v4, v5, v6, v7; 56 | float v1_rd, v2_rd, v3_rd, v4_rd, v5_rd, v6_rd, v7_rd; 57 | 58 | //Functions related to trajectory timing. Here we calculate the actual execution time, and find out what type of move we are in. 59 | void CalculateTs(float vmax, float amax, float jmax); 60 | float CalculateTj(float Ts_jerk, float Tj_vmax, float Tj_jmax, float Tj_amax); 61 | float CalculateTa(float Ts_, float Tj_, float amax_, float vmax_); 62 | float CalculateTv(); 63 | float CalculateTv_rampToCero(); 64 | 65 | float findTf(float Ts, float Tj, float Ta, float Tv, float Td); 66 | 67 | float jmax; // Max jerk 68 | float amax; // Max acceleration 69 | float vmax; // Max velocity 70 | float dmax; // Max displacemnt 71 | float smax; // MAX snap 72 | 73 | //Variables for cases where ramp down is diferent from ramp up. 74 | float jmax_rampToCero; // Max jerk 75 | float amax_rampToCero; // Max acceleration 76 | float vmax_rampToCero; // Max velocity 77 | 78 | //Trajectory parameters in time Ts (varying jerk ), Tj (constant jerk), Ta (constant acceleration), Tv (constant velocity), , Tf (final). 79 | float Ts; 80 | float Tj; 81 | float Ta; 82 | float Tv; 83 | float Tf; 84 | 85 | //Initial Ts calculations made in order to determine the type of move. 86 | float Ts_d; 87 | float Ts_v; 88 | float Ts_a; 89 | float Ts_j; 90 | 91 | //Jerk calculations 92 | float Tjv; 93 | float Tjd; 94 | float Tja; 95 | 96 | //Acceleration calculations 97 | float Tad; 98 | float Tav; 99 | 100 | //Variables for ramp-down, if ramp-donw is different from ramp-up. 101 | float Ts_rampToCero; 102 | float Ts_d_rampToCero; 103 | float Tv_rampToCero; 104 | float Ta_rampToCero; 105 | float Tj_rampToCero; 106 | 107 | //Current planner output @execution time 108 | float pos_target; 109 | float jerk_now; 110 | float acel_now; 111 | float vel_target; 112 | 113 | //M400 commands tells the planner to wait until all moves are completed before proceeding. 114 | bool m400_flag = false; 115 | 116 | //Initial values and target position 117 | float Xi_, Xf_, Vi_; 118 | 119 | //Are we in a move 120 | bool isTrajectoryExecuting; 121 | 122 | //When this movement began 123 | unsigned long plannerStartingMovementTimeStamp; 124 | 125 | //Send new position for trajectory calculation 126 | void Initiate_Move(float Pos); 127 | 128 | //Varius funtions related to trajectory planning. 129 | void disp_void(); 130 | void disp_void_rampToCero(); 131 | void vel_void(); 132 | void vel_void_rampToCero(); 133 | void acel_void(); 134 | void acel_void_rampToCero(); 135 | void jerk_void(); 136 | void jerk_void_rampToCero(); 137 | void Tjd_void(); 138 | void Tjv_void(); 139 | void Tad_void(); 140 | void Tad_void_rampToCero(); 141 | void Tav_void(); 142 | 143 | //Debug output 144 | void print_debug_variables(); 145 | 146 | //Buffer related variables 147 | char* tailItem; 148 | char* nextItem; 149 | char* command_char; 150 | 151 | //CircuarBuffer holder for commands, set desired size. 152 | CircularBuffer buffer = CircularBuffer(100); 153 | 154 | //Additional setpoints for trajectory 155 | float dXmin; 156 | 157 | }; 158 | 159 | #endif --------------------------------------------------------------------------------