Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

RobotControl/RobotControl.cpp@5:48a258f6335e, 2013-03-21 (annotated)

Committer:
chrigelburri
Date:
Thu Mar 21 08:56:53 2013 +0000
Revision:
5:48a258f6335e
Parent:
3:92ba0254af87
Child:
6:48eeb41188dd
vor dem Gr?nden m?nd?li;

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