Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

Motion.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

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