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/RobotControl.cpp@0:31f7be68e52d, 2013-02-07 (annotated)
- Committer:
- chrigelburri
- Date:
- Thu Feb 07 17:43:19 2013 +0000
- Revision:
- 0:31f7be68e52d
- Child:
- 1:6cd533a712c6
first steps
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chrigelburri | 0:31f7be68e52d | 1 | #include "RobotControl.h" |
chrigelburri | 0:31f7be68e52d | 2 | |
chrigelburri | 0:31f7be68e52d | 3 | using namespace std; |
chrigelburri | 0:31f7be68e52d | 4 | |
chrigelburri | 0:31f7be68e52d | 5 | RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period) : Task(period) |
chrigelburri | 0:31f7be68e52d | 6 | { |
chrigelburri | 0:31f7be68e52d | 7 | /* get peripherals */ |
chrigelburri | 0:31f7be68e52d | 8 | this->motorControllerLeft = motorControllerLeft; |
chrigelburri | 0:31f7be68e52d | 9 | this->motorControllerRight = motorControllerRight; |
chrigelburri | 0:31f7be68e52d | 10 | this->compass = compass; |
chrigelburri | 0:31f7be68e52d | 11 | this->period = period; |
chrigelburri | 0:31f7be68e52d | 12 | |
chrigelburri | 0:31f7be68e52d | 13 | /* initialize peripherals */ |
chrigelburri | 0:31f7be68e52d | 14 | motorControllerLeft->enable(false); |
chrigelburri | 0:31f7be68e52d | 15 | motorControllerRight->enable(false); |
chrigelburri | 0:31f7be68e52d | 16 | |
chrigelburri | 0:31f7be68e52d | 17 | /* initialize remaining state values */ |
chrigelburri | 0:31f7be68e52d | 18 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 19 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 20 | |
chrigelburri | 0:31f7be68e52d | 21 | motorControllerLeft->setPulses(0); |
chrigelburri | 0:31f7be68e52d | 22 | motorControllerRight->setPulses(0); |
chrigelburri | 0:31f7be68e52d | 23 | |
chrigelburri | 0:31f7be68e52d | 24 | Desired.setAcceleration(ACCELERATION); |
chrigelburri | 0:31f7be68e52d | 25 | Desired.setThetaAcceleration(THETA_ACCELERATION); |
chrigelburri | 0:31f7be68e52d | 26 | |
chrigelburri | 0:31f7be68e52d | 27 | } |
chrigelburri | 0:31f7be68e52d | 28 | |
chrigelburri | 0:31f7be68e52d | 29 | RobotControl::~RobotControl() |
chrigelburri | 0:31f7be68e52d | 30 | { |
chrigelburri | 0:31f7be68e52d | 31 | |
chrigelburri | 0:31f7be68e52d | 32 | } |
chrigelburri | 0:31f7be68e52d | 33 | |
chrigelburri | 0:31f7be68e52d | 34 | void RobotControl::setEnable(bool enable) |
chrigelburri | 0:31f7be68e52d | 35 | { |
chrigelburri | 0:31f7be68e52d | 36 | motorControllerLeft->enable(enable); |
chrigelburri | 0:31f7be68e52d | 37 | motorControllerRight->enable(enable); |
chrigelburri | 0:31f7be68e52d | 38 | } |
chrigelburri | 0:31f7be68e52d | 39 | |
chrigelburri | 0:31f7be68e52d | 40 | bool RobotControl::isEnabled() |
chrigelburri | 0:31f7be68e52d | 41 | { |
chrigelburri | 0:31f7be68e52d | 42 | return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled()); |
chrigelburri | 0:31f7be68e52d | 43 | } |
chrigelburri | 0:31f7be68e52d | 44 | |
chrigelburri | 0:31f7be68e52d | 45 | void RobotControl::setAcceleration(float acc) |
chrigelburri | 0:31f7be68e52d | 46 | { |
chrigelburri | 0:31f7be68e52d | 47 | Desired.setAcceleration(acc); |
chrigelburri | 0:31f7be68e52d | 48 | |
chrigelburri | 0:31f7be68e52d | 49 | } |
chrigelburri | 0:31f7be68e52d | 50 | |
chrigelburri | 0:31f7be68e52d | 51 | void RobotControl::setThetaAcceleration(float acc) |
chrigelburri | 0:31f7be68e52d | 52 | { |
chrigelburri | 0:31f7be68e52d | 53 | Desired.setThetaAcceleration(acc); |
chrigelburri | 0:31f7be68e52d | 54 | } |
chrigelburri | 0:31f7be68e52d | 55 | |
chrigelburri | 0:31f7be68e52d | 56 | void RobotControl::setDesiredSpeed(float speed) |
chrigelburri | 0:31f7be68e52d | 57 | { |
chrigelburri | 0:31f7be68e52d | 58 | this->speed = speed; |
chrigelburri | 0:31f7be68e52d | 59 | } |
chrigelburri | 0:31f7be68e52d | 60 | |
chrigelburri | 0:31f7be68e52d | 61 | void RobotControl::setDesiredOmega(float omega) |
chrigelburri | 0:31f7be68e52d | 62 | { |
chrigelburri | 0:31f7be68e52d | 63 | this->omega = omega; |
chrigelburri | 0:31f7be68e52d | 64 | } |
chrigelburri | 0:31f7be68e52d | 65 | |
chrigelburri | 0:31f7be68e52d | 66 | void RobotControl::setxPosition(float position) |
chrigelburri | 0:31f7be68e52d | 67 | { |
chrigelburri | 0:31f7be68e52d | 68 | Desired.xposition = position; |
chrigelburri | 0:31f7be68e52d | 69 | } |
chrigelburri | 0:31f7be68e52d | 70 | |
chrigelburri | 0:31f7be68e52d | 71 | void RobotControl::setyPosition(float position) |
chrigelburri | 0:31f7be68e52d | 72 | { |
chrigelburri | 0:31f7be68e52d | 73 | Desired.yposition = position; |
chrigelburri | 0:31f7be68e52d | 74 | } |
chrigelburri | 0:31f7be68e52d | 75 | |
chrigelburri | 0:31f7be68e52d | 76 | void RobotControl::setTheta(float theta) |
chrigelburri | 0:31f7be68e52d | 77 | { |
chrigelburri | 0:31f7be68e52d | 78 | Desired.theta = theta; |
chrigelburri | 0:31f7be68e52d | 79 | } |
chrigelburri | 0:31f7be68e52d | 80 | |
chrigelburri | 0:31f7be68e52d | 81 | float RobotControl::getDesiredSpeed() |
chrigelburri | 0:31f7be68e52d | 82 | { |
chrigelburri | 0:31f7be68e52d | 83 | return speed; |
chrigelburri | 0:31f7be68e52d | 84 | } |
chrigelburri | 0:31f7be68e52d | 85 | |
chrigelburri | 0:31f7be68e52d | 86 | float RobotControl::getActualSpeed() |
chrigelburri | 0:31f7be68e52d | 87 | { |
chrigelburri | 0:31f7be68e52d | 88 | return Actual.speed; |
chrigelburri | 0:31f7be68e52d | 89 | } |
chrigelburri | 0:31f7be68e52d | 90 | |
chrigelburri | 0:31f7be68e52d | 91 | float RobotControl::getDesiredOmega() |
chrigelburri | 0:31f7be68e52d | 92 | { |
chrigelburri | 0:31f7be68e52d | 93 | return omega; |
chrigelburri | 0:31f7be68e52d | 94 | } |
chrigelburri | 0:31f7be68e52d | 95 | |
chrigelburri | 0:31f7be68e52d | 96 | float RobotControl::getActualOmega() |
chrigelburri | 0:31f7be68e52d | 97 | { |
chrigelburri | 0:31f7be68e52d | 98 | return Actual.omega; |
chrigelburri | 0:31f7be68e52d | 99 | } |
chrigelburri | 0:31f7be68e52d | 100 | |
chrigelburri | 0:31f7be68e52d | 101 | float RobotControl::getxActualPosition() |
chrigelburri | 0:31f7be68e52d | 102 | { |
chrigelburri | 0:31f7be68e52d | 103 | return Actual.getxPosition(); |
chrigelburri | 0:31f7be68e52d | 104 | } |
chrigelburri | 0:31f7be68e52d | 105 | |
chrigelburri | 0:31f7be68e52d | 106 | float RobotControl::getxPositionError() |
chrigelburri | 0:31f7be68e52d | 107 | { |
chrigelburri | 0:31f7be68e52d | 108 | return Desired.getxPosition()-Actual.getxPosition(); |
chrigelburri | 0:31f7be68e52d | 109 | } |
chrigelburri | 0:31f7be68e52d | 110 | |
chrigelburri | 0:31f7be68e52d | 111 | float RobotControl::getyActualPosition() |
chrigelburri | 0:31f7be68e52d | 112 | { |
chrigelburri | 0:31f7be68e52d | 113 | return Actual.getyPosition(); |
chrigelburri | 0:31f7be68e52d | 114 | } |
chrigelburri | 0:31f7be68e52d | 115 | |
chrigelburri | 0:31f7be68e52d | 116 | float RobotControl::getyPositionError() |
chrigelburri | 0:31f7be68e52d | 117 | { |
chrigelburri | 0:31f7be68e52d | 118 | return Desired.getyPosition()-Actual.getyPosition(); |
chrigelburri | 0:31f7be68e52d | 119 | } |
chrigelburri | 0:31f7be68e52d | 120 | |
chrigelburri | 0:31f7be68e52d | 121 | float RobotControl::getActualTheta() |
chrigelburri | 0:31f7be68e52d | 122 | { |
chrigelburri | 0:31f7be68e52d | 123 | return Actual.getTheta(); |
chrigelburri | 0:31f7be68e52d | 124 | } |
chrigelburri | 0:31f7be68e52d | 125 | |
chrigelburri | 0:31f7be68e52d | 126 | float RobotControl::getThetaError() |
chrigelburri | 0:31f7be68e52d | 127 | { |
chrigelburri | 0:31f7be68e52d | 128 | return Desired.getTheta()-Actual.getTheta(); |
chrigelburri | 0:31f7be68e52d | 129 | } |
chrigelburri | 0:31f7be68e52d | 130 | |
chrigelburri | 0:31f7be68e52d | 131 | void RobotControl::setAllToZero(float xZeroPos, float yZeroPos) |
chrigelburri | 0:31f7be68e52d | 132 | { |
chrigelburri | 0:31f7be68e52d | 133 | Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 134 | Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 135 | stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 136 | stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 137 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 138 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 139 | } |
chrigelburri | 0:31f7be68e52d | 140 | |
chrigelburri | 0:31f7be68e52d | 141 | |
chrigelburri | 0:31f7be68e52d | 142 | void RobotControl::run() |
chrigelburri | 0:31f7be68e52d | 143 | { |
chrigelburri | 0:31f7be68e52d | 144 | /* motion planning */ |
chrigelburri | 0:31f7be68e52d | 145 | if (isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 146 | Desired.increment(speed, omega, period); |
chrigelburri | 0:31f7be68e52d | 147 | } else { |
chrigelburri | 0:31f7be68e52d | 148 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 149 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 150 | Desired.setState(&Actual); |
chrigelburri | 0:31f7be68e52d | 151 | } |
chrigelburri | 0:31f7be68e52d | 152 | |
chrigelburri | 0:31f7be68e52d | 153 | /* position calculation */ |
chrigelburri | 0:31f7be68e52d | 154 | |
chrigelburri | 0:31f7be68e52d | 155 | /* Set the state of speed from Left und Right Wheel*/ |
chrigelburri | 0:31f7be68e52d | 156 | stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI; |
chrigelburri | 0:31f7be68e52d | 157 | stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI; |
chrigelburri | 0:31f7be68e52d | 158 | |
chrigelburri | 0:31f7be68e52d | 159 | /* translational speed of the Robot (average) */ |
chrigelburri | 0:31f7be68e52d | 160 | Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f; |
chrigelburri | 0:31f7be68e52d | 161 | |
chrigelburri | 0:31f7be68e52d | 162 | /* rotational speed of the Robot */ |
chrigelburri | 0:31f7be68e52d | 163 | Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE; |
chrigelburri | 0:31f7be68e52d | 164 | |
chrigelburri | 0:31f7be68e52d | 165 | /* rotational theta of the Robot */ |
chrigelburri | 0:31f7be68e52d | 166 | Actual.theta += Actual.omega * period; |
chrigelburri | 0:31f7be68e52d | 167 | if(Actual.theta <= -PI) { |
chrigelburri | 0:31f7be68e52d | 168 | Actual.theta += 2* PI; |
chrigelburri | 0:31f7be68e52d | 169 | } else if (Actual.theta > PI) { |
chrigelburri | 0:31f7be68e52d | 170 | Actual.theta -= 2* PI; |
chrigelburri | 0:31f7be68e52d | 171 | } else { |
chrigelburri | 0:31f7be68e52d | 172 | //nothing |
chrigelburri | 0:31f7be68e52d | 173 | } |
chrigelburri | 0:31f7be68e52d | 174 | Actual.theta += Actual.omega * period; |
chrigelburri | 0:31f7be68e52d | 175 | Actual.thetaCompass = compass->getFilteredAngle(); |
chrigelburri | 0:31f7be68e52d | 176 | |
chrigelburri | 0:31f7be68e52d | 177 | /* translational X and Y Position. integrate the speed with the time */ |
chrigelburri | 0:31f7be68e52d | 178 | // Actual.xposition += (Actual.speed * period * sin(Actual.theta)); |
chrigelburri | 0:31f7be68e52d | 179 | // Actual.yposition += (Actual.speed * period * cos(Actual.theta)); |
chrigelburri | 0:31f7be68e52d | 180 | |
chrigelburri | 0:31f7be68e52d | 181 | /* translational X and Y Position. integrate the speed with the time theta from compass */ |
chrigelburri | 0:31f7be68e52d | 182 | Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass)); |
chrigelburri | 0:31f7be68e52d | 183 | Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass)); |
chrigelburri | 0:31f7be68e52d | 184 | |
chrigelburri | 0:31f7be68e52d | 185 | |
chrigelburri | 0:31f7be68e52d | 186 | |
chrigelburri | 0:31f7be68e52d | 187 | |
chrigelburri | 0:31f7be68e52d | 188 | /* postition control calculate */ |
chrigelburri | 0:31f7be68e52d | 189 | |
chrigelburri | 0:31f7be68e52d | 190 | rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2)); |
chrigelburri | 0:31f7be68e52d | 191 | |
chrigelburri | 0:31f7be68e52d | 192 | |
chrigelburri | 0:31f7be68e52d | 193 | |
chrigelburri | 0:31f7be68e52d | 194 | |
chrigelburri | 0:31f7be68e52d | 195 | |
chrigelburri | 0:31f7be68e52d | 196 | /* motor control */ |
chrigelburri | 0:31f7be68e52d | 197 | if (isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 198 | |
chrigelburri | 0:31f7be68e52d | 199 | motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) ); |
chrigelburri | 0:31f7be68e52d | 200 | motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) ); |
chrigelburri | 0:31f7be68e52d | 201 | |
chrigelburri | 0:31f7be68e52d | 202 | |
chrigelburri | 0:31f7be68e52d | 203 | } else { |
chrigelburri | 0:31f7be68e52d | 204 | |
chrigelburri | 0:31f7be68e52d | 205 | motorControllerLeft->setVelocity(0.0f); |
chrigelburri | 0:31f7be68e52d | 206 | motorControllerRight->setVelocity(0.0f); |
chrigelburri | 0:31f7be68e52d | 207 | |
chrigelburri | 0:31f7be68e52d | 208 | } |
chrigelburri | 0:31f7be68e52d | 209 | } |
chrigelburri | 0:31f7be68e52d | 210 |