#include <moveCommand.h>
Public Member Functions | |
| virtual double | getPos (double TimeElapsed) |
| returns the planned position for TimeElapsed (seconds) | |
| double | getPos () |
| virtual double | getTotalTime () |
| returns the planned total time for the movement (in seconds) | |
| virtual double | getVel (double TimeElapsed) |
| returns the planned velocity for TimeElapsed (seconds) | |
| double | getVel () |
| virtual bool | inPhase1 () |
| virtual bool | inPhase3 () |
| virtual RampCommand & | operator= (const RampCommand &rc) |
| RampCommand (double x0, double v0, double xtarget, double amax, double vmax) | |
| RampCommand (const RampCommand &rc) | |
| virtual double | T1 () |
| Return the times of the different phases of the ramp move. | |
| virtual double | T2 () |
| virtual double | T3 () |
| virtual | ~RampCommand () |
Static Public Member Functions | |
| static void | calculateAV (double x0, double v0, double xtarget, double time, double T3, double amax, double vmax, double &a, double &v) |
| Calculate the necessary a and v of a rampmove, so that the move will take the desired time. | |
Private Attributes | |
| double | m_a1 |
| double | m_a3 |
| double | m_amax |
| RampCommand * | m_nachumkehr |
| double | m_T1 |
| double | m_T2 |
| double | m_T3 |
| bool | m_umkehr |
| double | m_v0 |
| double | m_v2 |
| double | m_vmax |
| double | m_x0 |
| double | m_xtarget |
Static Private Attributes | |
| static std::ofstream | debug |
Definition at line 129 of file moveCommand.h.
| RampCommand::RampCommand | ( | double | x0, |
| double | v0, | ||
| double | xtarget, | ||
| double | amax, | ||
| double | vmax | ||
| ) |
Just absolute values should be used. The direction will be set later on by using Delta
Set up the movement phases:
Current velocity should have same sign as desired velocity. (No reversion possible.)
Absolute value of current velocity is less than value of desired velocity.
Calculate critical deltas:
delta = 0
Absolute value of current velocity is greater than value of desired velocity.
Calculate critical values:
v0 / root(2)
(fabs(m_vmax) < fabs(vstern))
if (m_v0 * m_vmax >= 0) If sign of current velocity different to sign of desired velocity -> reversion needed
Go on till 0. By reaching 0 calling new RampCommand with v0 = 0
Done. Contructor finished, motion defined.
Definition at line 68 of file moveCommand.cpp.
| RampCommand::RampCommand | ( | const RampCommand & | rc | ) |
Attention of recursion! In this implementation we make sure that m_nachumkehr RampCommand has no antoher m_nachumkehr ( (*m_nachumkehr).m_nachumkehr=NULL ). After possible changes please make sure there is not any longer infinite recursion.
Definition at line 193 of file moveCommand.cpp.
| virtual RampCommand::~RampCommand | ( | ) | [inline, virtual] |
Definition at line 137 of file moveCommand.h.
| void RampCommand::calculateAV | ( | double | x0, |
| double | v0, | ||
| double | xtarget, | ||
| double | time, | ||
| double | T3, | ||
| double | amax, | ||
| double | vmax, | ||
| double & | acc, | ||
| double & | vel | ||
| ) | [static] |
Calculate the necessary a and v of a rampmove, so that the move will take the desired time.
If possible, the deceleration phase should take the time T3, so that it can be made equal for all joints
Do not trust sign, set correct sign by have a look on delta
ToDo: Noch Fehlerhaft! Wurzelterme < 0 etc, richtig machen!
v by calculation formula (I). a,b,c for quadratic formula:
Quadratic formula:
Now calculate a with formula (1):
fabs(delta) > d2s verhindert, prevent root of negative values!
v by calculation formula (II). a,b,c for quadratic formula:
Quadratic formula:
Now calculate a with formula (1):
ToDo: Check if needed
Dieser Fall bei bestimmten (relativ biedrigen) Deltas in Kombination mit relativ hohen v0s auf, und für diesen Fall kann ich keine Formel finden, für die Abbremsphase = T3 !!! Ich finde es komisch, dass hier die Formel oben versagt (Wurzelterm < 0) und die untere falsche Ergebnisse liefert... Lösung, bei der Abbremsphase != T3: out << " Fall 3:\n";
ToDo This special case is not solved yet. Occures with great values of v0 "Wrong Solution" (Joint arrives to soon):
Dieser Spezialfall ist noch nicht gelöst, tritt bei eher größeren v0 auf "Falsche Lösung" (Joint kommt zu früh an):
ToDo Oversteer. It can't be ensure to have a slowing-down process while T3. (System of equations would be overdetermined.)
Übersteuern, hier kann nicht gewährleistet werden, dass Abbremsphase Während T3 (Gleichungssystem wäre überbestimmt). out << " Fall 4\n";
Root does not make any problems.
Root reports some issues, temporary solution:
out << " Fall 5\n"; This is not "clean" yet ... temporary, incorrect solution
out << " Error in RampCommand::CalculateAV! time T3 is negative!\n";
out << " acc: " << acc << ",\tvel: " << vel << "\n"; out << "RampCommand::calculateAV ENDE\n"; debug.flush();
Definition at line 334 of file moveCommand.cpp.
| double RampCommand::getPos | ( | double | TimeElapsed | ) | [virtual] |
returns the planned position for TimeElapsed (seconds)
Returns the planned position for TimeElapsed (seconds)
Motion done. Targetposition reached.
Implements moveCommand.
Definition at line 251 of file moveCommand.cpp.
| double RampCommand::getPos | ( | ) | [inline] |
Definition at line 143 of file moveCommand.h.
| double RampCommand::getTotalTime | ( | ) | [virtual] |
returns the planned total time for the movement (in seconds)
Returns the planned total time for the movement in seconds.
Implements moveCommand.
Definition at line 320 of file moveCommand.cpp.
| double RampCommand::getVel | ( | double | TimeElapsed | ) | [virtual] |
returns the planned velocity for TimeElapsed (seconds)
returns the planned velocity for TimeElapsed
Motion done. Motor inactive.
Implements moveCommand.
Definition at line 287 of file moveCommand.cpp.
| double RampCommand::getVel | ( | ) | [inline] |
Definition at line 149 of file moveCommand.h.
| virtual bool RampCommand::inPhase1 | ( | ) | [inline, virtual] |
Definition at line 156 of file moveCommand.h.
| virtual bool RampCommand::inPhase3 | ( | ) | [inline, virtual] |
Definition at line 157 of file moveCommand.h.
| RampCommand & RampCommand::operator= | ( | const RampCommand & | rc | ) | [virtual] |
Attention of recursion! In this implementation we make sure that m_nachumkehr RampCommand has no antoher m_nachumkehr ( (*m_nachumkehr).m_nachumkehr=NULL ). After possible changes please make sure there is not any longer infinite recursion.
Definition at line 218 of file moveCommand.cpp.
| virtual double RampCommand::T1 | ( | ) | [inline, virtual] |
Return the times of the different phases of the ramp move.
Definition at line 163 of file moveCommand.h.
| virtual double RampCommand::T2 | ( | ) | [inline, virtual] |
Definition at line 164 of file moveCommand.h.
| virtual double RampCommand::T3 | ( | ) | [inline, virtual] |
Definition at line 165 of file moveCommand.h.
std::ofstream RampCommand::debug [static, private] |
Definition at line 175 of file moveCommand.h.
double RampCommand::m_a1 [private] |
Definition at line 182 of file moveCommand.h.
double RampCommand::m_a3 [private] |
Definition at line 182 of file moveCommand.h.
double RampCommand::m_amax [private] |
Definition at line 179 of file moveCommand.h.
RampCommand* RampCommand::m_nachumkehr [private] |
Definition at line 184 of file moveCommand.h.
double RampCommand::m_T1 [private] |
Definition at line 181 of file moveCommand.h.
double RampCommand::m_T2 [private] |
Definition at line 181 of file moveCommand.h.
double RampCommand::m_T3 [private] |
Definition at line 181 of file moveCommand.h.
bool RampCommand::m_umkehr [private] |
Definition at line 183 of file moveCommand.h.
double RampCommand::m_v0 [private] |
Definition at line 177 of file moveCommand.h.
double RampCommand::m_v2 [private] |
Definition at line 182 of file moveCommand.h.
double RampCommand::m_vmax [private] |
Definition at line 179 of file moveCommand.h.
double RampCommand::m_x0 [private] |
Definition at line 177 of file moveCommand.h.
double RampCommand::m_xtarget [private] |
Definition at line 178 of file moveCommand.h.
