ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

Motion.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

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