ROME_Praktikum / Mbed 2 deprecated Rome_P_3

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:   mbed

Motion.cpp@0:20ec9d702676, 2020-03-31 (annotated)

Committer:
Jacqueline
Date:
Tue Mar 31 11:58:30 2020 +0000
Revision:
0:20ec9d702676
Praktikum_3

Who changed what in which revision?

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