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@1:6cd533a712c6, 2013-03-02 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 02 09:39:34 2013 +0000
Revision:
1:6cd533a712c6
Parent:
0:31f7be68e52d
Child:
2:d8e1613dc38b
Pos Regler funktioniert getestet im leerlauf;

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