Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

Motion.cpp@0:e360940c4b88, 2018-03-16 (annotated)

Committer:
Appalco
Date:
Fri Mar 16 13:32:47 2018 +0000
Revision:
0:e360940c4b88
import

Who changed what in which revision?

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