TeamSurface / Library

P2 halbfertig

Fork of Library by St Knz

Motion.cpp@0:bb408887ab78, 2018-03-09 (annotated)

Committer:
kueenste
Date:
Fri Mar 09 15:29:36 2018 +0000
Revision:
0:bb408887ab78
P2_unfertig;

Who changed what in which revision?

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