Nicolas Borla / Mbed OS BBR_1Ebene

BBR 1 Ebene

Motion.cpp@0:fbdae7e6d805, 2018-05-14 (annotated)

Committer:
borlanic
Date:
Mon May 14 11:29:06 2018 +0000
Revision:
0:fbdae7e6d805
BBR

Who changed what in which revision?

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