This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf
Fork of autonomous Robot Android by
RobotControl/MotionState.cpp@0:31f7be68e52d, 2013-02-07 (annotated)
- Committer:
- chrigelburri
- Date:
- Thu Feb 07 17:43:19 2013 +0000
- Revision:
- 0:31f7be68e52d
- Child:
- 6:48eeb41188dd
first steps
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chrigelburri | 0:31f7be68e52d | 1 | #include "MotionState.h" |
chrigelburri | 0:31f7be68e52d | 2 | |
chrigelburri | 0:31f7be68e52d | 3 | using namespace std; |
chrigelburri | 0:31f7be68e52d | 4 | |
chrigelburri | 0:31f7be68e52d | 5 | MotionState::MotionState() |
chrigelburri | 0:31f7be68e52d | 6 | { |
chrigelburri | 0:31f7be68e52d | 7 | xposition = 0.0f; |
chrigelburri | 0:31f7be68e52d | 8 | yposition = 0.0f; |
chrigelburri | 0:31f7be68e52d | 9 | theta = 0.0f; |
chrigelburri | 0:31f7be68e52d | 10 | thetaCompass = 0.0f; |
chrigelburri | 0:31f7be68e52d | 11 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 12 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 13 | |
chrigelburri | 0:31f7be68e52d | 14 | acceleration = ACCELERATION; |
chrigelburri | 0:31f7be68e52d | 15 | thetaAcceleration = THETA_ACCELERATION; |
chrigelburri | 0:31f7be68e52d | 16 | } |
chrigelburri | 0:31f7be68e52d | 17 | |
chrigelburri | 0:31f7be68e52d | 18 | |
chrigelburri | 0:31f7be68e52d | 19 | MotionState::MotionState(float xposition, float yposition, float theta, float speed, float omega) |
chrigelburri | 0:31f7be68e52d | 20 | { |
chrigelburri | 0:31f7be68e52d | 21 | this->xposition = xposition; |
chrigelburri | 0:31f7be68e52d | 22 | this->yposition = yposition; |
chrigelburri | 0:31f7be68e52d | 23 | this->theta = theta; |
chrigelburri | 0:31f7be68e52d | 24 | this->speed = speed; |
chrigelburri | 0:31f7be68e52d | 25 | this->omega = omega; |
chrigelburri | 0:31f7be68e52d | 26 | acceleration = ACCELERATION; |
chrigelburri | 0:31f7be68e52d | 27 | thetaAcceleration = THETA_ACCELERATION; |
chrigelburri | 0:31f7be68e52d | 28 | } |
chrigelburri | 0:31f7be68e52d | 29 | |
chrigelburri | 0:31f7be68e52d | 30 | MotionState::~MotionState() |
chrigelburri | 0:31f7be68e52d | 31 | { |
chrigelburri | 0:31f7be68e52d | 32 | |
chrigelburri | 0:31f7be68e52d | 33 | } |
chrigelburri | 0:31f7be68e52d | 34 | |
chrigelburri | 0:31f7be68e52d | 35 | void MotionState::setState(float xposition, float yposition, float theta, float speed, float omega) |
chrigelburri | 0:31f7be68e52d | 36 | { |
chrigelburri | 0:31f7be68e52d | 37 | this->xposition = xposition; |
chrigelburri | 0:31f7be68e52d | 38 | this->yposition = yposition; |
chrigelburri | 0:31f7be68e52d | 39 | this->theta = theta; |
chrigelburri | 0:31f7be68e52d | 40 | this->speed = speed; |
chrigelburri | 0:31f7be68e52d | 41 | this->omega = omega; |
chrigelburri | 0:31f7be68e52d | 42 | } |
chrigelburri | 0:31f7be68e52d | 43 | |
chrigelburri | 0:31f7be68e52d | 44 | void MotionState::setState(MotionState* motionState) |
chrigelburri | 0:31f7be68e52d | 45 | { |
chrigelburri | 0:31f7be68e52d | 46 | xposition = motionState->xposition; |
chrigelburri | 0:31f7be68e52d | 47 | yposition = motionState->yposition; |
chrigelburri | 0:31f7be68e52d | 48 | theta = motionState->theta; |
chrigelburri | 0:31f7be68e52d | 49 | speed = motionState->speed; |
chrigelburri | 0:31f7be68e52d | 50 | omega = motionState->omega; |
chrigelburri | 0:31f7be68e52d | 51 | } |
chrigelburri | 0:31f7be68e52d | 52 | |
chrigelburri | 0:31f7be68e52d | 53 | void MotionState::setxPosition(float xposition) |
chrigelburri | 0:31f7be68e52d | 54 | { |
chrigelburri | 0:31f7be68e52d | 55 | this->xposition = xposition; |
chrigelburri | 0:31f7be68e52d | 56 | } |
chrigelburri | 0:31f7be68e52d | 57 | |
chrigelburri | 0:31f7be68e52d | 58 | float MotionState::getxPosition() |
chrigelburri | 0:31f7be68e52d | 59 | { |
chrigelburri | 0:31f7be68e52d | 60 | return xposition; |
chrigelburri | 0:31f7be68e52d | 61 | } |
chrigelburri | 0:31f7be68e52d | 62 | |
chrigelburri | 0:31f7be68e52d | 63 | void MotionState::setyPosition(float yposition) |
chrigelburri | 0:31f7be68e52d | 64 | { |
chrigelburri | 0:31f7be68e52d | 65 | this->yposition = yposition; |
chrigelburri | 0:31f7be68e52d | 66 | } |
chrigelburri | 0:31f7be68e52d | 67 | |
chrigelburri | 0:31f7be68e52d | 68 | float MotionState::getyPosition() |
chrigelburri | 0:31f7be68e52d | 69 | { |
chrigelburri | 0:31f7be68e52d | 70 | return yposition; |
chrigelburri | 0:31f7be68e52d | 71 | } |
chrigelburri | 0:31f7be68e52d | 72 | |
chrigelburri | 0:31f7be68e52d | 73 | void MotionState::setTheta(float theta) |
chrigelburri | 0:31f7be68e52d | 74 | { |
chrigelburri | 0:31f7be68e52d | 75 | this->theta = theta; |
chrigelburri | 0:31f7be68e52d | 76 | } |
chrigelburri | 0:31f7be68e52d | 77 | |
chrigelburri | 0:31f7be68e52d | 78 | float MotionState::getTheta() |
chrigelburri | 0:31f7be68e52d | 79 | { |
chrigelburri | 0:31f7be68e52d | 80 | return theta; |
chrigelburri | 0:31f7be68e52d | 81 | } |
chrigelburri | 0:31f7be68e52d | 82 | |
chrigelburri | 0:31f7be68e52d | 83 | void MotionState::setSpeed(float speed) |
chrigelburri | 0:31f7be68e52d | 84 | { |
chrigelburri | 0:31f7be68e52d | 85 | this->speed = speed; |
chrigelburri | 0:31f7be68e52d | 86 | } |
chrigelburri | 0:31f7be68e52d | 87 | |
chrigelburri | 0:31f7be68e52d | 88 | float MotionState::getSpeed() |
chrigelburri | 0:31f7be68e52d | 89 | { |
chrigelburri | 0:31f7be68e52d | 90 | return speed; |
chrigelburri | 0:31f7be68e52d | 91 | } |
chrigelburri | 0:31f7be68e52d | 92 | |
chrigelburri | 0:31f7be68e52d | 93 | void MotionState::setOmega(float omega) |
chrigelburri | 0:31f7be68e52d | 94 | { |
chrigelburri | 0:31f7be68e52d | 95 | this->omega = omega; |
chrigelburri | 0:31f7be68e52d | 96 | } |
chrigelburri | 0:31f7be68e52d | 97 | |
chrigelburri | 0:31f7be68e52d | 98 | float MotionState::getOmega() |
chrigelburri | 0:31f7be68e52d | 99 | { |
chrigelburri | 0:31f7be68e52d | 100 | return omega; |
chrigelburri | 0:31f7be68e52d | 101 | } |
chrigelburri | 0:31f7be68e52d | 102 | |
chrigelburri | 0:31f7be68e52d | 103 | void MotionState::setAcceleration(float acceleration) |
chrigelburri | 0:31f7be68e52d | 104 | { |
chrigelburri | 0:31f7be68e52d | 105 | this->acceleration = acceleration; |
chrigelburri | 0:31f7be68e52d | 106 | } |
chrigelburri | 0:31f7be68e52d | 107 | |
chrigelburri | 0:31f7be68e52d | 108 | float MotionState::getAcceleration() |
chrigelburri | 0:31f7be68e52d | 109 | { |
chrigelburri | 0:31f7be68e52d | 110 | return acceleration; |
chrigelburri | 0:31f7be68e52d | 111 | } |
chrigelburri | 0:31f7be68e52d | 112 | |
chrigelburri | 0:31f7be68e52d | 113 | void MotionState::setThetaAcceleration(float thetaAcceleration) |
chrigelburri | 0:31f7be68e52d | 114 | { |
chrigelburri | 0:31f7be68e52d | 115 | this->thetaAcceleration = thetaAcceleration; |
chrigelburri | 0:31f7be68e52d | 116 | } |
chrigelburri | 0:31f7be68e52d | 117 | |
chrigelburri | 0:31f7be68e52d | 118 | float MotionState::getThetaAcceleration() |
chrigelburri | 0:31f7be68e52d | 119 | { |
chrigelburri | 0:31f7be68e52d | 120 | return thetaAcceleration; |
chrigelburri | 0:31f7be68e52d | 121 | } |
chrigelburri | 0:31f7be68e52d | 122 | |
chrigelburri | 0:31f7be68e52d | 123 | void MotionState::increment(float desiredSpeed, float desiredOmega, float period) |
chrigelburri | 0:31f7be68e52d | 124 | { |
chrigelburri | 0:31f7be68e52d | 125 | float acceleration = (desiredSpeed-speed)/period; |
chrigelburri | 0:31f7be68e52d | 126 | if (acceleration < -this->acceleration) acceleration = -this->acceleration; |
chrigelburri | 0:31f7be68e52d | 127 | else if (acceleration > this->acceleration) acceleration = this->acceleration; |
chrigelburri | 0:31f7be68e52d | 128 | |
chrigelburri | 0:31f7be68e52d | 129 | float thetaAcceleration = (desiredOmega-omega)/period; |
chrigelburri | 0:31f7be68e52d | 130 | if (thetaAcceleration < -this->thetaAcceleration) thetaAcceleration = -this->thetaAcceleration; |
chrigelburri | 0:31f7be68e52d | 131 | else if (thetaAcceleration > this->thetaAcceleration) thetaAcceleration = this->thetaAcceleration; |
chrigelburri | 0:31f7be68e52d | 132 | |
chrigelburri | 0:31f7be68e52d | 133 | speed += acceleration * period; |
chrigelburri | 0:31f7be68e52d | 134 | omega += thetaAcceleration * period; |
chrigelburri | 0:31f7be68e52d | 135 | } |