Christian Burri / Mbed 2 deprecated autonomous Robot Android

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

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

RobotControl/RobotControl.cpp@11:775ebb69d5e1, 2013-04-05 (annotated)

Committer:
chrigelburri
Date:
Fri Apr 05 10:58:42 2013 +0000
Revision:
11:775ebb69d5e1
Parent:
8:696c2f9dfc62
Child:
12:235e318a414f
doku soweit gut ohne android

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 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 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::setDesiredSpeed(float speed)
chrigelburri 0:31f7be68e52d 46 {
chrigelburri 0:31f7be68e52d 47 this->speed = speed;
chrigelburri 0:31f7be68e52d 48 }
chrigelburri 0:31f7be68e52d 49
chrigelburri 0:31f7be68e52d 50 void RobotControl::setDesiredOmega(float omega)
chrigelburri 0:31f7be68e52d 51 {
chrigelburri 0:31f7be68e52d 52 this->omega = omega;
chrigelburri 0:31f7be68e52d 53 }
chrigelburri 0:31f7be68e52d 54
chrigelburri 5:48a258f6335e 55 void RobotControl::setDesiredxPosition(float xposition)
chrigelburri 0:31f7be68e52d 56 {
chrigelburri 1:6cd533a712c6 57 Desired.xposition = xposition;
chrigelburri 0:31f7be68e52d 58 }
chrigelburri 0:31f7be68e52d 59
chrigelburri 5:48a258f6335e 60 void RobotControl::setDesiredyPosition(float yposition)
chrigelburri 0:31f7be68e52d 61 {
chrigelburri 1:6cd533a712c6 62 Desired.yposition = yposition;
chrigelburri 0:31f7be68e52d 63 }
chrigelburri 0:31f7be68e52d 64
chrigelburri 5:48a258f6335e 65 void RobotControl::setDesiredTheta(float theta)
chrigelburri 0:31f7be68e52d 66 {
chrigelburri 0:31f7be68e52d 67 Desired.theta = theta;
chrigelburri 0:31f7be68e52d 68 }
chrigelburri 0:31f7be68e52d 69
chrigelburri 5:48a258f6335e 70 float RobotControl::getDesiredxPosition()
chrigelburri 5:48a258f6335e 71 {
chrigelburri 5:48a258f6335e 72 return Desired.xposition;
chrigelburri 5:48a258f6335e 73 }
chrigelburri 5:48a258f6335e 74
chrigelburri 5:48a258f6335e 75 float RobotControl::getDesiredyPosition()
chrigelburri 2:d8e1613dc38b 76 {
chrigelburri 5:48a258f6335e 77 return Desired.yposition;
chrigelburri 5:48a258f6335e 78 }
chrigelburri 5:48a258f6335e 79
chrigelburri 5:48a258f6335e 80 float RobotControl::getDesiredTheta()
chrigelburri 5:48a258f6335e 81 {
chrigelburri 5:48a258f6335e 82 return Desired.theta;
chrigelburri 5:48a258f6335e 83 }
chrigelburri 5:48a258f6335e 84
chrigelburri 6:48eeb41188dd 85 void RobotControl::setDesiredPositionAndAngle(float xposition,
chrigelburri 6:48eeb41188dd 86 float yposition,
chrigelburri 6:48eeb41188dd 87 float theta)
chrigelburri 5:48a258f6335e 88 {
chrigelburri 5:48a258f6335e 89 setDesiredxPosition(xposition);
chrigelburri 5:48a258f6335e 90 setDesiredyPosition(yposition);
chrigelburri 5:48a258f6335e 91 setDesiredTheta(theta);
chrigelburri 2:d8e1613dc38b 92 }
chrigelburri 2:d8e1613dc38b 93
chrigelburri 1:6cd533a712c6 94 float RobotControl::getTheta()
chrigelburri 1:6cd533a712c6 95 {
chrigelburri 1:6cd533a712c6 96 return Desired.theta;
chrigelburri 1:6cd533a712c6 97 }
chrigelburri 1:6cd533a712c6 98
chrigelburri 0:31f7be68e52d 99 float RobotControl::getDesiredSpeed()
chrigelburri 0:31f7be68e52d 100 {
chrigelburri 0:31f7be68e52d 101 return speed;
chrigelburri 0:31f7be68e52d 102 }
chrigelburri 0:31f7be68e52d 103
chrigelburri 0:31f7be68e52d 104 float RobotControl::getActualSpeed()
chrigelburri 0:31f7be68e52d 105 {
chrigelburri 0:31f7be68e52d 106 return Actual.speed;
chrigelburri 0:31f7be68e52d 107 }
chrigelburri 0:31f7be68e52d 108
chrigelburri 0:31f7be68e52d 109 float RobotControl::getDesiredOmega()
chrigelburri 0:31f7be68e52d 110 {
chrigelburri 0:31f7be68e52d 111 return omega;
chrigelburri 0:31f7be68e52d 112 }
chrigelburri 0:31f7be68e52d 113
chrigelburri 0:31f7be68e52d 114 float RobotControl::getActualOmega()
chrigelburri 0:31f7be68e52d 115 {
chrigelburri 0:31f7be68e52d 116 return Actual.omega;
chrigelburri 0:31f7be68e52d 117 }
chrigelburri 0:31f7be68e52d 118
chrigelburri 0:31f7be68e52d 119 float RobotControl::getxActualPosition()
chrigelburri 0:31f7be68e52d 120 {
chrigelburri 0:31f7be68e52d 121 return Actual.getxPosition();
chrigelburri 0:31f7be68e52d 122 }
chrigelburri 0:31f7be68e52d 123
chrigelburri 0:31f7be68e52d 124 float RobotControl::getxPositionError()
chrigelburri 0:31f7be68e52d 125 {
chrigelburri 0:31f7be68e52d 126 return Desired.getxPosition()-Actual.getxPosition();
chrigelburri 0:31f7be68e52d 127 }
chrigelburri 0:31f7be68e52d 128
chrigelburri 0:31f7be68e52d 129 float RobotControl::getyActualPosition()
chrigelburri 0:31f7be68e52d 130 {
chrigelburri 0:31f7be68e52d 131 return Actual.getyPosition();
chrigelburri 0:31f7be68e52d 132 }
chrigelburri 0:31f7be68e52d 133
chrigelburri 0:31f7be68e52d 134 float RobotControl::getyPositionError()
chrigelburri 0:31f7be68e52d 135 {
chrigelburri 0:31f7be68e52d 136 return Desired.getyPosition()-Actual.getyPosition();
chrigelburri 0:31f7be68e52d 137 }
chrigelburri 0:31f7be68e52d 138
chrigelburri 0:31f7be68e52d 139 float RobotControl::getActualTheta()
chrigelburri 0:31f7be68e52d 140 {
chrigelburri 0:31f7be68e52d 141 return Actual.getTheta();
chrigelburri 0:31f7be68e52d 142 }
chrigelburri 0:31f7be68e52d 143
chrigelburri 0:31f7be68e52d 144 float RobotControl::getThetaError()
chrigelburri 0:31f7be68e52d 145 {
chrigelburri 0:31f7be68e52d 146 return Desired.getTheta()-Actual.getTheta();
chrigelburri 0:31f7be68e52d 147 }
chrigelburri 0:31f7be68e52d 148
chrigelburri 1:6cd533a712c6 149 float RobotControl::getDistanceError()
chrigelburri 1:6cd533a712c6 150 {
chrigelburri 1:6cd533a712c6 151 return sqrt( ( getxPositionError() * getxPositionError() ) + (getyPositionError() * getyPositionError() ) );
chrigelburri 1:6cd533a712c6 152 }
chrigelburri 1:6cd533a712c6 153
chrigelburri 1:6cd533a712c6 154 float RobotControl::getThetaErrorToGoal()
chrigelburri 0:31f7be68e52d 155 {
chrigelburri 3:92ba0254af87 156 return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
chrigelburri 1:6cd533a712c6 157 }
chrigelburri 1:6cd533a712c6 158
chrigelburri 1:6cd533a712c6 159 float RobotControl::getThetaGoal()
chrigelburri 1:6cd533a712c6 160 {
chrigelburri 3:92ba0254af87 161 return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
chrigelburri 1:6cd533a712c6 162 }
chrigelburri 1:6cd533a712c6 163
chrigelburri 6:48eeb41188dd 164 void RobotControl::setAllToZero(float xZeroPos,
chrigelburri 6:48eeb41188dd 165 float yZeroPos,
chrigelburri 6:48eeb41188dd 166 float theta)
chrigelburri 1:6cd533a712c6 167 {
chrigelburri 1:6cd533a712c6 168 Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 169 Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 170 stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 171 stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 172 speed = 0.0f;
chrigelburri 0:31f7be68e52d 173 omega = 0.0f;
chrigelburri 0:31f7be68e52d 174 }
chrigelburri 0:31f7be68e52d 175
chrigelburri 0:31f7be68e52d 176 void RobotControl::run()
chrigelburri 0:31f7be68e52d 177 {
chrigelburri 11:775ebb69d5e1 178 // kann glaub wegggggg
chrigelburri 0:31f7be68e52d 179 if (isEnabled()) {
chrigelburri 0:31f7be68e52d 180 } else {
chrigelburri 0:31f7be68e52d 181 speed = 0.0f;
chrigelburri 0:31f7be68e52d 182 omega = 0.0f;
chrigelburri 0:31f7be68e52d 183 Desired.setState(&Actual);
chrigelburri 0:31f7be68e52d 184 }
chrigelburri 11:775ebb69d5e1 185 ////bis hieerrrrr
chrigelburri 0:31f7be68e52d 186
chrigelburri 0:31f7be68e52d 187 /* position calculation */
chrigelburri 0:31f7be68e52d 188
chrigelburri 0:31f7be68e52d 189 /* Set the state of speed from Left und Right Wheel*/
chrigelburri 6:48eeb41188dd 190 stateLeft.speed = motorControllerLeft->getActualSpeed() *
chrigelburri 6:48eeb41188dd 191 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
chrigelburri 6:48eeb41188dd 192 stateRight.speed = - motorControllerRight->getActualSpeed() *
chrigelburri 8:696c2f9dfc62 193 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ;
chrigelburri 0:31f7be68e52d 194
chrigelburri 0:31f7be68e52d 195 /* translational speed of the Robot (average) */
chrigelburri 8:696c2f9dfc62 196 Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f;
chrigelburri 0:31f7be68e52d 197
chrigelburri 0:31f7be68e52d 198 /* rotational speed of the Robot */
chrigelburri 8:696c2f9dfc62 199 Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE;
chrigelburri 0:31f7be68e52d 200
chrigelburri 1:6cd533a712c6 201 /* rotational theta of the Robot integrate the omega with the time*/
chrigelburri 0:31f7be68e52d 202 Actual.theta += Actual.omega * period;
chrigelburri 3:92ba0254af87 203 Actual.theta = PiRange(Actual.theta);
chrigelburri 6:48eeb41188dd 204
chrigelburri 1:6cd533a712c6 205 /* translational X and Y Position. integrate the speed with the time */
chrigelburri 1:6cd533a712c6 206 Actual.xposition += (Actual.speed * period * cos(Actual.theta));
chrigelburri 1:6cd533a712c6 207 Actual.yposition += (Actual.speed * period * sin(Actual.theta));
chrigelburri 0:31f7be68e52d 208
chrigelburri 1:6cd533a712c6 209 /* motor control */
chrigelburri 1:6cd533a712c6 210 if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) {
chrigelburri 0:31f7be68e52d 211
chrigelburri 1:6cd533a712c6 212 /* postition control */
chrigelburri 2:d8e1613dc38b 213
chrigelburri 1:6cd533a712c6 214 speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
chrigelburri 2:d8e1613dc38b 215 omega = K2 * getThetaErrorToGoal() +
chrigelburri 6:48eeb41188dd 216 K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) /
chrigelburri 6:48eeb41188dd 217 (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
chrigelburri 0:31f7be68e52d 218
chrigelburri 6:48eeb41188dd 219 motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) /
chrigelburri 6:48eeb41188dd 220 (2 * WHEEL_RADIUS_LEFT * PI * GEAR) );
chrigelburri 6:48eeb41188dd 221 motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) /
chrigelburri 6:48eeb41188dd 222 (2 * WHEEL_RADIUS_RIGHT * PI * GEAR) );
chrigelburri 0:31f7be68e52d 223
chrigelburri 0:31f7be68e52d 224 } else {
chrigelburri 0:31f7be68e52d 225
chrigelburri 0:31f7be68e52d 226 motorControllerLeft->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 227 motorControllerRight->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 228
chrigelburri 1:6cd533a712c6 229 }
chrigelburri 0:31f7be68e52d 230 }
chrigelburri 3:92ba0254af87 231
chrigelburri 3:92ba0254af87 232 float RobotControl::PiRange(float theta)
chrigelburri 3:92ba0254af87 233 {
chrigelburri 3:92ba0254af87 234 if(theta <= -PI) {
chrigelburri 3:92ba0254af87 235 return theta += 2*PI;
chrigelburri 3:92ba0254af87 236 } else if (theta > PI) {
chrigelburri 3:92ba0254af87 237 return theta -= 2*PI;
chrigelburri 3:92ba0254af87 238 } else {
chrigelburri 3:92ba0254af87 239 return theta;
chrigelburri 3:92ba0254af87 240 }
chrigelburri 3:92ba0254af87 241 }