Marco Oehler / Mbed OS Lab3

ROME2 Lab3

Motion.cpp@2:fc9e2aebf9d5, 2020-03-25 (annotated)

Committer:
oehlemar
Date:
Wed Mar 25 14:15:52 2020 +0000
Revision:
2:fc9e2aebf9d5
Parent:
0:6a4d3264c067 
final

Who changed what in which revision?

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