Christian Burri / Mbed 2 deprecated autonomous Robot Android

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 autonomous Robot Android by Christian Burri

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?

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 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 }