Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

Motion.cpp@41:7484471403aa, 2022-05-11 (annotated)

Committer:
pmic
Date:
Wed May 11 09:44:25 2022 +0200
Revision:
41:7484471403aa
Parent:
37:698d6b73b50c 
Delete StateMachine

Who changed what in which revision?

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