Michael Ernst Peter / PM2_Libary

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

Motion.cpp@29:335fb9b01ca7, 2022-05-13 (annotated)

Committer:
pmic
Date:
Fri May 13 20:53:33 2022 +0000
Revision:
29:335fb9b01ca7
Motion integrated into SpeedController and PositionController class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 29:335fb9b01ca7 1 /*
pmic 29:335fb9b01ca7 2 * Motion.cpp
pmic 29:335fb9b01ca7 3 * Copyright (c) 2022, ZHAW
pmic 29:335fb9b01ca7 4 * All rights reserved.
pmic 29:335fb9b01ca7 5 */
pmic 29:335fb9b01ca7 6
pmic 29:335fb9b01ca7 7 #include <cmath>
pmic 29:335fb9b01ca7 8 #include <algorithm>
pmic 29:335fb9b01ca7 9 #include "Motion.h"
pmic 29:335fb9b01ca7 10
pmic 29:335fb9b01ca7 11 using namespace std;
pmic 29:335fb9b01ca7 12
pmic 29:335fb9b01ca7 13 const float Motion::DEFAULT_LIMIT = 1.0f; // default value for limits
pmic 29:335fb9b01ca7 14 const float Motion::MINIMUM_LIMIT = 1.0e-9f; // smallest value allowed for limits
pmic 29:335fb9b01ca7 15
pmic 29:335fb9b01ca7 16 /**
pmic 29:335fb9b01ca7 17 * Creates a <code>Motion</code> object.
pmic 29:335fb9b01ca7 18 * The values for position, velocity and acceleration are set to 0.
pmic 29:335fb9b01ca7 19 */
pmic 29:335fb9b01ca7 20 Motion::Motion() {
pmic 29:335fb9b01ca7 21
pmic 29:335fb9b01ca7 22 position = 0.0;
pmic 29:335fb9b01ca7 23 velocity = 0.0f;
pmic 29:335fb9b01ca7 24
pmic 29:335fb9b01ca7 25 profileVelocity = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 26 profileAcceleration = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 27 profileDeceleration = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 28 }
pmic 29:335fb9b01ca7 29
pmic 29:335fb9b01ca7 30 /**
pmic 29:335fb9b01ca7 31 * Creates a <code>Motion</code> object with given values for position and velocity.
pmic 29:335fb9b01ca7 32 * @param position the initial position value of this motion, given in [m] or [rad].
pmic 29:335fb9b01ca7 33 * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
pmic 29:335fb9b01ca7 34 */
pmic 29:335fb9b01ca7 35 Motion::Motion(double position, float velocity) {
pmic 29:335fb9b01ca7 36
pmic 29:335fb9b01ca7 37 this->position = position;
pmic 29:335fb9b01ca7 38 this->velocity = velocity;
pmic 29:335fb9b01ca7 39
pmic 29:335fb9b01ca7 40 profileVelocity = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 41 profileAcceleration = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 42 profileDeceleration = DEFAULT_LIMIT;
pmic 29:335fb9b01ca7 43 }
pmic 29:335fb9b01ca7 44
pmic 29:335fb9b01ca7 45 /**
pmic 29:335fb9b01ca7 46 * Creates a <code>Motion</code> object with given values for position and velocity.
pmic 29:335fb9b01ca7 47 * @param motion another <code>Motion</code> object to copy the values from.
pmic 29:335fb9b01ca7 48 */
pmic 29:335fb9b01ca7 49 Motion::Motion(const Motion& motion) {
pmic 29:335fb9b01ca7 50
pmic 29:335fb9b01ca7 51 position = motion.position;
pmic 29:335fb9b01ca7 52 velocity = motion.velocity;
pmic 29:335fb9b01ca7 53
pmic 29:335fb9b01ca7 54 profileVelocity = motion.profileVelocity;
pmic 29:335fb9b01ca7 55 profileAcceleration = motion.profileAcceleration;
pmic 29:335fb9b01ca7 56 profileDeceleration = motion.profileDeceleration;
pmic 29:335fb9b01ca7 57 }
pmic 29:335fb9b01ca7 58
pmic 29:335fb9b01ca7 59 /**
pmic 29:335fb9b01ca7 60 * Deletes the Motion object.
pmic 29:335fb9b01ca7 61 */
pmic 29:335fb9b01ca7 62 Motion::~Motion() {}
pmic 29:335fb9b01ca7 63
pmic 29:335fb9b01ca7 64 /**
pmic 29:335fb9b01ca7 65 * Sets the values for position and velocity.
pmic 29:335fb9b01ca7 66 * @param position the desired position value of this motion, given in [m] or [rad].
pmic 29:335fb9b01ca7 67 * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
pmic 29:335fb9b01ca7 68 */
pmic 29:335fb9b01ca7 69 void Motion::set(double position, float velocity) {
pmic 29:335fb9b01ca7 70
pmic 29:335fb9b01ca7 71 this->position = position;
pmic 29:335fb9b01ca7 72 this->velocity = velocity;
pmic 29:335fb9b01ca7 73 }
pmic 29:335fb9b01ca7 74
pmic 29:335fb9b01ca7 75 /**
pmic 29:335fb9b01ca7 76 * Sets the values for position and velocity.
pmic 29:335fb9b01ca7 77 * @param motion another <code>Motion</code> object to copy the values from.
pmic 29:335fb9b01ca7 78 */
pmic 29:335fb9b01ca7 79 void Motion::set(const Motion& motion) {
pmic 29:335fb9b01ca7 80
pmic 29:335fb9b01ca7 81 position = motion.position;
pmic 29:335fb9b01ca7 82 velocity = motion.velocity;
pmic 29:335fb9b01ca7 83 }
pmic 29:335fb9b01ca7 84
pmic 29:335fb9b01ca7 85 /**
pmic 29:335fb9b01ca7 86 * Sets the position value.
pmic 29:335fb9b01ca7 87 * @param position the desired position value of this motion, given in [m] or [rad].
pmic 29:335fb9b01ca7 88 */
pmic 29:335fb9b01ca7 89 void Motion::setPosition(double position) {
pmic 29:335fb9b01ca7 90
pmic 29:335fb9b01ca7 91 this->position = position;
pmic 29:335fb9b01ca7 92 }
pmic 29:335fb9b01ca7 93
pmic 29:335fb9b01ca7 94 /**
pmic 29:335fb9b01ca7 95 * Gets the position value.
pmic 29:335fb9b01ca7 96 * @return the position value of this motion, given in [m] or [rad].
pmic 29:335fb9b01ca7 97 */
pmic 29:335fb9b01ca7 98 double Motion::getPosition() {
pmic 29:335fb9b01ca7 99
pmic 29:335fb9b01ca7 100 return position;
pmic 29:335fb9b01ca7 101 }
pmic 29:335fb9b01ca7 102
pmic 29:335fb9b01ca7 103 /**
pmic 29:335fb9b01ca7 104 * Sets the velocity value.
pmic 29:335fb9b01ca7 105 * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
pmic 29:335fb9b01ca7 106 */
pmic 29:335fb9b01ca7 107 void Motion::setVelocity(float velocity) {
pmic 29:335fb9b01ca7 108
pmic 29:335fb9b01ca7 109 this->velocity = velocity;
pmic 29:335fb9b01ca7 110 }
pmic 29:335fb9b01ca7 111
pmic 29:335fb9b01ca7 112 /**
pmic 29:335fb9b01ca7 113 * Gets the velocity value.
pmic 29:335fb9b01ca7 114 * @return the velocity value of this motion, given in [m/s] or [rad/s].
pmic 29:335fb9b01ca7 115 */
pmic 29:335fb9b01ca7 116 float Motion::getVelocity() {
pmic 29:335fb9b01ca7 117
pmic 29:335fb9b01ca7 118 return velocity;
pmic 29:335fb9b01ca7 119 }
pmic 29:335fb9b01ca7 120
pmic 29:335fb9b01ca7 121 /**
pmic 29:335fb9b01ca7 122 * Sets the limit for the velocity value.
pmic 29:335fb9b01ca7 123 * @param profileVelocity the limit of the velocity.
pmic 29:335fb9b01ca7 124 */
pmic 29:335fb9b01ca7 125 void Motion::setProfileVelocity(float profileVelocity) {
pmic 29:335fb9b01ca7 126
pmic 29:335fb9b01ca7 127 if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 128 }
pmic 29:335fb9b01ca7 129
pmic 29:335fb9b01ca7 130 /**
pmic 29:335fb9b01ca7 131 * Sets the limit for the acceleration value.
pmic 29:335fb9b01ca7 132 * @param profileAcceleration the limit of the acceleration.
pmic 29:335fb9b01ca7 133 */
pmic 29:335fb9b01ca7 134 void Motion::setProfileAcceleration(float profileAcceleration) {
pmic 29:335fb9b01ca7 135
pmic 29:335fb9b01ca7 136 if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 137 }
pmic 29:335fb9b01ca7 138
pmic 29:335fb9b01ca7 139 /**
pmic 29:335fb9b01ca7 140 * Sets the limit for the deceleration value.
pmic 29:335fb9b01ca7 141 * @param profileDeceleration the limit of the deceleration.
pmic 29:335fb9b01ca7 142 */
pmic 29:335fb9b01ca7 143 void Motion::setProfileDeceleration(float profileDeceleration) {
pmic 29:335fb9b01ca7 144
pmic 29:335fb9b01ca7 145 if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 146 }
pmic 29:335fb9b01ca7 147
pmic 29:335fb9b01ca7 148 /**
pmic 29:335fb9b01ca7 149 * Sets the limits for velocity, acceleration and deceleration values.
pmic 29:335fb9b01ca7 150 * @param profileVelocity the limit of the velocity.
pmic 29:335fb9b01ca7 151 * @param profileAcceleration the limit of the acceleration.
pmic 29:335fb9b01ca7 152 * @param profileDeceleration the limit of the deceleration.
pmic 29:335fb9b01ca7 153 */
pmic 29:335fb9b01ca7 154 void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
pmic 29:335fb9b01ca7 155
pmic 29:335fb9b01ca7 156 if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 157 if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 158 if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
pmic 29:335fb9b01ca7 159 }
pmic 29:335fb9b01ca7 160
pmic 29:335fb9b01ca7 161 /**
pmic 29:335fb9b01ca7 162 * Gets the time needed to move to a given target position.
pmic 29:335fb9b01ca7 163 * @param targetPosition the desired target position given in [m] or [rad].
pmic 29:335fb9b01ca7 164 * @return the time to move to the target position, given in [s].
pmic 29:335fb9b01ca7 165 */
pmic 29:335fb9b01ca7 166 float Motion::getTimeToPosition(double targetPosition) {
pmic 29:335fb9b01ca7 167
pmic 29:335fb9b01ca7 168 // calculate position, when velocity is reduced to zero
pmic 29:335fb9b01ca7 169
pmic 29:335fb9b01ca7 170 double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
pmic 29:335fb9b01ca7 171
pmic 29:335fb9b01ca7 172 if (targetPosition > stopPosition) { // positive velocity required
pmic 29:335fb9b01ca7 173
pmic 29:335fb9b01ca7 174 if (velocity > profileVelocity) { // slow down to profile velocity first
pmic 29:335fb9b01ca7 175
pmic 29:335fb9b01ca7 176 float t1 = (velocity-profileVelocity)/profileDeceleration;
pmic 29:335fb9b01ca7 177 float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
pmic 29:335fb9b01ca7 178 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 179
pmic 29:335fb9b01ca7 180 return t1+t2+t3;
pmic 29:335fb9b01ca7 181
pmic 29:335fb9b01ca7 182 } else if (velocity > 0.0f) { // speed up to profile velocity
pmic 29:335fb9b01ca7 183
pmic 29:335fb9b01ca7 184 float t1 = (profileVelocity-velocity)/profileAcceleration;
pmic 29:335fb9b01ca7 185 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 186 float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
pmic 29:335fb9b01ca7 187
pmic 29:335fb9b01ca7 188 if (t2 < 0.0f) {
pmic 29:335fb9b01ca7 189 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 190 t1 = (maxVelocity-velocity)/profileAcceleration;
pmic 29:335fb9b01ca7 191 t2 = 0.0f;
pmic 29:335fb9b01ca7 192 t3 = maxVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 193 }
pmic 29:335fb9b01ca7 194
pmic 29:335fb9b01ca7 195 return t1+t2+t3;
pmic 29:335fb9b01ca7 196
pmic 29:335fb9b01ca7 197 } else { // slow down to zero first, and then speed up to profile velocity
pmic 29:335fb9b01ca7 198
pmic 29:335fb9b01ca7 199 float t1 = -velocity/profileDeceleration;
pmic 29:335fb9b01ca7 200 float t2 = profileVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 201 float t4 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 202 float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
pmic 29:335fb9b01ca7 203
pmic 29:335fb9b01ca7 204 if (t3 < 0.0f) {
pmic 29:335fb9b01ca7 205 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 206 t2 = maxVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 207 t3 = 0.0f;
pmic 29:335fb9b01ca7 208 t4 = maxVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 209 }
pmic 29:335fb9b01ca7 210
pmic 29:335fb9b01ca7 211 return t1+t2+t3+t4;
pmic 29:335fb9b01ca7 212 }
pmic 29:335fb9b01ca7 213
pmic 29:335fb9b01ca7 214 } else { // negative velocity required
pmic 29:335fb9b01ca7 215
pmic 29:335fb9b01ca7 216 if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
pmic 29:335fb9b01ca7 217
pmic 29:335fb9b01ca7 218 float t1 = (-profileVelocity-velocity)/profileDeceleration;
pmic 29:335fb9b01ca7 219 float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
pmic 29:335fb9b01ca7 220 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 221
pmic 29:335fb9b01ca7 222 return t1+t2+t3;
pmic 29:335fb9b01ca7 223
pmic 29:335fb9b01ca7 224 } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
pmic 29:335fb9b01ca7 225
pmic 29:335fb9b01ca7 226 float t1 = (velocity+profileVelocity)/profileAcceleration;
pmic 29:335fb9b01ca7 227 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 228 float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
pmic 29:335fb9b01ca7 229
pmic 29:335fb9b01ca7 230 if (t2 < 0.0f) {
pmic 29:335fb9b01ca7 231 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 232 t1 = (velocity-minVelocity)/profileAcceleration;
pmic 29:335fb9b01ca7 233 t2 = 0.0f;
pmic 29:335fb9b01ca7 234 t3 = -minVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 235 }
pmic 29:335fb9b01ca7 236
pmic 29:335fb9b01ca7 237 return t1+t2+t3;
pmic 29:335fb9b01ca7 238
pmic 29:335fb9b01ca7 239 } else { // slow down to zero first, and then speed up to (negative) profile velocity
pmic 29:335fb9b01ca7 240
pmic 29:335fb9b01ca7 241 float t1 = velocity/profileDeceleration;
pmic 29:335fb9b01ca7 242 float t2 = profileVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 243 float t4 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 244 float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
pmic 29:335fb9b01ca7 245
pmic 29:335fb9b01ca7 246 if (t3 < 0.0f) {
pmic 29:335fb9b01ca7 247 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 248 t2 = -minVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 249 t3 = 0.0f;
pmic 29:335fb9b01ca7 250 t4 = -minVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 251 }
pmic 29:335fb9b01ca7 252
pmic 29:335fb9b01ca7 253 return t1+t2+t3+t4;
pmic 29:335fb9b01ca7 254 }
pmic 29:335fb9b01ca7 255 }
pmic 29:335fb9b01ca7 256 }
pmic 29:335fb9b01ca7 257
pmic 29:335fb9b01ca7 258 /**
pmic 29:335fb9b01ca7 259 * Increments the current motion towards a given target velocity.
pmic 29:335fb9b01ca7 260 * @param targetVelocity the desired target velocity given in [m/s] or [rad/s].
pmic 29:335fb9b01ca7 261 * @param period the time period to increment the motion values for, given in [s].
pmic 29:335fb9b01ca7 262 */
pmic 29:335fb9b01ca7 263 void Motion::incrementToVelocity(float targetVelocity, float period) {
pmic 29:335fb9b01ca7 264
pmic 29:335fb9b01ca7 265 if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
pmic 29:335fb9b01ca7 266 else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
pmic 29:335fb9b01ca7 267
pmic 29:335fb9b01ca7 268 if (targetVelocity > 0.0f) {
pmic 29:335fb9b01ca7 269
pmic 29:335fb9b01ca7 270 if (velocity > targetVelocity) { // slow down to target velocity
pmic 29:335fb9b01ca7 271
pmic 29:335fb9b01ca7 272 float t1 = (velocity-targetVelocity)/profileDeceleration;
pmic 29:335fb9b01ca7 273
pmic 29:335fb9b01ca7 274 if (t1 > period) {
pmic 29:335fb9b01ca7 275 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 276 velocity += -profileDeceleration*period;
pmic 29:335fb9b01ca7 277 } else {
pmic 29:335fb9b01ca7 278 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 279 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 280 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 281 }
pmic 29:335fb9b01ca7 282
pmic 29:335fb9b01ca7 283 } else if (velocity > 0.0f) { // speed up to target velocity
pmic 29:335fb9b01ca7 284
pmic 29:335fb9b01ca7 285 float t1 = (targetVelocity-velocity)/profileAcceleration;
pmic 29:335fb9b01ca7 286
pmic 29:335fb9b01ca7 287 if (t1 > period) {
pmic 29:335fb9b01ca7 288 position += (double)((velocity+profileAcceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 289 velocity += profileAcceleration*period;
pmic 29:335fb9b01ca7 290 } else {
pmic 29:335fb9b01ca7 291 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 292 velocity += profileAcceleration*t1;
pmic 29:335fb9b01ca7 293 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 294 }
pmic 29:335fb9b01ca7 295
pmic 29:335fb9b01ca7 296 } else { // slow down to zero first, and then speed up to target velocity
pmic 29:335fb9b01ca7 297
pmic 29:335fb9b01ca7 298 float t1 = -velocity/profileDeceleration;
pmic 29:335fb9b01ca7 299 float t2 = targetVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 300
pmic 29:335fb9b01ca7 301 if (t1 > period) {
pmic 29:335fb9b01ca7 302 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 303 velocity += profileDeceleration*period;
pmic 29:335fb9b01ca7 304 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 305 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 306 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 307 position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
pmic 29:335fb9b01ca7 308 velocity += profileAcceleration*(period-t1);
pmic 29:335fb9b01ca7 309 } else {
pmic 29:335fb9b01ca7 310 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 311 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 312 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 313 velocity += profileAcceleration*t2;
pmic 29:335fb9b01ca7 314 position += (double)(velocity*(period-t1-t2));
pmic 29:335fb9b01ca7 315 }
pmic 29:335fb9b01ca7 316 }
pmic 29:335fb9b01ca7 317
pmic 29:335fb9b01ca7 318 } else {
pmic 29:335fb9b01ca7 319
pmic 29:335fb9b01ca7 320 if (velocity < targetVelocity) { // slow down to (negative) target velocity
pmic 29:335fb9b01ca7 321
pmic 29:335fb9b01ca7 322 float t1 = (targetVelocity-velocity)/profileDeceleration;
pmic 29:335fb9b01ca7 323
pmic 29:335fb9b01ca7 324 if (t1 > period) {
pmic 29:335fb9b01ca7 325 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 326 velocity += profileDeceleration*period;
pmic 29:335fb9b01ca7 327 } else {
pmic 29:335fb9b01ca7 328 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 329 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 330 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 331 }
pmic 29:335fb9b01ca7 332
pmic 29:335fb9b01ca7 333 } else if (velocity < 0.0f) { // speed up to (negative) target velocity
pmic 29:335fb9b01ca7 334
pmic 29:335fb9b01ca7 335 float t1 = (velocity-targetVelocity)/profileAcceleration;
pmic 29:335fb9b01ca7 336
pmic 29:335fb9b01ca7 337 if (t1 > period) {
pmic 29:335fb9b01ca7 338 position += (double)((velocity-profileAcceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 339 velocity += -profileAcceleration*period;
pmic 29:335fb9b01ca7 340 } else {
pmic 29:335fb9b01ca7 341 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 342 velocity += -profileAcceleration*t1;
pmic 29:335fb9b01ca7 343 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 344 }
pmic 29:335fb9b01ca7 345
pmic 29:335fb9b01ca7 346 } else { // slow down to zero first, and then speed up to (negative) target velocity
pmic 29:335fb9b01ca7 347
pmic 29:335fb9b01ca7 348 float t1 = velocity/profileDeceleration;
pmic 29:335fb9b01ca7 349 float t2 = -targetVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 350
pmic 29:335fb9b01ca7 351 if (t1 > period) {
pmic 29:335fb9b01ca7 352 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 353 velocity += -profileDeceleration*period;
pmic 29:335fb9b01ca7 354 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 355 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 356 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 357 position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
pmic 29:335fb9b01ca7 358 velocity += -profileAcceleration*(period-t1);
pmic 29:335fb9b01ca7 359 } else {
pmic 29:335fb9b01ca7 360 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 361 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 362 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 363 velocity += -profileAcceleration*t2;
pmic 29:335fb9b01ca7 364 position += (double)(velocity*(period-t1-t2));
pmic 29:335fb9b01ca7 365 }
pmic 29:335fb9b01ca7 366 }
pmic 29:335fb9b01ca7 367 }
pmic 29:335fb9b01ca7 368 }
pmic 29:335fb9b01ca7 369
pmic 29:335fb9b01ca7 370 /**
pmic 29:335fb9b01ca7 371 * Increments the current motion towards a given target position.
pmic 29:335fb9b01ca7 372 * @param targetPosition the desired target position given in [m] or [rad].
pmic 29:335fb9b01ca7 373 * @param period the time period to increment the motion values for, given in [s].
pmic 29:335fb9b01ca7 374 */
pmic 29:335fb9b01ca7 375 void Motion::incrementToPosition(double targetPosition, float period) {
pmic 29:335fb9b01ca7 376
pmic 29:335fb9b01ca7 377 // calculate position, when velocity is reduced to zero
pmic 29:335fb9b01ca7 378
pmic 29:335fb9b01ca7 379 double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
pmic 29:335fb9b01ca7 380
pmic 29:335fb9b01ca7 381 if (targetPosition > stopPosition) { // positive velocity required
pmic 29:335fb9b01ca7 382
pmic 29:335fb9b01ca7 383 if (velocity > profileVelocity) { // slow down to profile velocity first
pmic 29:335fb9b01ca7 384
pmic 29:335fb9b01ca7 385 float t1 = (velocity-profileVelocity)/profileDeceleration;
pmic 29:335fb9b01ca7 386 float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
pmic 29:335fb9b01ca7 387 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 388
pmic 29:335fb9b01ca7 389 if (t1 > period) {
pmic 29:335fb9b01ca7 390 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 391 velocity += -profileDeceleration*period;
pmic 29:335fb9b01ca7 392 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 393 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 394 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 395 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 396 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 397 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 398 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 399 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 400 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
pmic 29:335fb9b01ca7 401 velocity += -profileDeceleration*(period-t1-t2);
pmic 29:335fb9b01ca7 402 } else {
pmic 29:335fb9b01ca7 403 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 404 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 405 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 406 position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
pmic 29:335fb9b01ca7 407 velocity += -profileDeceleration*t3;
pmic 29:335fb9b01ca7 408 }
pmic 29:335fb9b01ca7 409
pmic 29:335fb9b01ca7 410 } else if (velocity > 0.0f) { // speed up to profile velocity
pmic 29:335fb9b01ca7 411
pmic 29:335fb9b01ca7 412 float t1 = (profileVelocity-velocity)/profileAcceleration;
pmic 29:335fb9b01ca7 413 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 414 float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
pmic 29:335fb9b01ca7 415
pmic 29:335fb9b01ca7 416 if (t2 < 0.0f) {
pmic 29:335fb9b01ca7 417 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 418 t1 = (maxVelocity-velocity)/profileAcceleration;
pmic 29:335fb9b01ca7 419 t2 = 0.0f;
pmic 29:335fb9b01ca7 420 t3 = maxVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 421 }
pmic 29:335fb9b01ca7 422
pmic 29:335fb9b01ca7 423 if (t1 > period) {
pmic 29:335fb9b01ca7 424 position += (double)((velocity+profileAcceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 425 velocity += profileAcceleration*period;
pmic 29:335fb9b01ca7 426 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 427 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 428 velocity += profileAcceleration*t1;
pmic 29:335fb9b01ca7 429 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 430 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 431 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 432 velocity += profileAcceleration*t1;
pmic 29:335fb9b01ca7 433 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 434 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
pmic 29:335fb9b01ca7 435 velocity += -profileDeceleration*(period-t1-t2);
pmic 29:335fb9b01ca7 436 } else {
pmic 29:335fb9b01ca7 437 position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 438 velocity += profileAcceleration*t1;
pmic 29:335fb9b01ca7 439 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 440 position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
pmic 29:335fb9b01ca7 441 velocity += -profileDeceleration*t3;
pmic 29:335fb9b01ca7 442 }
pmic 29:335fb9b01ca7 443
pmic 29:335fb9b01ca7 444 } else { // slow down to zero first, and then speed up to profile velocity
pmic 29:335fb9b01ca7 445
pmic 29:335fb9b01ca7 446 float t1 = -velocity/profileDeceleration;
pmic 29:335fb9b01ca7 447 float t2 = profileVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 448 float t4 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 449 float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
pmic 29:335fb9b01ca7 450
pmic 29:335fb9b01ca7 451 if (t3 < 0.0f) {
pmic 29:335fb9b01ca7 452 float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 453 t2 = maxVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 454 t3 = 0.0f;
pmic 29:335fb9b01ca7 455 t4 = maxVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 456 }
pmic 29:335fb9b01ca7 457
pmic 29:335fb9b01ca7 458 if (t1 > period) {
pmic 29:335fb9b01ca7 459 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 460 velocity += profileDeceleration*period;
pmic 29:335fb9b01ca7 461 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 462 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 463 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 464 position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
pmic 29:335fb9b01ca7 465 velocity += profileAcceleration*(period-t1);
pmic 29:335fb9b01ca7 466 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 467 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 468 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 469 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 470 velocity += profileAcceleration*t2;
pmic 29:335fb9b01ca7 471 position += (double)(velocity*(period-t1-t2));
pmic 29:335fb9b01ca7 472 } else if (t1+t2+t3+t4 > period) {
pmic 29:335fb9b01ca7 473 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 474 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 475 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 476 velocity += profileAcceleration*t2;
pmic 29:335fb9b01ca7 477 position += (double)(velocity*t3);
pmic 29:335fb9b01ca7 478 position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
pmic 29:335fb9b01ca7 479 velocity += -profileDeceleration*(period-t1-t2-t3);
pmic 29:335fb9b01ca7 480 } else {
pmic 29:335fb9b01ca7 481 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 482 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 483 position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 484 velocity += profileAcceleration*t2;
pmic 29:335fb9b01ca7 485 position += (double)(velocity*t3);
pmic 29:335fb9b01ca7 486 position += (double)((velocity-profileDeceleration*0.5f*t4)*t4);
pmic 29:335fb9b01ca7 487 velocity += -profileDeceleration*t4;
pmic 29:335fb9b01ca7 488 }
pmic 29:335fb9b01ca7 489 }
pmic 29:335fb9b01ca7 490
pmic 29:335fb9b01ca7 491 } else { // negative velocity required
pmic 29:335fb9b01ca7 492
pmic 29:335fb9b01ca7 493 if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
pmic 29:335fb9b01ca7 494
pmic 29:335fb9b01ca7 495 float t1 = (-profileVelocity-velocity)/profileDeceleration;
pmic 29:335fb9b01ca7 496 float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
pmic 29:335fb9b01ca7 497 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 498
pmic 29:335fb9b01ca7 499 if (t1 > period) {
pmic 29:335fb9b01ca7 500 position += (double)((velocity+profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 501 velocity += profileDeceleration*period;
pmic 29:335fb9b01ca7 502 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 503 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 504 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 505 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 506 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 507 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 508 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 509 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 510 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
pmic 29:335fb9b01ca7 511 velocity += profileDeceleration*(period-t1-t2);
pmic 29:335fb9b01ca7 512 } else {
pmic 29:335fb9b01ca7 513 position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 514 velocity += profileDeceleration*t1;
pmic 29:335fb9b01ca7 515 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 516 position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
pmic 29:335fb9b01ca7 517 velocity += profileDeceleration*t3;
pmic 29:335fb9b01ca7 518 }
pmic 29:335fb9b01ca7 519
pmic 29:335fb9b01ca7 520 } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
pmic 29:335fb9b01ca7 521
pmic 29:335fb9b01ca7 522 float t1 = (velocity+profileVelocity)/profileAcceleration;
pmic 29:335fb9b01ca7 523 float t3 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 524 float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
pmic 29:335fb9b01ca7 525
pmic 29:335fb9b01ca7 526 if (t2 < 0.0f) {
pmic 29:335fb9b01ca7 527 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 528 t1 = (velocity-minVelocity)/profileAcceleration;
pmic 29:335fb9b01ca7 529 t2 = 0.0f;
pmic 29:335fb9b01ca7 530 t3 = -minVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 531 }
pmic 29:335fb9b01ca7 532
pmic 29:335fb9b01ca7 533 if (t1 > period) {
pmic 29:335fb9b01ca7 534 position += (double)((velocity-profileAcceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 535 velocity += -profileAcceleration*period;
pmic 29:335fb9b01ca7 536 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 537 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 538 velocity += -profileAcceleration*t1;
pmic 29:335fb9b01ca7 539 position += (double)(velocity*(period-t1));
pmic 29:335fb9b01ca7 540 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 541 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 542 velocity += -profileAcceleration*t1;
pmic 29:335fb9b01ca7 543 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 544 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
pmic 29:335fb9b01ca7 545 velocity += profileDeceleration*(period-t1-t2);
pmic 29:335fb9b01ca7 546 } else {
pmic 29:335fb9b01ca7 547 position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 548 velocity += -profileAcceleration*t1;
pmic 29:335fb9b01ca7 549 position += (double)(velocity*t2);
pmic 29:335fb9b01ca7 550 position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
pmic 29:335fb9b01ca7 551 velocity += profileDeceleration*t3;
pmic 29:335fb9b01ca7 552 }
pmic 29:335fb9b01ca7 553
pmic 29:335fb9b01ca7 554 } else { // slow down to zero first, and then speed up to (negative) profile velocity
pmic 29:335fb9b01ca7 555
pmic 29:335fb9b01ca7 556 float t1 = velocity/profileDeceleration;
pmic 29:335fb9b01ca7 557 float t2 = profileVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 558 float t4 = profileVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 559 float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
pmic 29:335fb9b01ca7 560
pmic 29:335fb9b01ca7 561 if (t3 < 0.0f) {
pmic 29:335fb9b01ca7 562 float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
pmic 29:335fb9b01ca7 563 t2 = -minVelocity/profileAcceleration;
pmic 29:335fb9b01ca7 564 t3 = 0.0f;
pmic 29:335fb9b01ca7 565 t4 = -minVelocity/profileDeceleration;
pmic 29:335fb9b01ca7 566 }
pmic 29:335fb9b01ca7 567
pmic 29:335fb9b01ca7 568 if (t1 > period) {
pmic 29:335fb9b01ca7 569 position += (double)((velocity-profileDeceleration*0.5f*period)*period);
pmic 29:335fb9b01ca7 570 velocity += -profileDeceleration*period;
pmic 29:335fb9b01ca7 571 } else if (t1+t2 > period) {
pmic 29:335fb9b01ca7 572 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 573 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 574 position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
pmic 29:335fb9b01ca7 575 velocity += -profileAcceleration*(period-t1);
pmic 29:335fb9b01ca7 576 } else if (t1+t2+t3 > period) {
pmic 29:335fb9b01ca7 577 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 578 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 579 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 580 velocity += -profileAcceleration*t2;
pmic 29:335fb9b01ca7 581 position += (double)(velocity*(period-t1-t2));
pmic 29:335fb9b01ca7 582 } else if (t1+t2+t3+t4 > period) {
pmic 29:335fb9b01ca7 583 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 584 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 585 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 586 velocity += -profileAcceleration*t2;
pmic 29:335fb9b01ca7 587 position += (double)(velocity*t3);
pmic 29:335fb9b01ca7 588 position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
pmic 29:335fb9b01ca7 589 velocity += profileDeceleration*(period-t1-t2-t3);
pmic 29:335fb9b01ca7 590 } else {
pmic 29:335fb9b01ca7 591 position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
pmic 29:335fb9b01ca7 592 velocity += -profileDeceleration*t1;
pmic 29:335fb9b01ca7 593 position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
pmic 29:335fb9b01ca7 594 velocity += -profileAcceleration*t2;
pmic 29:335fb9b01ca7 595 position += (double)(velocity*t3);
pmic 29:335fb9b01ca7 596 position += (double)((velocity+profileDeceleration*0.5f*t4)*t4);
pmic 29:335fb9b01ca7 597 velocity += profileDeceleration*t4;
pmic 29:335fb9b01ca7 598 }
pmic 29:335fb9b01ca7 599 }
pmic 29:335fb9b01ca7 600 }
pmic 29:335fb9b01ca7 601 }
pmic 29:335fb9b01ca7 602