altb_pmic
/
Test_Realbot
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: mbed
Motion.cpp@4:52d2d31a7347, 2019-12-05 (annotated)
- Committer:
- pmic
- Date:
- Thu Dec 05 09:09:31 2019 +0000
- Revision:
- 4:52d2d31a7347
- Parent:
- 3:e6d345973797
Use original (not adjusted) Motion class from Marcel -> correct results for getTimeToPosition (no problem with complex numbers??? <- we dont care right know and just accept it).
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| pmic | 3:e6d345973797 | 1 | /* |
| pmic | 3:e6d345973797 | 2 | * Motion.cpp |
| pmic | 3:e6d345973797 | 3 | * Copyright (c) 2017, ZHAW |
| pmic | 3:e6d345973797 | 4 | * All rights reserved. |
| pmic | 3:e6d345973797 | 5 | * |
| pmic | 3:e6d345973797 | 6 | * Created on: 02.02.2017 |
| pmic | 3:e6d345973797 | 7 | * Author: Marcel Honegger |
| pmic | 3:e6d345973797 | 8 | */ |
| pmic | 3:e6d345973797 | 9 | |
| pmic | 3:e6d345973797 | 10 | #include <cmath> |
| pmic | 3:e6d345973797 | 11 | #include <algorithm> |
| pmic | 3:e6d345973797 | 12 | #include "Motion.h" |
| pmic | 3:e6d345973797 | 13 | |
| pmic | 4:52d2d31a7347 | 14 | using namespace std; |
| pmic | 4:52d2d31a7347 | 15 | |
| pmic | 3:e6d345973797 | 16 | const float Motion::DEFAULT_LIMIT = 1.0f; // default value for limits |
| pmic | 3:e6d345973797 | 17 | const float Motion::MINIMUM_LIMIT = 1.0e-6f; // smallest value allowed for limits |
| pmic | 3:e6d345973797 | 18 | |
| pmic | 3:e6d345973797 | 19 | /** |
| pmic | 3:e6d345973797 | 20 | * Creates a <code>Motion</code> object. |
| pmic | 3:e6d345973797 | 21 | * The values for position, velocity and acceleration are set to 0. |
| pmic | 3:e6d345973797 | 22 | */ |
| pmic | 3:e6d345973797 | 23 | Motion::Motion() { |
| pmic | 4:52d2d31a7347 | 24 | |
| pmic | 4:52d2d31a7347 | 25 | position = 0.0; |
| pmic | 4:52d2d31a7347 | 26 | velocity = 0.0f; |
| pmic | 4:52d2d31a7347 | 27 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 28 | |
| pmic | 4:52d2d31a7347 | 29 | profileVelocity = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 30 | profileAcceleration = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 31 | profileDeceleration = DEFAULT_LIMIT; |
| pmic | 3:e6d345973797 | 32 | } |
| pmic | 3:e6d345973797 | 33 | |
| pmic | 3:e6d345973797 | 34 | /** |
| pmic | 3:e6d345973797 | 35 | * Creates a <code>Motion</code> object with given values for position and velocity. |
| pmic | 3:e6d345973797 | 36 | * @param position the initial position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 37 | * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 38 | */ |
| pmic | 3:e6d345973797 | 39 | Motion::Motion(double position, float velocity) { |
| pmic | 4:52d2d31a7347 | 40 | |
| pmic | 4:52d2d31a7347 | 41 | this->position = position; |
| pmic | 4:52d2d31a7347 | 42 | this->velocity = velocity; |
| pmic | 4:52d2d31a7347 | 43 | this->acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 44 | |
| pmic | 4:52d2d31a7347 | 45 | profileVelocity = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 46 | profileAcceleration = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 47 | profileDeceleration = DEFAULT_LIMIT; |
| pmic | 3:e6d345973797 | 48 | } |
| pmic | 3:e6d345973797 | 49 | |
| pmic | 3:e6d345973797 | 50 | /** |
| pmic | 3:e6d345973797 | 51 | * Creates a <code>Motion</code> object with given values for position, velocity and acceleration. |
| pmic | 3:e6d345973797 | 52 | * @param position the initial position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 53 | * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 54 | * @param acceleration the initial acceleration value of this motion, given in [m/s²] or [rad/s²]. |
| pmic | 3:e6d345973797 | 55 | */ |
| pmic | 3:e6d345973797 | 56 | Motion::Motion(double position, float velocity, float acceleration) { |
| pmic | 4:52d2d31a7347 | 57 | |
| pmic | 4:52d2d31a7347 | 58 | this->position = position; |
| pmic | 4:52d2d31a7347 | 59 | this->velocity = velocity; |
| pmic | 4:52d2d31a7347 | 60 | this->acceleration = acceleration; |
| pmic | 4:52d2d31a7347 | 61 | |
| pmic | 4:52d2d31a7347 | 62 | profileVelocity = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 63 | profileAcceleration = DEFAULT_LIMIT; |
| pmic | 4:52d2d31a7347 | 64 | profileDeceleration = DEFAULT_LIMIT; |
| pmic | 3:e6d345973797 | 65 | } |
| pmic | 3:e6d345973797 | 66 | |
| pmic | 3:e6d345973797 | 67 | /** |
| pmic | 3:e6d345973797 | 68 | * Creates a <code>Motion</code> object with given values for position, velocity and acceleration. |
| pmic | 3:e6d345973797 | 69 | * @param position the initial position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 70 | * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 71 | * @param profileVelocity the limit of the velocity. |
| pmic | 3:e6d345973797 | 72 | * @param profileAcceleration the limit of the acceleration. |
| pmic | 3:e6d345973797 | 73 | * @param profileDeceleration equal to profileAcceleration. |
| pmic | 3:e6d345973797 | 74 | */ |
| pmic | 3:e6d345973797 | 75 | Motion::Motion(double position, float velocity, float profileVelocity, float profileAcceleration) { |
| pmic | 3:e6d345973797 | 76 | |
| pmic | 3:e6d345973797 | 77 | this->position = position; |
| pmic | 3:e6d345973797 | 78 | this->velocity = velocity; |
| pmic | 3:e6d345973797 | 79 | this->acceleration = 0.0f; |
| pmic | 3:e6d345973797 | 80 | |
| pmic | 3:e6d345973797 | 81 | this->profileVelocity = profileVelocity; |
| pmic | 3:e6d345973797 | 82 | this->profileAcceleration = profileAcceleration; |
| pmic | 3:e6d345973797 | 83 | this->profileDeceleration = profileAcceleration; |
| pmic | 3:e6d345973797 | 84 | } |
| pmic | 3:e6d345973797 | 85 | |
| pmic | 3:e6d345973797 | 86 | /** |
| pmic | 3:e6d345973797 | 87 | * Creates a <code>Motion</code> object with given values for position and velocity. |
| pmic | 3:e6d345973797 | 88 | * @param motion another <code>Motion</code> object to copy the values from. |
| pmic | 3:e6d345973797 | 89 | */ |
| pmic | 3:e6d345973797 | 90 | Motion::Motion(const Motion& motion) { |
| pmic | 4:52d2d31a7347 | 91 | |
| pmic | 4:52d2d31a7347 | 92 | position = motion.position; |
| pmic | 4:52d2d31a7347 | 93 | velocity = motion.velocity; |
| pmic | 4:52d2d31a7347 | 94 | acceleration = motion.acceleration; |
| pmic | 4:52d2d31a7347 | 95 | |
| pmic | 4:52d2d31a7347 | 96 | profileVelocity = motion.profileVelocity; |
| pmic | 4:52d2d31a7347 | 97 | profileAcceleration = motion.profileAcceleration; |
| pmic | 4:52d2d31a7347 | 98 | profileDeceleration = motion.profileDeceleration; |
| pmic | 3:e6d345973797 | 99 | } |
| pmic | 3:e6d345973797 | 100 | |
| pmic | 3:e6d345973797 | 101 | Motion::~Motion() {} |
| pmic | 3:e6d345973797 | 102 | |
| pmic | 3:e6d345973797 | 103 | /** |
| pmic | 3:e6d345973797 | 104 | * Sets the values for position and velocity. |
| pmic | 3:e6d345973797 | 105 | * @param position the desired position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 106 | * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 107 | */ |
| pmic | 3:e6d345973797 | 108 | void Motion::set(double position, float velocity) { |
| pmic | 4:52d2d31a7347 | 109 | |
| pmic | 4:52d2d31a7347 | 110 | this->position = position; |
| pmic | 4:52d2d31a7347 | 111 | this->velocity = velocity; |
| pmic | 3:e6d345973797 | 112 | } |
| pmic | 3:e6d345973797 | 113 | |
| pmic | 3:e6d345973797 | 114 | /** |
| pmic | 3:e6d345973797 | 115 | * Sets the values for position, velocity and acceleration. |
| pmic | 3:e6d345973797 | 116 | * @param position the desired position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 117 | * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 118 | * @param acceleration the desired acceleration value of this motion, given in [m/s²] or [rad/s²]. |
| pmic | 3:e6d345973797 | 119 | */ |
| pmic | 3:e6d345973797 | 120 | void Motion::set(double position, float velocity, float acceleration) { |
| pmic | 4:52d2d31a7347 | 121 | |
| pmic | 4:52d2d31a7347 | 122 | this->position = position; |
| pmic | 4:52d2d31a7347 | 123 | this->velocity = velocity; |
| pmic | 4:52d2d31a7347 | 124 | this->acceleration = acceleration; |
| pmic | 3:e6d345973797 | 125 | } |
| pmic | 3:e6d345973797 | 126 | |
| pmic | 3:e6d345973797 | 127 | /** |
| pmic | 3:e6d345973797 | 128 | * Sets the values for position, velocity and acceleration. |
| pmic | 3:e6d345973797 | 129 | * @param motion another <code>Motion</code> object to copy the values from. |
| pmic | 3:e6d345973797 | 130 | */ |
| pmic | 3:e6d345973797 | 131 | void Motion::set(const Motion& motion) { |
| pmic | 4:52d2d31a7347 | 132 | |
| pmic | 4:52d2d31a7347 | 133 | position = motion.position; |
| pmic | 4:52d2d31a7347 | 134 | velocity = motion.velocity; |
| pmic | 4:52d2d31a7347 | 135 | acceleration = motion.acceleration; |
| pmic | 3:e6d345973797 | 136 | } |
| pmic | 3:e6d345973797 | 137 | |
| pmic | 3:e6d345973797 | 138 | /** |
| pmic | 3:e6d345973797 | 139 | * Sets the position value. |
| pmic | 3:e6d345973797 | 140 | * @param position the desired position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 141 | */ |
| pmic | 3:e6d345973797 | 142 | void Motion::setPosition(double position) { |
| pmic | 4:52d2d31a7347 | 143 | |
| pmic | 4:52d2d31a7347 | 144 | this->position = position; |
| pmic | 3:e6d345973797 | 145 | } |
| pmic | 3:e6d345973797 | 146 | |
| pmic | 3:e6d345973797 | 147 | /** |
| pmic | 3:e6d345973797 | 148 | * Gets the position value. |
| pmic | 3:e6d345973797 | 149 | * @return the position value of this motion, given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 150 | */ |
| pmic | 3:e6d345973797 | 151 | double Motion::getPosition() { |
| pmic | 4:52d2d31a7347 | 152 | |
| pmic | 4:52d2d31a7347 | 153 | return position; |
| pmic | 3:e6d345973797 | 154 | } |
| pmic | 3:e6d345973797 | 155 | |
| pmic | 3:e6d345973797 | 156 | /** |
| pmic | 3:e6d345973797 | 157 | * Sets the velocity value. |
| pmic | 3:e6d345973797 | 158 | * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 159 | */ |
| pmic | 3:e6d345973797 | 160 | void Motion::setVelocity(float velocity) { |
| pmic | 4:52d2d31a7347 | 161 | |
| pmic | 4:52d2d31a7347 | 162 | this->velocity = velocity; |
| pmic | 3:e6d345973797 | 163 | } |
| pmic | 3:e6d345973797 | 164 | |
| pmic | 3:e6d345973797 | 165 | /** |
| pmic | 3:e6d345973797 | 166 | * Gets the velocity value. |
| pmic | 3:e6d345973797 | 167 | * @return the velocity value of this motion, given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 168 | */ |
| pmic | 3:e6d345973797 | 169 | float Motion::getVelocity() { |
| pmic | 4:52d2d31a7347 | 170 | |
| pmic | 4:52d2d31a7347 | 171 | return velocity; |
| pmic | 3:e6d345973797 | 172 | } |
| pmic | 3:e6d345973797 | 173 | |
| pmic | 3:e6d345973797 | 174 | /** |
| pmic | 3:e6d345973797 | 175 | * Sets the acceleration value. |
| pmic | 3:e6d345973797 | 176 | * @param acceleration the desired acceleration value of this motion, given in [m/s²] or [rad/s²]. |
| pmic | 3:e6d345973797 | 177 | */ |
| pmic | 3:e6d345973797 | 178 | void Motion::setAcceleration(float acceleration) { |
| pmic | 4:52d2d31a7347 | 179 | |
| pmic | 4:52d2d31a7347 | 180 | this->acceleration = acceleration; |
| pmic | 3:e6d345973797 | 181 | } |
| pmic | 3:e6d345973797 | 182 | |
| pmic | 3:e6d345973797 | 183 | /** |
| pmic | 3:e6d345973797 | 184 | * Gets the acceleration value. |
| pmic | 3:e6d345973797 | 185 | * @return the acceleration value of this motion, given in [m/s²] or [rad/s²]. |
| pmic | 3:e6d345973797 | 186 | */ |
| pmic | 3:e6d345973797 | 187 | float Motion::getAcceleration() { |
| pmic | 4:52d2d31a7347 | 188 | |
| pmic | 4:52d2d31a7347 | 189 | return acceleration; |
| pmic | 3:e6d345973797 | 190 | } |
| pmic | 3:e6d345973797 | 191 | |
| pmic | 3:e6d345973797 | 192 | /** |
| pmic | 3:e6d345973797 | 193 | * Sets the limit for the velocity value. |
| pmic | 3:e6d345973797 | 194 | * @param profileVelocity the limit of the velocity. |
| pmic | 3:e6d345973797 | 195 | */ |
| pmic | 3:e6d345973797 | 196 | void Motion::setProfileVelocity(float profileVelocity) { |
| pmic | 4:52d2d31a7347 | 197 | |
| pmic | 4:52d2d31a7347 | 198 | if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT; |
| pmic | 3:e6d345973797 | 199 | } |
| pmic | 3:e6d345973797 | 200 | |
| pmic | 3:e6d345973797 | 201 | /** |
| pmic | 3:e6d345973797 | 202 | * Sets the limit for the acceleration value. |
| pmic | 3:e6d345973797 | 203 | * @param profileAcceleration the limit of the acceleration. |
| pmic | 3:e6d345973797 | 204 | */ |
| pmic | 3:e6d345973797 | 205 | void Motion::setProfileAcceleration(float profileAcceleration) { |
| pmic | 4:52d2d31a7347 | 206 | |
| pmic | 4:52d2d31a7347 | 207 | if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT; |
| pmic | 3:e6d345973797 | 208 | } |
| pmic | 3:e6d345973797 | 209 | |
| pmic | 3:e6d345973797 | 210 | /** |
| pmic | 3:e6d345973797 | 211 | * Sets the limit for the deceleration value. |
| pmic | 3:e6d345973797 | 212 | * @param profileDeceleration the limit of the deceleration. |
| pmic | 3:e6d345973797 | 213 | */ |
| pmic | 3:e6d345973797 | 214 | void Motion::setProfileDeceleration(float profileDeceleration) { |
| pmic | 4:52d2d31a7347 | 215 | |
| pmic | 4:52d2d31a7347 | 216 | if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT; |
| pmic | 3:e6d345973797 | 217 | } |
| pmic | 3:e6d345973797 | 218 | |
| pmic | 3:e6d345973797 | 219 | /** |
| pmic | 3:e6d345973797 | 220 | * Sets the limits for velocity, acceleration and deceleration values. |
| pmic | 3:e6d345973797 | 221 | * @param profileVelocity the limit of the velocity. |
| pmic | 3:e6d345973797 | 222 | * @param profileAcceleration the limit of the acceleration. |
| pmic | 3:e6d345973797 | 223 | * @param profileDeceleration the limit of the deceleration. |
| pmic | 3:e6d345973797 | 224 | */ |
| pmic | 3:e6d345973797 | 225 | void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) { |
| pmic | 4:52d2d31a7347 | 226 | |
| pmic | 4:52d2d31a7347 | 227 | if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT; |
| pmic | 4:52d2d31a7347 | 228 | if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT; |
| pmic | 4:52d2d31a7347 | 229 | if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT; |
| pmic | 3:e6d345973797 | 230 | } |
| pmic | 3:e6d345973797 | 231 | |
| pmic | 3:e6d345973797 | 232 | /** |
| pmic | 3:e6d345973797 | 233 | * Gets the time needed to move to a given target position. |
| pmic | 3:e6d345973797 | 234 | * @param targetPosition the desired target position given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 235 | * @return the time to move to the target position, given in [s]. |
| pmic | 3:e6d345973797 | 236 | */ |
| pmic | 3:e6d345973797 | 237 | float Motion::getTimeToPosition(double targetPosition) { |
| pmic | 4:52d2d31a7347 | 238 | |
| pmic | 4:52d2d31a7347 | 239 | // calculate position, when velocity is reduced to zero |
| pmic | 4:52d2d31a7347 | 240 | |
| pmic | 4:52d2d31a7347 | 241 | double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f); |
| pmic | 4:52d2d31a7347 | 242 | |
| pmic | 4:52d2d31a7347 | 243 | if (targetPosition > stopPosition) { // positive velocity required |
| pmic | 4:52d2d31a7347 | 244 | |
| pmic | 4:52d2d31a7347 | 245 | if (velocity > profileVelocity) { // slow down to profile velocity first |
| pmic | 4:52d2d31a7347 | 246 | |
| pmic | 4:52d2d31a7347 | 247 | float t1 = (velocity-profileVelocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 248 | float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity; |
| pmic | 4:52d2d31a7347 | 249 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 250 | |
| pmic | 4:52d2d31a7347 | 251 | return t1+t2+t3; |
| pmic | 4:52d2d31a7347 | 252 | |
| pmic | 4:52d2d31a7347 | 253 | } else if (velocity > 0.0f) { // speed up to profile velocity |
| pmic | 4:52d2d31a7347 | 254 | |
| pmic | 4:52d2d31a7347 | 255 | float t1 = (profileVelocity-velocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 256 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 257 | float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3; |
| pmic | 4:52d2d31a7347 | 258 | |
| pmic | 4:52d2d31a7347 | 259 | if (t2 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 260 | float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 4:52d2d31a7347 | 261 | t1 = (maxVelocity-velocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 262 | t2 = 0.0f; |
| pmic | 4:52d2d31a7347 | 263 | t3 = maxVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 264 | } |
| pmic | 4:52d2d31a7347 | 265 | |
| pmic | 4:52d2d31a7347 | 266 | return t1+t2+t3; |
| pmic | 4:52d2d31a7347 | 267 | |
| pmic | 4:52d2d31a7347 | 268 | } else { // slow down to zero first, and then speed up to profile velocity |
| pmic | 4:52d2d31a7347 | 269 | |
| pmic | 4:52d2d31a7347 | 270 | float t1 = -velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 271 | float t2 = profileVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 272 | float t4 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 273 | float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4); |
| pmic | 4:52d2d31a7347 | 274 | |
| pmic | 4:52d2d31a7347 | 275 | if (t3 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 276 | float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 4:52d2d31a7347 | 277 | t2 = maxVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 278 | t3 = 0.0f; |
| pmic | 4:52d2d31a7347 | 279 | t4 = maxVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 280 | } |
| pmic | 4:52d2d31a7347 | 281 | |
| pmic | 4:52d2d31a7347 | 282 | return t1+t2+t3+t4; |
| pmic | 4:52d2d31a7347 | 283 | } |
| pmic | 4:52d2d31a7347 | 284 | |
| pmic | 4:52d2d31a7347 | 285 | } else { // negative velocity required |
| pmic | 4:52d2d31a7347 | 286 | |
| pmic | 4:52d2d31a7347 | 287 | if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first |
| pmic | 4:52d2d31a7347 | 288 | |
| pmic | 4:52d2d31a7347 | 289 | float t1 = (-profileVelocity-velocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 290 | float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity; |
| pmic | 4:52d2d31a7347 | 291 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 292 | |
| pmic | 4:52d2d31a7347 | 293 | return t1+t2+t3; |
| pmic | 4:52d2d31a7347 | 294 | |
| pmic | 4:52d2d31a7347 | 295 | } else if (velocity < 0.0f) { // speed up to (negative) profile velocity |
| pmic | 4:52d2d31a7347 | 296 | |
| pmic | 4:52d2d31a7347 | 297 | float t1 = (velocity+profileVelocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 298 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 299 | float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3; |
| pmic | 4:52d2d31a7347 | 300 | |
| pmic | 4:52d2d31a7347 | 301 | if (t2 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 302 | float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 4:52d2d31a7347 | 303 | t1 = (velocity-minVelocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 304 | t2 = 0.0f; |
| pmic | 4:52d2d31a7347 | 305 | t3 = -minVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 306 | } |
| pmic | 4:52d2d31a7347 | 307 | |
| pmic | 4:52d2d31a7347 | 308 | return t1+t2+t3; |
| pmic | 4:52d2d31a7347 | 309 | |
| pmic | 4:52d2d31a7347 | 310 | } else { // slow down to zero first, and then speed up to (negative) profile velocity |
| pmic | 4:52d2d31a7347 | 311 | |
| pmic | 4:52d2d31a7347 | 312 | float t1 = velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 313 | float t2 = profileVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 314 | float t4 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 315 | float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4); |
| pmic | 4:52d2d31a7347 | 316 | |
| pmic | 4:52d2d31a7347 | 317 | if (t3 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 318 | float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 4:52d2d31a7347 | 319 | t2 = -minVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 320 | t3 = 0.0f; |
| pmic | 4:52d2d31a7347 | 321 | t4 = -minVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 322 | } |
| pmic | 4:52d2d31a7347 | 323 | |
| pmic | 4:52d2d31a7347 | 324 | return t1+t2+t3+t4; |
| pmic | 4:52d2d31a7347 | 325 | } |
| pmic | 3:e6d345973797 | 326 | } |
| pmic | 3:e6d345973797 | 327 | } |
| pmic | 3:e6d345973797 | 328 | |
| pmic | 3:e6d345973797 | 329 | /** |
| pmic | 3:e6d345973797 | 330 | * Gets the distance moved until the velocity reaches zero. |
| pmic | 3:e6d345973797 | 331 | * @return the distance to the stop position. |
| pmic | 3:e6d345973797 | 332 | */ |
| pmic | 3:e6d345973797 | 333 | double Motion::getDistanceToStop() { |
| pmic | 4:52d2d31a7347 | 334 | |
| pmic | 4:52d2d31a7347 | 335 | return static_cast<double>(velocity*velocity/profileDeceleration*0.5f); |
| pmic | 3:e6d345973797 | 336 | } |
| pmic | 3:e6d345973797 | 337 | |
| pmic | 3:e6d345973797 | 338 | /** |
| pmic | 3:e6d345973797 | 339 | * Increments the current motion towards a given target velocity. |
| pmic | 3:e6d345973797 | 340 | * @param targetVelocity the desired target velocity given in [m/s] or [rad/s]. |
| pmic | 3:e6d345973797 | 341 | * @param period the time period to increment the motion values for, given in [s]. |
| pmic | 3:e6d345973797 | 342 | */ |
| pmic | 3:e6d345973797 | 343 | void Motion::incrementToVelocity(float targetVelocity, float period) { |
| pmic | 4:52d2d31a7347 | 344 | |
| pmic | 4:52d2d31a7347 | 345 | if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity; |
| pmic | 4:52d2d31a7347 | 346 | else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity; |
| pmic | 4:52d2d31a7347 | 347 | |
| pmic | 4:52d2d31a7347 | 348 | if (targetVelocity > 0.0f) { |
| pmic | 4:52d2d31a7347 | 349 | |
| pmic | 4:52d2d31a7347 | 350 | if (velocity > targetVelocity) { // slow down to target velocity |
| pmic | 4:52d2d31a7347 | 351 | |
| pmic | 4:52d2d31a7347 | 352 | float t1 = (velocity-targetVelocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 353 | |
| pmic | 4:52d2d31a7347 | 354 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 355 | position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 356 | velocity += -profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 357 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 358 | } else { |
| pmic | 4:52d2d31a7347 | 359 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 360 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 361 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 362 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 363 | } |
| pmic | 4:52d2d31a7347 | 364 | |
| pmic | 4:52d2d31a7347 | 365 | } else if (velocity > 0.0f) { // speed up to target velocity |
| pmic | 4:52d2d31a7347 | 366 | |
| pmic | 4:52d2d31a7347 | 367 | float t1 = (targetVelocity-velocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 368 | |
| pmic | 4:52d2d31a7347 | 369 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 370 | position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 371 | velocity += profileAcceleration*period; |
| pmic | 4:52d2d31a7347 | 372 | acceleration = profileAcceleration; |
| pmic | 4:52d2d31a7347 | 373 | } else { |
| pmic | 4:52d2d31a7347 | 374 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 375 | velocity += profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 376 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 377 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 378 | } |
| pmic | 4:52d2d31a7347 | 379 | |
| pmic | 4:52d2d31a7347 | 380 | } else { // slow down to zero first, and then speed up to target velocity |
| pmic | 4:52d2d31a7347 | 381 | |
| pmic | 4:52d2d31a7347 | 382 | float t1 = -velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 383 | float t2 = targetVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 384 | |
| pmic | 4:52d2d31a7347 | 385 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 386 | position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 387 | velocity += profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 388 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 389 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 390 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 391 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 392 | position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1)); |
| pmic | 4:52d2d31a7347 | 393 | velocity += profileAcceleration*(period-t1); |
| pmic | 4:52d2d31a7347 | 394 | acceleration = profileAcceleration; |
| pmic | 4:52d2d31a7347 | 395 | } else { |
| pmic | 4:52d2d31a7347 | 396 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 397 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 398 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 399 | velocity += profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 400 | position += static_cast<double>(velocity*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 401 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 402 | } |
| pmic | 4:52d2d31a7347 | 403 | } |
| pmic | 4:52d2d31a7347 | 404 | |
| pmic | 4:52d2d31a7347 | 405 | } else { |
| pmic | 4:52d2d31a7347 | 406 | |
| pmic | 4:52d2d31a7347 | 407 | if (velocity < targetVelocity) { // slow down to (negative) target velocity |
| pmic | 4:52d2d31a7347 | 408 | |
| pmic | 4:52d2d31a7347 | 409 | float t1 = (targetVelocity-velocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 410 | |
| pmic | 4:52d2d31a7347 | 411 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 412 | position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 413 | velocity += profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 414 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 415 | } else { |
| pmic | 4:52d2d31a7347 | 416 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 417 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 418 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 419 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 420 | } |
| pmic | 4:52d2d31a7347 | 421 | |
| pmic | 4:52d2d31a7347 | 422 | } else if (velocity < 0.0f) { // speed up to (negative) target velocity |
| pmic | 4:52d2d31a7347 | 423 | |
| pmic | 4:52d2d31a7347 | 424 | float t1 = (velocity-targetVelocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 425 | |
| pmic | 4:52d2d31a7347 | 426 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 427 | position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 428 | velocity += -profileAcceleration*period; |
| pmic | 4:52d2d31a7347 | 429 | acceleration = -profileAcceleration; |
| pmic | 4:52d2d31a7347 | 430 | } else { |
| pmic | 4:52d2d31a7347 | 431 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 432 | velocity += -profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 433 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 434 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 435 | } |
| pmic | 4:52d2d31a7347 | 436 | |
| pmic | 4:52d2d31a7347 | 437 | } else { // slow down to zero first, and then speed up to (negative) target velocity |
| pmic | 4:52d2d31a7347 | 438 | |
| pmic | 4:52d2d31a7347 | 439 | float t1 = velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 440 | float t2 = -targetVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 441 | |
| pmic | 4:52d2d31a7347 | 442 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 443 | position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 444 | velocity += -profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 445 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 446 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 447 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 448 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 449 | position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1)); |
| pmic | 4:52d2d31a7347 | 450 | velocity += -profileAcceleration*(period-t1); |
| pmic | 4:52d2d31a7347 | 451 | acceleration = -profileAcceleration; |
| pmic | 4:52d2d31a7347 | 452 | } else { |
| pmic | 4:52d2d31a7347 | 453 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 454 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 455 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 456 | velocity += -profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 457 | position += static_cast<double>(velocity*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 458 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 459 | } |
| pmic | 4:52d2d31a7347 | 460 | } |
| pmic | 3:e6d345973797 | 461 | } |
| pmic | 3:e6d345973797 | 462 | } |
| pmic | 3:e6d345973797 | 463 | |
| pmic | 3:e6d345973797 | 464 | /** |
| pmic | 3:e6d345973797 | 465 | * Increments the current motion towards a given target position. |
| pmic | 3:e6d345973797 | 466 | * @param targetPosition the desired target position given in [m] or [rad]. |
| pmic | 3:e6d345973797 | 467 | * @param period the time period to increment the motion values for, given in [s]. |
| pmic | 3:e6d345973797 | 468 | */ |
| pmic | 3:e6d345973797 | 469 | void Motion::incrementToPosition(double targetPosition, float period) { |
| pmic | 4:52d2d31a7347 | 470 | |
| pmic | 4:52d2d31a7347 | 471 | // calculate position, when velocity is reduced to zero |
| pmic | 4:52d2d31a7347 | 472 | |
| pmic | 4:52d2d31a7347 | 473 | double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f); |
| pmic | 4:52d2d31a7347 | 474 | |
| pmic | 4:52d2d31a7347 | 475 | if (targetPosition > stopPosition) { // positive velocity required |
| pmic | 4:52d2d31a7347 | 476 | |
| pmic | 4:52d2d31a7347 | 477 | if (velocity > profileVelocity) { // slow down to profile velocity first |
| pmic | 4:52d2d31a7347 | 478 | |
| pmic | 4:52d2d31a7347 | 479 | float t1 = (velocity-profileVelocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 480 | float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity; |
| pmic | 4:52d2d31a7347 | 481 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 482 | |
| pmic | 4:52d2d31a7347 | 483 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 484 | position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 485 | velocity += -profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 486 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 487 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 488 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 489 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 490 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 491 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 492 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 493 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 494 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 495 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 496 | position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 497 | velocity += -profileDeceleration*(period-t1-t2); |
| pmic | 4:52d2d31a7347 | 498 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 499 | } else { |
| pmic | 4:52d2d31a7347 | 500 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 501 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 502 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 503 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3); |
| pmic | 4:52d2d31a7347 | 504 | velocity += -profileDeceleration*t3; |
| pmic | 4:52d2d31a7347 | 505 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 506 | } |
| pmic | 4:52d2d31a7347 | 507 | |
| pmic | 4:52d2d31a7347 | 508 | } else if (velocity > 0.0f) { // speed up to profile velocity |
| pmic | 4:52d2d31a7347 | 509 | |
| pmic | 4:52d2d31a7347 | 510 | float t1 = (profileVelocity-velocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 511 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 512 | float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3; |
| pmic | 4:52d2d31a7347 | 513 | |
| pmic | 4:52d2d31a7347 | 514 | if (t2 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 515 | float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 3:e6d345973797 | 516 | t1 = (maxVelocity-velocity)/profileAcceleration; |
| pmic | 3:e6d345973797 | 517 | t2 = 0.0f; |
| pmic | 3:e6d345973797 | 518 | t3 = maxVelocity/profileDeceleration; |
| pmic | 3:e6d345973797 | 519 | } |
| pmic | 4:52d2d31a7347 | 520 | |
| pmic | 4:52d2d31a7347 | 521 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 522 | position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 523 | velocity += profileAcceleration*period; |
| pmic | 4:52d2d31a7347 | 524 | acceleration = profileAcceleration; |
| pmic | 4:52d2d31a7347 | 525 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 526 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 527 | velocity += profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 528 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 529 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 530 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 531 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 532 | velocity += profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 533 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 534 | position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 535 | velocity += -profileDeceleration*(period-t1-t2); |
| pmic | 4:52d2d31a7347 | 536 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 537 | } else { |
| pmic | 4:52d2d31a7347 | 538 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 539 | velocity += profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 540 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 541 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3); |
| pmic | 4:52d2d31a7347 | 542 | velocity += -profileDeceleration*t3; |
| pmic | 4:52d2d31a7347 | 543 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 544 | } |
| pmic | 4:52d2d31a7347 | 545 | |
| pmic | 4:52d2d31a7347 | 546 | } else { // slow down to zero first, and then speed up to profile velocity |
| pmic | 4:52d2d31a7347 | 547 | |
| pmic | 4:52d2d31a7347 | 548 | float t1 = -velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 549 | float t2 = profileVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 550 | float t4 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 551 | float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4); |
| pmic | 4:52d2d31a7347 | 552 | |
| pmic | 4:52d2d31a7347 | 553 | if (t3 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 554 | float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 3:e6d345973797 | 555 | t2 = maxVelocity/profileAcceleration; |
| pmic | 3:e6d345973797 | 556 | t3 = 0.0f; |
| pmic | 3:e6d345973797 | 557 | t4 = maxVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 558 | } |
| pmic | 4:52d2d31a7347 | 559 | |
| pmic | 4:52d2d31a7347 | 560 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 561 | position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 562 | velocity += profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 563 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 564 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 565 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 566 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 567 | position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1)); |
| pmic | 4:52d2d31a7347 | 568 | velocity += profileAcceleration*(period-t1); |
| pmic | 4:52d2d31a7347 | 569 | acceleration = profileAcceleration; |
| pmic | 4:52d2d31a7347 | 570 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 571 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 572 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 573 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 574 | velocity += profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 575 | position += static_cast<double>(velocity*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 576 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 577 | } else if (t1+t2+t3+t4 > period) { |
| pmic | 4:52d2d31a7347 | 578 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 579 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 580 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 581 | velocity += profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 582 | position += static_cast<double>(velocity*t3); |
| pmic | 4:52d2d31a7347 | 583 | position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3)); |
| pmic | 4:52d2d31a7347 | 584 | velocity += -profileDeceleration*(period-t1-t2-t3); |
| pmic | 4:52d2d31a7347 | 585 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 586 | } else { |
| pmic | 4:52d2d31a7347 | 587 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 588 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 589 | position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 590 | velocity += profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 591 | position += static_cast<double>(velocity*t3); |
| pmic | 4:52d2d31a7347 | 592 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t4)*t4); |
| pmic | 4:52d2d31a7347 | 593 | velocity += -profileDeceleration*t4; |
| pmic | 4:52d2d31a7347 | 594 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 595 | } |
| pmic | 3:e6d345973797 | 596 | } |
| pmic | 4:52d2d31a7347 | 597 | |
| pmic | 4:52d2d31a7347 | 598 | } else { // negative velocity required |
| pmic | 4:52d2d31a7347 | 599 | |
| pmic | 4:52d2d31a7347 | 600 | if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first |
| pmic | 4:52d2d31a7347 | 601 | |
| pmic | 4:52d2d31a7347 | 602 | float t1 = (-profileVelocity-velocity)/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 603 | float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity; |
| pmic | 4:52d2d31a7347 | 604 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 605 | |
| pmic | 4:52d2d31a7347 | 606 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 607 | position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 608 | velocity += profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 609 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 610 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 611 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 612 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 613 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 614 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 615 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 616 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 617 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 618 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 619 | position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 620 | velocity += profileDeceleration*(period-t1-t2); |
| pmic | 4:52d2d31a7347 | 621 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 622 | } else { |
| pmic | 4:52d2d31a7347 | 623 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 624 | velocity += profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 625 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 626 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3); |
| pmic | 4:52d2d31a7347 | 627 | velocity += profileDeceleration*t3; |
| pmic | 4:52d2d31a7347 | 628 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 629 | } |
| pmic | 4:52d2d31a7347 | 630 | |
| pmic | 4:52d2d31a7347 | 631 | } else if (velocity < 0.0f) { // speed up to (negative) profile velocity |
| pmic | 4:52d2d31a7347 | 632 | |
| pmic | 4:52d2d31a7347 | 633 | float t1 = (velocity+profileVelocity)/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 634 | float t3 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 635 | float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3; |
| pmic | 4:52d2d31a7347 | 636 | |
| pmic | 4:52d2d31a7347 | 637 | if (t2 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 638 | float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 3:e6d345973797 | 639 | t1 = (velocity-minVelocity)/profileAcceleration; |
| pmic | 3:e6d345973797 | 640 | t2 = 0.0f; |
| pmic | 3:e6d345973797 | 641 | t3 = -minVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 642 | } |
| pmic | 4:52d2d31a7347 | 643 | |
| pmic | 4:52d2d31a7347 | 644 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 645 | position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 646 | velocity += -profileAcceleration*period; |
| pmic | 4:52d2d31a7347 | 647 | acceleration = -profileAcceleration; |
| pmic | 4:52d2d31a7347 | 648 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 649 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 650 | velocity += -profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 651 | position += static_cast<double>(velocity*(period-t1)); |
| pmic | 4:52d2d31a7347 | 652 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 653 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 654 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 655 | velocity += -profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 656 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 657 | position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 658 | velocity += profileDeceleration*(period-t1-t2); |
| pmic | 4:52d2d31a7347 | 659 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 660 | } else { |
| pmic | 4:52d2d31a7347 | 661 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 662 | velocity += -profileAcceleration*t1; |
| pmic | 4:52d2d31a7347 | 663 | position += static_cast<double>(velocity*t2); |
| pmic | 4:52d2d31a7347 | 664 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3); |
| pmic | 4:52d2d31a7347 | 665 | velocity += profileDeceleration*t3; |
| pmic | 4:52d2d31a7347 | 666 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 667 | } |
| pmic | 4:52d2d31a7347 | 668 | |
| pmic | 4:52d2d31a7347 | 669 | } else { // slow down to zero first, and then speed up to (negative) profile velocity |
| pmic | 4:52d2d31a7347 | 670 | |
| pmic | 4:52d2d31a7347 | 671 | float t1 = velocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 672 | float t2 = profileVelocity/profileAcceleration; |
| pmic | 4:52d2d31a7347 | 673 | float t4 = profileVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 674 | float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4); |
| pmic | 4:52d2d31a7347 | 675 | |
| pmic | 4:52d2d31a7347 | 676 | if (t3 < 0.0f) { |
| pmic | 4:52d2d31a7347 | 677 | float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration)); |
| pmic | 3:e6d345973797 | 678 | t2 = -minVelocity/profileAcceleration; |
| pmic | 3:e6d345973797 | 679 | t3 = 0.0f; |
| pmic | 3:e6d345973797 | 680 | t4 = -minVelocity/profileDeceleration; |
| pmic | 4:52d2d31a7347 | 681 | } |
| pmic | 4:52d2d31a7347 | 682 | |
| pmic | 4:52d2d31a7347 | 683 | if (t1 > period) { |
| pmic | 4:52d2d31a7347 | 684 | position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period); |
| pmic | 4:52d2d31a7347 | 685 | velocity += -profileDeceleration*period; |
| pmic | 4:52d2d31a7347 | 686 | acceleration = -profileDeceleration; |
| pmic | 4:52d2d31a7347 | 687 | } else if (t1+t2 > period) { |
| pmic | 4:52d2d31a7347 | 688 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 689 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 690 | position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1)); |
| pmic | 4:52d2d31a7347 | 691 | velocity += -profileAcceleration*(period-t1); |
| pmic | 4:52d2d31a7347 | 692 | acceleration = -profileAcceleration; |
| pmic | 4:52d2d31a7347 | 693 | } else if (t1+t2+t3 > period) { |
| pmic | 4:52d2d31a7347 | 694 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 695 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 696 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 697 | velocity += -profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 698 | position += static_cast<double>(velocity*(period-t1-t2)); |
| pmic | 4:52d2d31a7347 | 699 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 700 | } else if (t1+t2+t3+t4 > period) { |
| pmic | 4:52d2d31a7347 | 701 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 702 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 703 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 704 | velocity += -profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 705 | position += static_cast<double>(velocity*t3); |
| pmic | 4:52d2d31a7347 | 706 | position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3)); |
| pmic | 4:52d2d31a7347 | 707 | velocity += profileDeceleration*(period-t1-t2-t3); |
| pmic | 4:52d2d31a7347 | 708 | acceleration = profileDeceleration; |
| pmic | 4:52d2d31a7347 | 709 | } else { |
| pmic | 4:52d2d31a7347 | 710 | position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1); |
| pmic | 4:52d2d31a7347 | 711 | velocity += -profileDeceleration*t1; |
| pmic | 4:52d2d31a7347 | 712 | position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2); |
| pmic | 4:52d2d31a7347 | 713 | velocity += -profileAcceleration*t2; |
| pmic | 4:52d2d31a7347 | 714 | position += static_cast<double>(velocity*t3); |
| pmic | 4:52d2d31a7347 | 715 | position += static_cast<double>((velocity+profileDeceleration*0.5f*t4)*t4); |
| pmic | 4:52d2d31a7347 | 716 | velocity += profileDeceleration*t4; |
| pmic | 4:52d2d31a7347 | 717 | acceleration = 0.0f; |
| pmic | 4:52d2d31a7347 | 718 | } |
| pmic | 3:e6d345973797 | 719 | } |
| pmic | 3:e6d345973797 | 720 | } |
| pmic | 3:e6d345973797 | 721 | } |