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
Diff: Motion.cpp
- Revision:
- 4:52d2d31a7347
- Parent:
- 3:e6d345973797
diff -r e6d345973797 -r 52d2d31a7347 Motion.cpp
--- a/Motion.cpp Thu Dec 05 09:02:14 2019 +0000
+++ b/Motion.cpp Thu Dec 05 09:09:31 2019 +0000
@@ -11,6 +11,8 @@
#include <algorithm>
#include "Motion.h"
+using namespace std;
+
const float Motion::DEFAULT_LIMIT = 1.0f; // default value for limits
const float Motion::MINIMUM_LIMIT = 1.0e-6f; // smallest value allowed for limits
@@ -19,14 +21,14 @@
* The values for position, velocity and acceleration are set to 0.
*/
Motion::Motion() {
-
- position = 0.0;
- velocity = 0.0f;
- acceleration = 0.0f;
-
- profileVelocity = DEFAULT_LIMIT;
- profileAcceleration = DEFAULT_LIMIT;
- profileDeceleration = DEFAULT_LIMIT;
+
+ position = 0.0;
+ velocity = 0.0f;
+ acceleration = 0.0f;
+
+ profileVelocity = DEFAULT_LIMIT;
+ profileAcceleration = DEFAULT_LIMIT;
+ profileDeceleration = DEFAULT_LIMIT;
}
/**
@@ -35,14 +37,14 @@
* @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
*/
Motion::Motion(double position, float velocity) {
-
- this->position = position;
- this->velocity = velocity;
- this->acceleration = 0.0f;
-
- profileVelocity = DEFAULT_LIMIT;
- profileAcceleration = DEFAULT_LIMIT;
- profileDeceleration = DEFAULT_LIMIT;
+
+ this->position = position;
+ this->velocity = velocity;
+ this->acceleration = 0.0f;
+
+ profileVelocity = DEFAULT_LIMIT;
+ profileAcceleration = DEFAULT_LIMIT;
+ profileDeceleration = DEFAULT_LIMIT;
}
/**
@@ -52,14 +54,14 @@
* @param acceleration the initial acceleration value of this motion, given in [m/s²] or [rad/s²].
*/
Motion::Motion(double position, float velocity, float acceleration) {
-
- this->position = position;
- this->velocity = velocity;
- this->acceleration = acceleration;
-
- profileVelocity = DEFAULT_LIMIT;
- profileAcceleration = DEFAULT_LIMIT;
- profileDeceleration = DEFAULT_LIMIT;
+
+ this->position = position;
+ this->velocity = velocity;
+ this->acceleration = acceleration;
+
+ profileVelocity = DEFAULT_LIMIT;
+ profileAcceleration = DEFAULT_LIMIT;
+ profileDeceleration = DEFAULT_LIMIT;
}
/**
@@ -86,14 +88,14 @@
* @param motion another <code>Motion</code> object to copy the values from.
*/
Motion::Motion(const Motion& motion) {
-
- position = motion.position;
- velocity = motion.velocity;
- acceleration = motion.acceleration;
-
- profileVelocity = motion.profileVelocity;
- profileAcceleration = motion.profileAcceleration;
- profileDeceleration = motion.profileDeceleration;
+
+ position = motion.position;
+ velocity = motion.velocity;
+ acceleration = motion.acceleration;
+
+ profileVelocity = motion.profileVelocity;
+ profileAcceleration = motion.profileAcceleration;
+ profileDeceleration = motion.profileDeceleration;
}
Motion::~Motion() {}
@@ -104,9 +106,9 @@
* @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
*/
void Motion::set(double position, float velocity) {
-
- this->position = position;
- this->velocity = velocity;
+
+ this->position = position;
+ this->velocity = velocity;
}
/**
@@ -116,10 +118,10 @@
* @param acceleration the desired acceleration value of this motion, given in [m/s²] or [rad/s²].
*/
void Motion::set(double position, float velocity, float acceleration) {
-
- this->position = position;
- this->velocity = velocity;
- this->acceleration = acceleration;
+
+ this->position = position;
+ this->velocity = velocity;
+ this->acceleration = acceleration;
}
/**
@@ -127,10 +129,10 @@
* @param motion another <code>Motion</code> object to copy the values from.
*/
void Motion::set(const Motion& motion) {
-
- position = motion.position;
- velocity = motion.velocity;
- acceleration = motion.acceleration;
+
+ position = motion.position;
+ velocity = motion.velocity;
+ acceleration = motion.acceleration;
}
/**
@@ -138,8 +140,8 @@
* @param position the desired position value of this motion, given in [m] or [rad].
*/
void Motion::setPosition(double position) {
-
- this->position = position;
+
+ this->position = position;
}
/**
@@ -147,8 +149,8 @@
* @return the position value of this motion, given in [m] or [rad].
*/
double Motion::getPosition() {
-
- return position;
+
+ return position;
}
/**
@@ -156,8 +158,8 @@
* @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
*/
void Motion::setVelocity(float velocity) {
-
- this->velocity = velocity;
+
+ this->velocity = velocity;
}
/**
@@ -165,8 +167,8 @@
* @return the velocity value of this motion, given in [m/s] or [rad/s].
*/
float Motion::getVelocity() {
-
- return velocity;
+
+ return velocity;
}
/**
@@ -174,8 +176,8 @@
* @param acceleration the desired acceleration value of this motion, given in [m/s²] or [rad/s²].
*/
void Motion::setAcceleration(float acceleration) {
-
- this->acceleration = acceleration;
+
+ this->acceleration = acceleration;
}
/**
@@ -183,8 +185,8 @@
* @return the acceleration value of this motion, given in [m/s²] or [rad/s²].
*/
float Motion::getAcceleration() {
-
- return acceleration;
+
+ return acceleration;
}
/**
@@ -192,8 +194,8 @@
* @param profileVelocity the limit of the velocity.
*/
void Motion::setProfileVelocity(float profileVelocity) {
-
- if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+
+ if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
}
/**
@@ -201,8 +203,8 @@
* @param profileAcceleration the limit of the acceleration.
*/
void Motion::setProfileAcceleration(float profileAcceleration) {
-
- if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+
+ if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
}
/**
@@ -210,8 +212,8 @@
* @param profileDeceleration the limit of the deceleration.
*/
void Motion::setProfileDeceleration(float profileDeceleration) {
-
- if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+
+ if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
}
/**
@@ -221,10 +223,10 @@
* @param profileDeceleration the limit of the deceleration.
*/
void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
-
- if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
- if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
- if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+
+ if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+ if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+ if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
}
/**
@@ -233,95 +235,95 @@
* @return the time to move to the target position, given in [s].
*/
float Motion::getTimeToPosition(double targetPosition) {
-
- // calculate position, when velocity is reduced to zero
-
- double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
-
- if (targetPosition > stopPosition) { // positive velocity required
-
- if (velocity > profileVelocity) { // slow down to profile velocity first
-
- float t1 = (velocity-profileVelocity)/profileDeceleration;
- float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
- float t3 = profileVelocity/profileDeceleration;
-
- return t1+t2+t3;
-
- } else if (velocity > 0.0f) { // speed up to profile velocity
-
- float t1 = (profileVelocity-velocity)/profileAcceleration;
- float t3 = profileVelocity/profileDeceleration;
- float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
-
- if (t2 < 0.0f) {
- float maxVelocity = std::sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
- t1 = (maxVelocity-velocity)/profileAcceleration;
- t2 = 0.0f;
- t3 = maxVelocity/profileDeceleration;
- }
-
- return t1+t2+t3;
-
- } else { // slow down to zero first, and then speed up to profile velocity
-
- float t1 = -velocity/profileDeceleration;
- float t2 = profileVelocity/profileAcceleration;
- float t4 = profileVelocity/profileDeceleration;
- float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
-
- if (t3 < 0.0f) {
- float maxVelocity = std::sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
- t2 = maxVelocity/profileAcceleration;
- t3 = 0.0f;
- t4 = maxVelocity/profileDeceleration;
- }
-
- return t1+t2+t3+t4;
+
+ // calculate position, when velocity is reduced to zero
+
+ double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
+
+ if (targetPosition > stopPosition) { // positive velocity required
+
+ if (velocity > profileVelocity) { // slow down to profile velocity first
+
+ float t1 = (velocity-profileVelocity)/profileDeceleration;
+ float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ return t1+t2+t3;
+
+ } else if (velocity > 0.0f) { // speed up to profile velocity
+
+ float t1 = (profileVelocity-velocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (maxVelocity-velocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = maxVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3;
+
+ } else { // slow down to zero first, and then speed up to profile velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = maxVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = maxVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3+t4;
+ }
+
+ } else { // negative velocity required
+
+ if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+
+ float t1 = (-profileVelocity-velocity)/profileDeceleration;
+ float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ return t1+t2+t3;
+
+ } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+
+ float t1 = (velocity+profileVelocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (velocity-minVelocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = -minVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3;
+
+ } else { // slow down to zero first, and then speed up to (negative) profile velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = -minVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = -minVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3+t4;
+ }
}
-
- } else { // negative velocity required
-
- if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
-
- float t1 = (-profileVelocity-velocity)/profileDeceleration;
- float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
- float t3 = profileVelocity/profileDeceleration;
-
- return t1+t2+t3;
-
- } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
-
- float t1 = (velocity+profileVelocity)/profileAcceleration;
- float t3 = profileVelocity/profileDeceleration;
- float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
-
- if (t2 < 0.0f) {
- float minVelocity = -std::sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
- t1 = (velocity-minVelocity)/profileAcceleration;
- t2 = 0.0f;
- t3 = -minVelocity/profileDeceleration;
- }
-
- return t1+t2+t3;
-
- } else { // slow down to zero first, and then speed up to (negative) profile velocity
-
- float t1 = velocity/profileDeceleration;
- float t2 = profileVelocity/profileAcceleration;
- float t4 = profileVelocity/profileDeceleration;
- float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
-
- if (t3 < 0.0f) {
- float minVelocity = -std::sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
- t2 = -minVelocity/profileAcceleration;
- t3 = 0.0f;
- t4 = -minVelocity/profileDeceleration;
- }
-
- return t1+t2+t3+t4;
- }
- }
}
/**
@@ -329,8 +331,8 @@
* @return the distance to the stop position.
*/
double Motion::getDistanceToStop() {
-
- return static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
+
+ return static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
}
/**
@@ -339,124 +341,124 @@
* @param period the time period to increment the motion values for, given in [s].
*/
void Motion::incrementToVelocity(float targetVelocity, float period) {
-
- if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
- else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
-
- if (targetVelocity > 0.0f) {
-
- if (velocity > targetVelocity) { // slow down to target velocity
-
- float t1 = (velocity-targetVelocity)/profileDeceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
- velocity += -profileDeceleration*period;
- acceleration = -profileDeceleration;
- } else {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- }
-
- } else if (velocity > 0.0f) { // speed up to target velocity
-
- float t1 = (targetVelocity-velocity)/profileAcceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
- velocity += profileAcceleration*period;
- acceleration = profileAcceleration;
- } else {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
- velocity += profileAcceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- }
-
- } else { // slow down to zero first, and then speed up to target velocity
-
- float t1 = -velocity/profileDeceleration;
- float t2 = targetVelocity/profileAcceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
- velocity += profileDeceleration*period;
- acceleration = profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
- velocity += profileAcceleration*(period-t1);
- acceleration = profileAcceleration;
- } else {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
- velocity += profileAcceleration*t2;
- position += static_cast<double>(velocity*(period-t1-t2));
- acceleration = 0.0f;
- }
+
+ if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
+ else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
+
+ if (targetVelocity > 0.0f) {
+
+ if (velocity > targetVelocity) { // slow down to target velocity
+
+ float t1 = (velocity-targetVelocity)/profileDeceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ acceleration = -profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ }
+
+ } else if (velocity > 0.0f) { // speed up to target velocity
+
+ float t1 = (targetVelocity-velocity)/profileAcceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
+ velocity += profileAcceleration*period;
+ acceleration = profileAcceleration;
+ } else {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ }
+
+ } else { // slow down to zero first, and then speed up to target velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = targetVelocity/profileAcceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ acceleration = profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += profileAcceleration*(period-t1);
+ acceleration = profileAcceleration;
+ } else {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += static_cast<double>(velocity*(period-t1-t2));
+ acceleration = 0.0f;
+ }
+ }
+
+ } else {
+
+ if (velocity < targetVelocity) { // slow down to (negative) target velocity
+
+ float t1 = (targetVelocity-velocity)/profileDeceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ acceleration = profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ }
+
+ } else if (velocity < 0.0f) { // speed up to (negative) target velocity
+
+ float t1 = (velocity-targetVelocity)/profileAcceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
+ velocity += -profileAcceleration*period;
+ acceleration = -profileAcceleration;
+ } else {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ }
+
+ } else { // slow down to zero first, and then speed up to (negative) target velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = -targetVelocity/profileAcceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ acceleration = -profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += -profileAcceleration*(period-t1);
+ acceleration = -profileAcceleration;
+ } else {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += static_cast<double>(velocity*(period-t1-t2));
+ acceleration = 0.0f;
+ }
+ }
}
-
- } else {
-
- if (velocity < targetVelocity) { // slow down to (negative) target velocity
-
- float t1 = (targetVelocity-velocity)/profileDeceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
- velocity += profileDeceleration*period;
- acceleration = profileDeceleration;
- } else {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- }
-
- } else if (velocity < 0.0f) { // speed up to (negative) target velocity
-
- float t1 = (velocity-targetVelocity)/profileAcceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
- velocity += -profileAcceleration*period;
- acceleration = -profileAcceleration;
- } else {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
- velocity += -profileAcceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- }
-
- } else { // slow down to zero first, and then speed up to (negative) target velocity
-
- float t1 = velocity/profileDeceleration;
- float t2 = -targetVelocity/profileAcceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
- velocity += -profileDeceleration*period;
- acceleration = -profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
- velocity += -profileAcceleration*(period-t1);
- acceleration = -profileAcceleration;
- } else {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
- velocity += -profileAcceleration*t2;
- position += static_cast<double>(velocity*(period-t1-t2));
- acceleration = 0.0f;
- }
- }
- }
}
/**
@@ -465,314 +467,255 @@
* @param period the time period to increment the motion values for, given in [s].
*/
void Motion::incrementToPosition(double targetPosition, float period) {
-
- // calculate position, when velocity is reduced to zero
-
- double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
-
- if (targetPosition > stopPosition) { // positive velocity required
-
- if (velocity > profileVelocity) { // slow down to profile velocity first
-
- float t1 = (velocity-profileVelocity)/profileDeceleration;
- float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
- float t3 = profileVelocity/profileDeceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
- velocity += -profileDeceleration*period;
- acceleration = -profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
- velocity += -profileDeceleration*(period-t1-t2);
- acceleration = -profileDeceleration;
- } else {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
- velocity += -profileDeceleration*t3;
- acceleration = 0.0f;
- }
-
- } else if (velocity > 0.0f) { // speed up to profile velocity
-
- float t1 = (profileVelocity-velocity)/profileAcceleration;
- float t3 = profileVelocity/profileDeceleration;
- float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
-
- /*if (t2 < 0.0f) {
- float maxVelocity = std::sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+
+ // calculate position, when velocity is reduced to zero
+
+ double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
+
+ if (targetPosition > stopPosition) { // positive velocity required
+
+ if (velocity > profileVelocity) { // slow down to profile velocity first
+
+ float t1 = (velocity-profileVelocity)/profileDeceleration;
+ float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ acceleration = -profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += -profileDeceleration*(period-t1-t2);
+ acceleration = -profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
+ velocity += -profileDeceleration*t3;
+ acceleration = 0.0f;
+ }
+
+ } else if (velocity > 0.0f) { // speed up to profile velocity
+
+ float t1 = (profileVelocity-velocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
t1 = (maxVelocity-velocity)/profileAcceleration;
t2 = 0.0f;
t3 = maxVelocity/profileDeceleration;
}
- */
- if (t2 < 0.0f) {
- float temp((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
- if (temp < 0.0f) {
- t1 = 0.0f;
- t2 = 0.0f;
- t3 = 0.0f;
- } else {
- float maxVelocity(std::sqrt(temp));
- t1 = (maxVelocity-velocity)/profileAcceleration;
- t2 = 0.0f;
- t3 = maxVelocity/profileDeceleration;
- }
-
- }
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
- velocity += profileAcceleration*period;
- acceleration = profileAcceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
- velocity += profileAcceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
- velocity += profileAcceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
- velocity += -profileDeceleration*(period-t1-t2);
- acceleration = -profileDeceleration;
- } else {
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
- velocity += profileAcceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
- velocity += -profileDeceleration*t3;
- acceleration = 0.0f;
- }
-
- } else { // slow down to zero first, and then speed up to profile velocity
-
- float t1 = -velocity/profileDeceleration;
- float t2 = profileVelocity/profileAcceleration;
- float t4 = profileVelocity/profileDeceleration;
- float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
-
- /*if (t3 < 0.0f) {
- float maxVelocity = std::sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
+ velocity += profileAcceleration*period;
+ acceleration = profileAcceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += -profileDeceleration*(period-t1-t2);
+ acceleration = -profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
+ velocity += -profileDeceleration*t3;
+ acceleration = 0.0f;
+ }
+
+ } else { // slow down to zero first, and then speed up to profile velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
t2 = maxVelocity/profileAcceleration;
t3 = 0.0f;
t4 = maxVelocity/profileDeceleration;
- }*/
-
- if ( t3 < 0.0f ) {
- float temp((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
-
- if (temp<0.0f) {
- t2 = 0.0f;
- t3 = 0.0f;
- t4 = 0.0f;
- } else {
- float maxVelocity( std::sqrt( temp ) );
- t2 = ( maxVelocity - velocity ) / profileAcceleration;
- t3 = 0.0f;
- t4 = maxVelocity / profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ acceleration = profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += profileAcceleration*(period-t1);
+ acceleration = profileAcceleration;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += static_cast<double>(velocity*(period-t1-t2));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3+t4 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += static_cast<double>(velocity*t3);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+ velocity += -profileDeceleration*(period-t1-t2-t3);
+ acceleration = -profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += static_cast<double>(velocity*t3);
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t4)*t4);
+ velocity += -profileDeceleration*t4;
+ acceleration = 0.0f;
+ }
}
- }
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
- velocity += profileDeceleration*period;
- acceleration = profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
- velocity += profileAcceleration*(period-t1);
- acceleration = profileAcceleration;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
- velocity += profileAcceleration*t2;
- position += static_cast<double>(velocity*(period-t1-t2));
- acceleration = 0.0f;
- } else if (t1+t2+t3+t4 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
- velocity += profileAcceleration*t2;
- position += static_cast<double>(velocity*t3);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
- velocity += -profileDeceleration*(period-t1-t2-t3);
- acceleration = -profileDeceleration;
- } else {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
- velocity += profileAcceleration*t2;
- position += static_cast<double>(velocity*t3);
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t4)*t4);
- velocity += -profileDeceleration*t4;
- acceleration = 0.0f;
- }
- }
-
- } else { // negative velocity required
-
- if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
-
- float t1 = (-profileVelocity-velocity)/profileDeceleration;
- float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
- float t3 = profileVelocity/profileDeceleration;
-
- if (t1 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
- velocity += profileDeceleration*period;
- acceleration = profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
- velocity += profileDeceleration*(period-t1-t2);
- acceleration = profileDeceleration;
- } else {
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
- velocity += profileDeceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
- velocity += profileDeceleration*t3;
- acceleration = 0.0f;
- }
-
- } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
-
- float t1 = (velocity+profileVelocity)/profileAcceleration;
- float t3 = profileVelocity/profileDeceleration;
- float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
-
- /*if (t2 < 0.0f) {
- float minVelocity = -std::sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+
+ } else { // negative velocity required
+
+ if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+
+ float t1 = (-profileVelocity-velocity)/profileDeceleration;
+ float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ acceleration = profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += profileDeceleration*(period-t1-t2);
+ acceleration = profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
+ velocity += profileDeceleration*t3;
+ acceleration = 0.0f;
+ }
+
+ } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+
+ float t1 = (velocity+profileVelocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
t1 = (velocity-minVelocity)/profileAcceleration;
t2 = 0.0f;
t3 = -minVelocity/profileDeceleration;
- }*/
-
- if (t2 < 0.0f) {
- float temp((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
- if (temp<0.0f) {
- t1 = 0.0f;
- t2 = 0.0f;
- t3 = 0.0f;
- } else {
- float minVelocity( -std::sqrt(temp));
- t1 = (velocity-minVelocity)/profileAcceleration;
- t2 = 0.0f;
- t3 = -minVelocity/profileDeceleration;
- }
- }
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
- velocity += -profileAcceleration*period;
- acceleration = -profileAcceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
- velocity += -profileAcceleration*t1;
- position += static_cast<double>(velocity*(period-t1));
- acceleration = 0.0f;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
- velocity += -profileAcceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
- velocity += profileDeceleration*(period-t1-t2);
- acceleration = profileDeceleration;
- } else {
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
- velocity += -profileAcceleration*t1;
- position += static_cast<double>(velocity*t2);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
- velocity += profileDeceleration*t3;
- acceleration = 0.0f;
- }
-
- } else { // slow down to zero first, and then speed up to (negative) profile velocity
-
- float t1 = velocity/profileDeceleration;
- float t2 = profileVelocity/profileAcceleration;
- float t4 = profileVelocity/profileDeceleration;
- float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
-
- /*if (t3 < 0.0f) {
- float minVelocity = -std::sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ }
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
+ velocity += -profileAcceleration*period;
+ acceleration = -profileAcceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += static_cast<double>(velocity*(period-t1));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += profileDeceleration*(period-t1-t2);
+ acceleration = profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += static_cast<double>(velocity*t2);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
+ velocity += profileDeceleration*t3;
+ acceleration = 0.0f;
+ }
+
+ } else { // slow down to zero first, and then speed up to (negative) profile velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
t2 = -minVelocity/profileAcceleration;
t3 = 0.0f;
t4 = -minVelocity/profileDeceleration;
- }*/
-
- if (t3<0.0f){
- float temp((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
- if (temp < 0.0f) {
- t2 = 0.0f;
- t3 = 0.0f;
- t4 = 0.0f;
- } else {
- float minVelocity(-std::sqrt(temp));
- t2 = -minVelocity/profileAcceleration;
- t3 = 0.0f;
- t4 = -minVelocity/profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ acceleration = -profileDeceleration;
+ } else if (t1+t2 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += -profileAcceleration*(period-t1);
+ acceleration = -profileAcceleration;
+ } else if (t1+t2+t3 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += static_cast<double>(velocity*(period-t1-t2));
+ acceleration = 0.0f;
+ } else if (t1+t2+t3+t4 > period) {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += static_cast<double>(velocity*t3);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+ velocity += profileDeceleration*(period-t1-t2-t3);
+ acceleration = profileDeceleration;
+ } else {
+ position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += static_cast<double>(velocity*t3);
+ position += static_cast<double>((velocity+profileDeceleration*0.5f*t4)*t4);
+ velocity += profileDeceleration*t4;
+ acceleration = 0.0f;
+ }
}
- }
-
- if (t1 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
- velocity += -profileDeceleration*period;
- acceleration = -profileDeceleration;
- } else if (t1+t2 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
- velocity += -profileAcceleration*(period-t1);
- acceleration = -profileAcceleration;
- } else if (t1+t2+t3 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
- velocity += -profileAcceleration*t2;
- position += static_cast<double>(velocity*(period-t1-t2));
- acceleration = 0.0f;
- } else if (t1+t2+t3+t4 > period) {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
- velocity += -profileAcceleration*t2;
- position += static_cast<double>(velocity*t3);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
- velocity += profileDeceleration*(period-t1-t2-t3);
- acceleration = profileDeceleration;
- } else {
- position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
- velocity += -profileDeceleration*t1;
- position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
- velocity += -profileAcceleration*t2;
- position += static_cast<double>(velocity*t3);
- position += static_cast<double>((velocity+profileDeceleration*0.5f*t4)*t4);
- velocity += profileDeceleration*t4;
- acceleration = 0.0f;
- }
}
- }
}
-