Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

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>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

RobotControl/RobotControl.cpp@6:48eeb41188dd, 2013-03-23 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 23 13:52:48 2013 +0000
Revision:
6:48eeb41188dd
Parent:
5:48a258f6335e
Child:
8:696c2f9dfc62
mit link und rechten Radradius

Who changed what in which revision?

UserRevisionLine numberNew 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 /*HMC6352* compass,*/
chrigelburri 6:48eeb41188dd 8 float period) : Task(period)
chrigelburri 0:31f7be68e52d 9 {
chrigelburri 0:31f7be68e52d 10 /* get peripherals */
chrigelburri 0:31f7be68e52d 11 this->motorControllerLeft = motorControllerLeft;
chrigelburri 0:31f7be68e52d 12 this->motorControllerRight = motorControllerRight;
chrigelburri 1:6cd533a712c6 13 // this->compass = compass;
chrigelburri 0:31f7be68e52d 14 this->period = period;
chrigelburri 0:31f7be68e52d 15
chrigelburri 0:31f7be68e52d 16 /* initialize peripherals */
chrigelburri 0:31f7be68e52d 17 motorControllerLeft->enable(false);
chrigelburri 0:31f7be68e52d 18 motorControllerRight->enable(false);
chrigelburri 0:31f7be68e52d 19
chrigelburri 0:31f7be68e52d 20 /* initialize remaining state values */
chrigelburri 0:31f7be68e52d 21 speed = 0.0f;
chrigelburri 0:31f7be68e52d 22 omega = 0.0f;
chrigelburri 0:31f7be68e52d 23
chrigelburri 0:31f7be68e52d 24 motorControllerLeft->setPulses(0);
chrigelburri 0:31f7be68e52d 25 motorControllerRight->setPulses(0);
chrigelburri 0:31f7be68e52d 26
chrigelburri 0:31f7be68e52d 27 Desired.setAcceleration(ACCELERATION);
chrigelburri 0:31f7be68e52d 28 Desired.setThetaAcceleration(THETA_ACCELERATION);
chrigelburri 0:31f7be68e52d 29 }
chrigelburri 0:31f7be68e52d 30
chrigelburri 1:6cd533a712c6 31 RobotControl::~RobotControl()
chrigelburri 0:31f7be68e52d 32 {
chrigelburri 0:31f7be68e52d 33
chrigelburri 0:31f7be68e52d 34 }
chrigelburri 0:31f7be68e52d 35
chrigelburri 0:31f7be68e52d 36 void RobotControl::setEnable(bool enable)
chrigelburri 0:31f7be68e52d 37 {
chrigelburri 0:31f7be68e52d 38 motorControllerLeft->enable(enable);
chrigelburri 0:31f7be68e52d 39 motorControllerRight->enable(enable);
chrigelburri 0:31f7be68e52d 40 }
chrigelburri 0:31f7be68e52d 41
chrigelburri 0:31f7be68e52d 42 bool RobotControl::isEnabled()
chrigelburri 0:31f7be68e52d 43 {
chrigelburri 0:31f7be68e52d 44 return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
chrigelburri 0:31f7be68e52d 45 }
chrigelburri 0:31f7be68e52d 46
chrigelburri 1:6cd533a712c6 47 void RobotControl::setAcceleration(float acceleration)
chrigelburri 0:31f7be68e52d 48 {
chrigelburri 1:6cd533a712c6 49 Desired.setAcceleration(acceleration);
chrigelburri 0:31f7be68e52d 50 }
chrigelburri 0:31f7be68e52d 51
chrigelburri 1:6cd533a712c6 52 void RobotControl::setThetaAcceleration(float acceleration)
chrigelburri 0:31f7be68e52d 53 {
chrigelburri 1:6cd533a712c6 54 Desired.setThetaAcceleration(acceleration);
chrigelburri 0:31f7be68e52d 55 }
chrigelburri 0:31f7be68e52d 56
chrigelburri 0:31f7be68e52d 57 void RobotControl::setDesiredSpeed(float speed)
chrigelburri 0:31f7be68e52d 58 {
chrigelburri 0:31f7be68e52d 59 this->speed = speed;
chrigelburri 0:31f7be68e52d 60 }
chrigelburri 0:31f7be68e52d 61
chrigelburri 0:31f7be68e52d 62 void RobotControl::setDesiredOmega(float omega)
chrigelburri 0:31f7be68e52d 63 {
chrigelburri 0:31f7be68e52d 64 this->omega = omega;
chrigelburri 0:31f7be68e52d 65 }
chrigelburri 0:31f7be68e52d 66
chrigelburri 5:48a258f6335e 67 void RobotControl::setDesiredxPosition(float xposition)
chrigelburri 0:31f7be68e52d 68 {
chrigelburri 1:6cd533a712c6 69 Desired.xposition = xposition;
chrigelburri 0:31f7be68e52d 70 }
chrigelburri 0:31f7be68e52d 71
chrigelburri 5:48a258f6335e 72 void RobotControl::setDesiredyPosition(float yposition)
chrigelburri 0:31f7be68e52d 73 {
chrigelburri 1:6cd533a712c6 74 Desired.yposition = yposition;
chrigelburri 0:31f7be68e52d 75 }
chrigelburri 0:31f7be68e52d 76
chrigelburri 5:48a258f6335e 77 void RobotControl::setDesiredTheta(float theta)
chrigelburri 0:31f7be68e52d 78 {
chrigelburri 0:31f7be68e52d 79 Desired.theta = theta;
chrigelburri 0:31f7be68e52d 80 }
chrigelburri 0:31f7be68e52d 81
chrigelburri 5:48a258f6335e 82 float RobotControl::getDesiredxPosition()
chrigelburri 5:48a258f6335e 83 {
chrigelburri 5:48a258f6335e 84 return Desired.xposition;
chrigelburri 5:48a258f6335e 85 }
chrigelburri 5:48a258f6335e 86
chrigelburri 5:48a258f6335e 87 float RobotControl::getDesiredyPosition()
chrigelburri 2:d8e1613dc38b 88 {
chrigelburri 5:48a258f6335e 89 return Desired.yposition;
chrigelburri 5:48a258f6335e 90 }
chrigelburri 5:48a258f6335e 91
chrigelburri 5:48a258f6335e 92 float RobotControl::getDesiredTheta()
chrigelburri 5:48a258f6335e 93 {
chrigelburri 5:48a258f6335e 94 return Desired.theta;
chrigelburri 5:48a258f6335e 95 }
chrigelburri 5:48a258f6335e 96
chrigelburri 6:48eeb41188dd 97 void RobotControl::setDesiredPositionAndAngle(float xposition,
chrigelburri 6:48eeb41188dd 98 float yposition,
chrigelburri 6:48eeb41188dd 99 float theta)
chrigelburri 5:48a258f6335e 100 {
chrigelburri 5:48a258f6335e 101 setDesiredxPosition(xposition);
chrigelburri 5:48a258f6335e 102 setDesiredyPosition(yposition);
chrigelburri 5:48a258f6335e 103 setDesiredTheta(theta);
chrigelburri 2:d8e1613dc38b 104 }
chrigelburri 2:d8e1613dc38b 105
chrigelburri 1:6cd533a712c6 106 float RobotControl::getTheta()
chrigelburri 1:6cd533a712c6 107 {
chrigelburri 1:6cd533a712c6 108 return Desired.theta;
chrigelburri 1:6cd533a712c6 109 }
chrigelburri 1:6cd533a712c6 110
chrigelburri 0:31f7be68e52d 111 float RobotControl::getDesiredSpeed()
chrigelburri 0:31f7be68e52d 112 {
chrigelburri 0:31f7be68e52d 113 return speed;
chrigelburri 0:31f7be68e52d 114 }
chrigelburri 0:31f7be68e52d 115
chrigelburri 0:31f7be68e52d 116 float RobotControl::getActualSpeed()
chrigelburri 0:31f7be68e52d 117 {
chrigelburri 0:31f7be68e52d 118 return Actual.speed;
chrigelburri 0:31f7be68e52d 119 }
chrigelburri 0:31f7be68e52d 120
chrigelburri 0:31f7be68e52d 121 float RobotControl::getDesiredOmega()
chrigelburri 0:31f7be68e52d 122 {
chrigelburri 0:31f7be68e52d 123 return omega;
chrigelburri 0:31f7be68e52d 124 }
chrigelburri 0:31f7be68e52d 125
chrigelburri 0:31f7be68e52d 126 float RobotControl::getActualOmega()
chrigelburri 0:31f7be68e52d 127 {
chrigelburri 0:31f7be68e52d 128 return Actual.omega;
chrigelburri 0:31f7be68e52d 129 }
chrigelburri 0:31f7be68e52d 130
chrigelburri 0:31f7be68e52d 131 float RobotControl::getxActualPosition()
chrigelburri 0:31f7be68e52d 132 {
chrigelburri 0:31f7be68e52d 133 return Actual.getxPosition();
chrigelburri 0:31f7be68e52d 134 }
chrigelburri 0:31f7be68e52d 135
chrigelburri 0:31f7be68e52d 136 float RobotControl::getxPositionError()
chrigelburri 0:31f7be68e52d 137 {
chrigelburri 0:31f7be68e52d 138 return Desired.getxPosition()-Actual.getxPosition();
chrigelburri 0:31f7be68e52d 139 }
chrigelburri 0:31f7be68e52d 140
chrigelburri 0:31f7be68e52d 141 float RobotControl::getyActualPosition()
chrigelburri 0:31f7be68e52d 142 {
chrigelburri 0:31f7be68e52d 143 return Actual.getyPosition();
chrigelburri 0:31f7be68e52d 144 }
chrigelburri 0:31f7be68e52d 145
chrigelburri 0:31f7be68e52d 146 float RobotControl::getyPositionError()
chrigelburri 0:31f7be68e52d 147 {
chrigelburri 0:31f7be68e52d 148 return Desired.getyPosition()-Actual.getyPosition();
chrigelburri 0:31f7be68e52d 149 }
chrigelburri 0:31f7be68e52d 150
chrigelburri 0:31f7be68e52d 151 float RobotControl::getActualTheta()
chrigelburri 0:31f7be68e52d 152 {
chrigelburri 0:31f7be68e52d 153 return Actual.getTheta();
chrigelburri 0:31f7be68e52d 154 }
chrigelburri 0:31f7be68e52d 155
chrigelburri 0:31f7be68e52d 156 float RobotControl::getThetaError()
chrigelburri 0:31f7be68e52d 157 {
chrigelburri 0:31f7be68e52d 158 return Desired.getTheta()-Actual.getTheta();
chrigelburri 0:31f7be68e52d 159 }
chrigelburri 0:31f7be68e52d 160
chrigelburri 1:6cd533a712c6 161 float RobotControl::getDistanceError()
chrigelburri 1:6cd533a712c6 162 {
chrigelburri 1:6cd533a712c6 163 return sqrt( ( getxPositionError() * getxPositionError() ) + (getyPositionError() * getyPositionError() ) );
chrigelburri 1:6cd533a712c6 164 }
chrigelburri 1:6cd533a712c6 165
chrigelburri 1:6cd533a712c6 166 float RobotControl::getThetaErrorToGoal()
chrigelburri 0:31f7be68e52d 167 {
chrigelburri 3:92ba0254af87 168 return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
chrigelburri 1:6cd533a712c6 169 }
chrigelburri 1:6cd533a712c6 170
chrigelburri 1:6cd533a712c6 171 float RobotControl::getThetaGoal()
chrigelburri 1:6cd533a712c6 172 {
chrigelburri 3:92ba0254af87 173 return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
chrigelburri 1:6cd533a712c6 174 }
chrigelburri 1:6cd533a712c6 175
chrigelburri 6:48eeb41188dd 176 void RobotControl::setAllToZero(float xZeroPos,
chrigelburri 6:48eeb41188dd 177 float yZeroPos,
chrigelburri 6:48eeb41188dd 178 float theta)
chrigelburri 1:6cd533a712c6 179 {
chrigelburri 1:6cd533a712c6 180 Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 181 Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 182 stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 183 stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 184 speed = 0.0f;
chrigelburri 0:31f7be68e52d 185 omega = 0.0f;
chrigelburri 0:31f7be68e52d 186 }
chrigelburri 0:31f7be68e52d 187
chrigelburri 0:31f7be68e52d 188 void RobotControl::run()
chrigelburri 0:31f7be68e52d 189 {
chrigelburri 1:6cd533a712c6 190 ///// DAs kan glaub raus ab hier
chrigelburri 2:d8e1613dc38b 191 /////////////////////////////////////////////////////////7
chrigelburri 0:31f7be68e52d 192 /* motion planning */
chrigelburri 0:31f7be68e52d 193 if (isEnabled()) {
chrigelburri 1:6cd533a712c6 194 ///// DAs kan glaub raus bis hier
chrigelburri 0:31f7be68e52d 195 Desired.increment(speed, omega, period);
chrigelburri 1:6cd533a712c6 196
chrigelburri 2:d8e1613dc38b 197 ///// DAs kan glaub raus bis hier
chrigelburri 1:6cd533a712c6 198
chrigelburri 0:31f7be68e52d 199 } else {
chrigelburri 0:31f7be68e52d 200 speed = 0.0f;
chrigelburri 0:31f7be68e52d 201 omega = 0.0f;
chrigelburri 0:31f7be68e52d 202 Desired.setState(&Actual);
chrigelburri 0:31f7be68e52d 203 }
chrigelburri 0:31f7be68e52d 204
chrigelburri 0:31f7be68e52d 205 /* position calculation */
chrigelburri 0:31f7be68e52d 206
chrigelburri 0:31f7be68e52d 207 /* Set the state of speed from Left und Right Wheel*/
chrigelburri 6:48eeb41188dd 208 stateLeft.speed = motorControllerLeft->getActualSpeed() *
chrigelburri 6:48eeb41188dd 209 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
chrigelburri 6:48eeb41188dd 210 stateRight.speed = - motorControllerRight->getActualSpeed() *
chrigelburri 6:48eeb41188dd 211 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
chrigelburri 0:31f7be68e52d 212
chrigelburri 0:31f7be68e52d 213 /* translational speed of the Robot (average) */
chrigelburri 0:31f7be68e52d 214 Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
chrigelburri 0:31f7be68e52d 215
chrigelburri 0:31f7be68e52d 216 /* rotational speed of the Robot */
chrigelburri 0:31f7be68e52d 217 Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE;
chrigelburri 0:31f7be68e52d 218
chrigelburri 1:6cd533a712c6 219 /* rotational theta of the Robot integrate the omega with the time*/
chrigelburri 0:31f7be68e52d 220 Actual.theta += Actual.omega * period;
chrigelburri 3:92ba0254af87 221 Actual.theta = PiRange(Actual.theta);
chrigelburri 6:48eeb41188dd 222
chrigelburri 1:6cd533a712c6 223 /* translational X and Y Position. integrate the speed with the time */
chrigelburri 1:6cd533a712c6 224 Actual.xposition += (Actual.speed * period * cos(Actual.theta));
chrigelburri 1:6cd533a712c6 225 Actual.yposition += (Actual.speed * period * sin(Actual.theta));
chrigelburri 0:31f7be68e52d 226
chrigelburri 1:6cd533a712c6 227 // Actual.thetaCompass = compass->getFilteredAngle();
chrigelburri 1:6cd533a712c6 228 /* translational X and Y Position. integrate the speed with the time theta from compass */
chrigelburri 1:6cd533a712c6 229 // Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass));
chrigelburri 1:6cd533a712c6 230 // Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass));
chrigelburri 2:d8e1613dc38b 231
chrigelburri 1:6cd533a712c6 232 /* motor control */
chrigelburri 1:6cd533a712c6 233 if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) {
chrigelburri 0:31f7be68e52d 234
chrigelburri 1:6cd533a712c6 235 /* postition control */
chrigelburri 2:d8e1613dc38b 236
chrigelburri 1:6cd533a712c6 237 speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
chrigelburri 2:d8e1613dc38b 238 omega = K2 * getThetaErrorToGoal() +
chrigelburri 6:48eeb41188dd 239 K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) /
chrigelburri 6:48eeb41188dd 240 (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
chrigelburri 0:31f7be68e52d 241
chrigelburri 6:48eeb41188dd 242 motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) /
chrigelburri 6:48eeb41188dd 243 (2 * WHEEL_RADIUS_LEFT * PI * GEAR) );
chrigelburri 6:48eeb41188dd 244 motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) /
chrigelburri 6:48eeb41188dd 245 (2 * WHEEL_RADIUS_RIGHT * PI * GEAR) );
chrigelburri 0:31f7be68e52d 246
chrigelburri 0:31f7be68e52d 247 } else {
chrigelburri 0:31f7be68e52d 248
chrigelburri 0:31f7be68e52d 249 motorControllerLeft->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 250 motorControllerRight->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 251
chrigelburri 1:6cd533a712c6 252 }
chrigelburri 0:31f7be68e52d 253 }
chrigelburri 3:92ba0254af87 254
chrigelburri 3:92ba0254af87 255 float RobotControl::PiRange(float theta)
chrigelburri 3:92ba0254af87 256 {
chrigelburri 3:92ba0254af87 257 if(theta <= -PI) {
chrigelburri 3:92ba0254af87 258 return theta += 2*PI;
chrigelburri 3:92ba0254af87 259 } else if (theta > PI) {
chrigelburri 3:92ba0254af87 260 return theta -= 2*PI;
chrigelburri 3:92ba0254af87 261 } else {
chrigelburri 3:92ba0254af87 262 return theta;
chrigelburri 3:92ba0254af87 263 }
chrigelburri 3:92ba0254af87 264 }