Marco Oehler / Mbed 2 deprecated Lab2

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:1a972ed770da, 2020-03-09 (annotated)

Committer:
oehlemar
Date:
Mon Mar 09 16:23:04 2020 +0000
Revision:
0:1a972ed770da
LAB2

Who changed what in which revision?

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