This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> 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: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
Fork of autonomousRobotAndroid by
RobotControl/RobotControl.cpp@14:6a45a9f940a8, 2013-04-11 (annotated)
- Committer:
- chrigelburri
- Date:
- Thu Apr 11 09:22:35 2013 +0000
- Revision:
- 14:6a45a9f940a8
- Parent:
- 12:235e318a414f
- Child:
- 37:fd68b9e0be08
android 2.0 implemented (untested!)
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 | 6:48eeb41188dd | 5 | RobotControl::RobotControl(MaxonESCON* motorControllerLeft, |
chrigelburri | 6:48eeb41188dd | 6 | MaxonESCON* motorControllerRight, |
chrigelburri | 6:48eeb41188dd | 7 | float period) : Task(period) |
chrigelburri | 0:31f7be68e52d | 8 | { |
chrigelburri | 0:31f7be68e52d | 9 | /* get peripherals */ |
chrigelburri | 0:31f7be68e52d | 10 | this->motorControllerLeft = motorControllerLeft; |
chrigelburri | 0:31f7be68e52d | 11 | this->motorControllerRight = motorControllerRight; |
chrigelburri | 0:31f7be68e52d | 12 | this->period = period; |
chrigelburri | 0:31f7be68e52d | 13 | |
chrigelburri | 0:31f7be68e52d | 14 | /* initialize peripherals */ |
chrigelburri | 0:31f7be68e52d | 15 | motorControllerLeft->enable(false); |
chrigelburri | 0:31f7be68e52d | 16 | motorControllerRight->enable(false); |
chrigelburri | 0:31f7be68e52d | 17 | |
chrigelburri | 0:31f7be68e52d | 18 | /* initialize remaining state values */ |
chrigelburri | 0:31f7be68e52d | 19 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 20 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 21 | |
chrigelburri | 0:31f7be68e52d | 22 | motorControllerLeft->setPulses(0); |
chrigelburri | 0:31f7be68e52d | 23 | motorControllerRight->setPulses(0); |
chrigelburri | 0:31f7be68e52d | 24 | |
chrigelburri | 8:696c2f9dfc62 | 25 | motorControllerLeft->setVelocity(0.0f); |
chrigelburri | 8:696c2f9dfc62 | 26 | motorControllerRight->setVelocity(0.0f); |
chrigelburri | 0:31f7be68e52d | 27 | } |
chrigelburri | 0:31f7be68e52d | 28 | |
chrigelburri | 1:6cd533a712c6 | 29 | RobotControl::~RobotControl() |
chrigelburri | 0:31f7be68e52d | 30 | { |
chrigelburri | 0:31f7be68e52d | 31 | |
chrigelburri | 0:31f7be68e52d | 32 | } |
chrigelburri | 0:31f7be68e52d | 33 | |
chrigelburri | 12:235e318a414f | 34 | float RobotControl::PiRange(float theta) |
chrigelburri | 12:235e318a414f | 35 | { |
chrigelburri | 12:235e318a414f | 36 | if(theta <= -PI) { |
chrigelburri | 12:235e318a414f | 37 | return theta += 2*PI; |
chrigelburri | 12:235e318a414f | 38 | } else if (theta > PI) { |
chrigelburri | 12:235e318a414f | 39 | return theta -= 2*PI; |
chrigelburri | 12:235e318a414f | 40 | } else { |
chrigelburri | 12:235e318a414f | 41 | return theta; |
chrigelburri | 12:235e318a414f | 42 | } |
chrigelburri | 12:235e318a414f | 43 | } |
chrigelburri | 12:235e318a414f | 44 | |
chrigelburri | 0:31f7be68e52d | 45 | void RobotControl::setEnable(bool enable) |
chrigelburri | 0:31f7be68e52d | 46 | { |
chrigelburri | 0:31f7be68e52d | 47 | motorControllerLeft->enable(enable); |
chrigelburri | 0:31f7be68e52d | 48 | motorControllerRight->enable(enable); |
chrigelburri | 0:31f7be68e52d | 49 | } |
chrigelburri | 0:31f7be68e52d | 50 | |
chrigelburri | 0:31f7be68e52d | 51 | bool RobotControl::isEnabled() |
chrigelburri | 0:31f7be68e52d | 52 | { |
chrigelburri | 0:31f7be68e52d | 53 | return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled()); |
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 | 5:48a258f6335e | 66 | void RobotControl::setDesiredxPosition(float xposition) |
chrigelburri | 0:31f7be68e52d | 67 | { |
chrigelburri | 1:6cd533a712c6 | 68 | Desired.xposition = xposition; |
chrigelburri | 0:31f7be68e52d | 69 | } |
chrigelburri | 0:31f7be68e52d | 70 | |
chrigelburri | 5:48a258f6335e | 71 | void RobotControl::setDesiredyPosition(float yposition) |
chrigelburri | 0:31f7be68e52d | 72 | { |
chrigelburri | 1:6cd533a712c6 | 73 | Desired.yposition = yposition; |
chrigelburri | 0:31f7be68e52d | 74 | } |
chrigelburri | 0:31f7be68e52d | 75 | |
chrigelburri | 5:48a258f6335e | 76 | void RobotControl::setDesiredTheta(float theta) |
chrigelburri | 0:31f7be68e52d | 77 | { |
chrigelburri | 0:31f7be68e52d | 78 | Desired.theta = theta; |
chrigelburri | 0:31f7be68e52d | 79 | } |
chrigelburri | 0:31f7be68e52d | 80 | |
chrigelburri | 5:48a258f6335e | 81 | float RobotControl::getDesiredxPosition() |
chrigelburri | 5:48a258f6335e | 82 | { |
chrigelburri | 5:48a258f6335e | 83 | return Desired.xposition; |
chrigelburri | 5:48a258f6335e | 84 | } |
chrigelburri | 5:48a258f6335e | 85 | |
chrigelburri | 5:48a258f6335e | 86 | float RobotControl::getDesiredyPosition() |
chrigelburri | 2:d8e1613dc38b | 87 | { |
chrigelburri | 5:48a258f6335e | 88 | return Desired.yposition; |
chrigelburri | 5:48a258f6335e | 89 | } |
chrigelburri | 5:48a258f6335e | 90 | |
chrigelburri | 5:48a258f6335e | 91 | float RobotControl::getDesiredTheta() |
chrigelburri | 5:48a258f6335e | 92 | { |
chrigelburri | 5:48a258f6335e | 93 | return Desired.theta; |
chrigelburri | 5:48a258f6335e | 94 | } |
chrigelburri | 5:48a258f6335e | 95 | |
chrigelburri | 6:48eeb41188dd | 96 | void RobotControl::setDesiredPositionAndAngle(float xposition, |
chrigelburri | 6:48eeb41188dd | 97 | float yposition, |
chrigelburri | 6:48eeb41188dd | 98 | float theta) |
chrigelburri | 5:48a258f6335e | 99 | { |
chrigelburri | 5:48a258f6335e | 100 | setDesiredxPosition(xposition); |
chrigelburri | 5:48a258f6335e | 101 | setDesiredyPosition(yposition); |
chrigelburri | 5:48a258f6335e | 102 | setDesiredTheta(theta); |
chrigelburri | 2:d8e1613dc38b | 103 | } |
chrigelburri | 2:d8e1613dc38b | 104 | |
chrigelburri | 1:6cd533a712c6 | 105 | float RobotControl::getTheta() |
chrigelburri | 1:6cd533a712c6 | 106 | { |
chrigelburri | 1:6cd533a712c6 | 107 | return Desired.theta; |
chrigelburri | 1:6cd533a712c6 | 108 | } |
chrigelburri | 1:6cd533a712c6 | 109 | |
chrigelburri | 0:31f7be68e52d | 110 | float RobotControl::getDesiredSpeed() |
chrigelburri | 0:31f7be68e52d | 111 | { |
chrigelburri | 0:31f7be68e52d | 112 | return speed; |
chrigelburri | 0:31f7be68e52d | 113 | } |
chrigelburri | 0:31f7be68e52d | 114 | |
chrigelburri | 0:31f7be68e52d | 115 | float RobotControl::getActualSpeed() |
chrigelburri | 0:31f7be68e52d | 116 | { |
chrigelburri | 0:31f7be68e52d | 117 | return Actual.speed; |
chrigelburri | 0:31f7be68e52d | 118 | } |
chrigelburri | 0:31f7be68e52d | 119 | |
chrigelburri | 0:31f7be68e52d | 120 | float RobotControl::getDesiredOmega() |
chrigelburri | 0:31f7be68e52d | 121 | { |
chrigelburri | 0:31f7be68e52d | 122 | return omega; |
chrigelburri | 0:31f7be68e52d | 123 | } |
chrigelburri | 0:31f7be68e52d | 124 | |
chrigelburri | 0:31f7be68e52d | 125 | float RobotControl::getActualOmega() |
chrigelburri | 0:31f7be68e52d | 126 | { |
chrigelburri | 0:31f7be68e52d | 127 | return Actual.omega; |
chrigelburri | 0:31f7be68e52d | 128 | } |
chrigelburri | 0:31f7be68e52d | 129 | |
chrigelburri | 0:31f7be68e52d | 130 | float RobotControl::getxActualPosition() |
chrigelburri | 0:31f7be68e52d | 131 | { |
chrigelburri | 0:31f7be68e52d | 132 | return Actual.getxPosition(); |
chrigelburri | 0:31f7be68e52d | 133 | } |
chrigelburri | 0:31f7be68e52d | 134 | |
chrigelburri | 0:31f7be68e52d | 135 | float RobotControl::getxPositionError() |
chrigelburri | 0:31f7be68e52d | 136 | { |
chrigelburri | 0:31f7be68e52d | 137 | return Desired.getxPosition()-Actual.getxPosition(); |
chrigelburri | 0:31f7be68e52d | 138 | } |
chrigelburri | 0:31f7be68e52d | 139 | |
chrigelburri | 0:31f7be68e52d | 140 | float RobotControl::getyActualPosition() |
chrigelburri | 0:31f7be68e52d | 141 | { |
chrigelburri | 0:31f7be68e52d | 142 | return Actual.getyPosition(); |
chrigelburri | 0:31f7be68e52d | 143 | } |
chrigelburri | 0:31f7be68e52d | 144 | |
chrigelburri | 0:31f7be68e52d | 145 | float RobotControl::getyPositionError() |
chrigelburri | 0:31f7be68e52d | 146 | { |
chrigelburri | 0:31f7be68e52d | 147 | return Desired.getyPosition()-Actual.getyPosition(); |
chrigelburri | 0:31f7be68e52d | 148 | } |
chrigelburri | 0:31f7be68e52d | 149 | |
chrigelburri | 0:31f7be68e52d | 150 | float RobotControl::getActualTheta() |
chrigelburri | 0:31f7be68e52d | 151 | { |
chrigelburri | 0:31f7be68e52d | 152 | return Actual.getTheta(); |
chrigelburri | 0:31f7be68e52d | 153 | } |
chrigelburri | 0:31f7be68e52d | 154 | |
chrigelburri | 0:31f7be68e52d | 155 | float RobotControl::getThetaError() |
chrigelburri | 0:31f7be68e52d | 156 | { |
chrigelburri | 0:31f7be68e52d | 157 | return Desired.getTheta()-Actual.getTheta(); |
chrigelburri | 0:31f7be68e52d | 158 | } |
chrigelburri | 0:31f7be68e52d | 159 | |
chrigelburri | 1:6cd533a712c6 | 160 | float RobotControl::getDistanceError() |
chrigelburri | 1:6cd533a712c6 | 161 | { |
chrigelburri | 14:6a45a9f940a8 | 162 | return sqrt( ( getxPositionError() * getxPositionError() ) + ( getyPositionError() * getyPositionError() ) ); |
chrigelburri | 1:6cd533a712c6 | 163 | } |
chrigelburri | 1:6cd533a712c6 | 164 | |
chrigelburri | 1:6cd533a712c6 | 165 | float RobotControl::getThetaErrorToGoal() |
chrigelburri | 0:31f7be68e52d | 166 | { |
chrigelburri | 3:92ba0254af87 | 167 | return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta()); |
chrigelburri | 1:6cd533a712c6 | 168 | } |
chrigelburri | 1:6cd533a712c6 | 169 | |
chrigelburri | 1:6cd533a712c6 | 170 | float RobotControl::getThetaGoal() |
chrigelburri | 1:6cd533a712c6 | 171 | { |
chrigelburri | 3:92ba0254af87 | 172 | return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta()); |
chrigelburri | 1:6cd533a712c6 | 173 | } |
chrigelburri | 1:6cd533a712c6 | 174 | |
chrigelburri | 6:48eeb41188dd | 175 | void RobotControl::setAllToZero(float xZeroPos, |
chrigelburri | 6:48eeb41188dd | 176 | float yZeroPos, |
chrigelburri | 6:48eeb41188dd | 177 | float theta) |
chrigelburri | 1:6cd533a712c6 | 178 | { |
chrigelburri | 1:6cd533a712c6 | 179 | Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 180 | Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 181 | stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 182 | stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
chrigelburri | 0:31f7be68e52d | 183 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 184 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 185 | } |
chrigelburri | 0:31f7be68e52d | 186 | |
chrigelburri | 0:31f7be68e52d | 187 | void RobotControl::run() |
chrigelburri | 0:31f7be68e52d | 188 | { |
chrigelburri | 12:235e318a414f | 189 | // When the Motor is note enable set the desired speed to the acutal speed. |
chrigelburri | 12:235e318a414f | 190 | // only used by setting the speed and omega via the WII control. |
chrigelburri | 12:235e318a414f | 191 | if (!isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 192 | speed = 0.0f; |
chrigelburri | 0:31f7be68e52d | 193 | omega = 0.0f; |
chrigelburri | 0:31f7be68e52d | 194 | Desired.setState(&Actual); |
chrigelburri | 0:31f7be68e52d | 195 | } |
chrigelburri | 0:31f7be68e52d | 196 | |
chrigelburri | 12:235e318a414f | 197 | // position calculation |
chrigelburri | 12:235e318a414f | 198 | // Set the state of speed from Left und Right Wheel |
chrigelburri | 6:48eeb41188dd | 199 | stateLeft.speed = motorControllerLeft->getActualSpeed() * |
chrigelburri | 6:48eeb41188dd | 200 | 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR; |
chrigelburri | 6:48eeb41188dd | 201 | stateRight.speed = - motorControllerRight->getActualSpeed() * |
chrigelburri | 8:696c2f9dfc62 | 202 | 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ; |
chrigelburri | 0:31f7be68e52d | 203 | |
chrigelburri | 12:235e318a414f | 204 | //translational speed of the Robot (average) |
chrigelburri | 8:696c2f9dfc62 | 205 | Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f; |
chrigelburri | 0:31f7be68e52d | 206 | |
chrigelburri | 12:235e318a414f | 207 | //rotational speed of the Robot |
chrigelburri | 8:696c2f9dfc62 | 208 | Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE; |
chrigelburri | 0:31f7be68e52d | 209 | |
chrigelburri | 12:235e318a414f | 210 | //rotational theta of the Robot integrate the omega with the time |
chrigelburri | 0:31f7be68e52d | 211 | Actual.theta += Actual.omega * period; |
chrigelburri | 3:92ba0254af87 | 212 | Actual.theta = PiRange(Actual.theta); |
chrigelburri | 6:48eeb41188dd | 213 | |
chrigelburri | 12:235e318a414f | 214 | //translational X and Y Position. integrate the speed with the time |
chrigelburri | 1:6cd533a712c6 | 215 | Actual.xposition += (Actual.speed * period * cos(Actual.theta)); |
chrigelburri | 1:6cd533a712c6 | 216 | Actual.yposition += (Actual.speed * period * sin(Actual.theta)); |
chrigelburri | 0:31f7be68e52d | 217 | |
chrigelburri | 12:235e318a414f | 218 | //motor control |
chrigelburri | 1:6cd533a712c6 | 219 | if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) { |
chrigelburri | 0:31f7be68e52d | 220 | |
chrigelburri | 12:235e318a414f | 221 | //postition control |
chrigelburri | 1:6cd533a712c6 | 222 | speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() ); |
chrigelburri | 2:d8e1613dc38b | 223 | omega = K2 * getThetaErrorToGoal() + |
chrigelburri | 12:235e318a414f | 224 | K1 * |
chrigelburri | 12:235e318a414f | 225 | ( |
chrigelburri | 12:235e318a414f | 226 | ( |
chrigelburri | 12:235e318a414f | 227 | sin(getThetaErrorToGoal() ) * cos(getThetaErrorToGoal() ) |
chrigelburri | 12:235e318a414f | 228 | ) / getThetaErrorToGoal() |
chrigelburri | 12:235e318a414f | 229 | ) * |
chrigelburri | 12:235e318a414f | 230 | ( getThetaErrorToGoal() |
chrigelburri | 12:235e318a414f | 231 | + K3 * getThetaGoal() ); |
chrigelburri | 0:31f7be68e52d | 232 | |
chrigelburri | 12:235e318a414f | 233 | motorControllerLeft->setVelocity( |
chrigelburri | 12:235e318a414f | 234 | ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / |
chrigelburri | 12:235e318a414f | 235 | (2 * WHEEL_RADIUS_LEFT * PI * GEAR) |
chrigelburri | 12:235e318a414f | 236 | ); |
chrigelburri | 12:235e318a414f | 237 | motorControllerRight->setVelocity( |
chrigelburri | 12:235e318a414f | 238 | -( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / |
chrigelburri | 12:235e318a414f | 239 | (2 * WHEEL_RADIUS_RIGHT * PI * GEAR) |
chrigelburri | 12:235e318a414f | 240 | ); |
chrigelburri | 0:31f7be68e52d | 241 | |
chrigelburri | 0:31f7be68e52d | 242 | } else { |
chrigelburri | 0:31f7be68e52d | 243 | motorControllerLeft->setVelocity(0.0f); |
chrigelburri | 0:31f7be68e52d | 244 | motorControllerRight->setVelocity(0.0f); |
chrigelburri | 1:6cd533a712c6 | 245 | } |
chrigelburri | 0:31f7be68e52d | 246 | } |