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@0:31f7be68e52d, 2013-02-07 (annotated)

Committer:
chrigelburri
Date:
Thu Feb 07 17:43:19 2013 +0000
Revision:
0:31f7be68e52d
Child:
1:6cd533a712c6
first steps

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 0:31f7be68e52d 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 0:31f7be68e52d 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 0:31f7be68e52d 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::setAcceleration(float acc)
chrigelburri 0:31f7be68e52d 46 {
chrigelburri 0:31f7be68e52d 47 Desired.setAcceleration(acc);
chrigelburri 0:31f7be68e52d 48
chrigelburri 0:31f7be68e52d 49 }
chrigelburri 0:31f7be68e52d 50
chrigelburri 0:31f7be68e52d 51 void RobotControl::setThetaAcceleration(float acc)
chrigelburri 0:31f7be68e52d 52 {
chrigelburri 0:31f7be68e52d 53 Desired.setThetaAcceleration(acc);
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 0:31f7be68e52d 66 void RobotControl::setxPosition(float position)
chrigelburri 0:31f7be68e52d 67 {
chrigelburri 0:31f7be68e52d 68 Desired.xposition = position;
chrigelburri 0:31f7be68e52d 69 }
chrigelburri 0:31f7be68e52d 70
chrigelburri 0:31f7be68e52d 71 void RobotControl::setyPosition(float position)
chrigelburri 0:31f7be68e52d 72 {
chrigelburri 0:31f7be68e52d 73 Desired.yposition = position;
chrigelburri 0:31f7be68e52d 74 }
chrigelburri 0:31f7be68e52d 75
chrigelburri 0:31f7be68e52d 76 void RobotControl::setTheta(float theta)
chrigelburri 0:31f7be68e52d 77 {
chrigelburri 0:31f7be68e52d 78 Desired.theta = theta;
chrigelburri 0:31f7be68e52d 79 }
chrigelburri 0:31f7be68e52d 80
chrigelburri 0:31f7be68e52d 81 float RobotControl::getDesiredSpeed()
chrigelburri 0:31f7be68e52d 82 {
chrigelburri 0:31f7be68e52d 83 return speed;
chrigelburri 0:31f7be68e52d 84 }
chrigelburri 0:31f7be68e52d 85
chrigelburri 0:31f7be68e52d 86 float RobotControl::getActualSpeed()
chrigelburri 0:31f7be68e52d 87 {
chrigelburri 0:31f7be68e52d 88 return Actual.speed;
chrigelburri 0:31f7be68e52d 89 }
chrigelburri 0:31f7be68e52d 90
chrigelburri 0:31f7be68e52d 91 float RobotControl::getDesiredOmega()
chrigelburri 0:31f7be68e52d 92 {
chrigelburri 0:31f7be68e52d 93 return omega;
chrigelburri 0:31f7be68e52d 94 }
chrigelburri 0:31f7be68e52d 95
chrigelburri 0:31f7be68e52d 96 float RobotControl::getActualOmega()
chrigelburri 0:31f7be68e52d 97 {
chrigelburri 0:31f7be68e52d 98 return Actual.omega;
chrigelburri 0:31f7be68e52d 99 }
chrigelburri 0:31f7be68e52d 100
chrigelburri 0:31f7be68e52d 101 float RobotControl::getxActualPosition()
chrigelburri 0:31f7be68e52d 102 {
chrigelburri 0:31f7be68e52d 103 return Actual.getxPosition();
chrigelburri 0:31f7be68e52d 104 }
chrigelburri 0:31f7be68e52d 105
chrigelburri 0:31f7be68e52d 106 float RobotControl::getxPositionError()
chrigelburri 0:31f7be68e52d 107 {
chrigelburri 0:31f7be68e52d 108 return Desired.getxPosition()-Actual.getxPosition();
chrigelburri 0:31f7be68e52d 109 }
chrigelburri 0:31f7be68e52d 110
chrigelburri 0:31f7be68e52d 111 float RobotControl::getyActualPosition()
chrigelburri 0:31f7be68e52d 112 {
chrigelburri 0:31f7be68e52d 113 return Actual.getyPosition();
chrigelburri 0:31f7be68e52d 114 }
chrigelburri 0:31f7be68e52d 115
chrigelburri 0:31f7be68e52d 116 float RobotControl::getyPositionError()
chrigelburri 0:31f7be68e52d 117 {
chrigelburri 0:31f7be68e52d 118 return Desired.getyPosition()-Actual.getyPosition();
chrigelburri 0:31f7be68e52d 119 }
chrigelburri 0:31f7be68e52d 120
chrigelburri 0:31f7be68e52d 121 float RobotControl::getActualTheta()
chrigelburri 0:31f7be68e52d 122 {
chrigelburri 0:31f7be68e52d 123 return Actual.getTheta();
chrigelburri 0:31f7be68e52d 124 }
chrigelburri 0:31f7be68e52d 125
chrigelburri 0:31f7be68e52d 126 float RobotControl::getThetaError()
chrigelburri 0:31f7be68e52d 127 {
chrigelburri 0:31f7be68e52d 128 return Desired.getTheta()-Actual.getTheta();
chrigelburri 0:31f7be68e52d 129 }
chrigelburri 0:31f7be68e52d 130
chrigelburri 0:31f7be68e52d 131 void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
chrigelburri 0:31f7be68e52d 132 {
chrigelburri 0:31f7be68e52d 133 Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 134 Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 135 stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 136 stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
chrigelburri 0:31f7be68e52d 137 speed = 0.0f;
chrigelburri 0:31f7be68e52d 138 omega = 0.0f;
chrigelburri 0:31f7be68e52d 139 }
chrigelburri 0:31f7be68e52d 140
chrigelburri 0:31f7be68e52d 141
chrigelburri 0:31f7be68e52d 142 void RobotControl::run()
chrigelburri 0:31f7be68e52d 143 {
chrigelburri 0:31f7be68e52d 144 /* motion planning */
chrigelburri 0:31f7be68e52d 145 if (isEnabled()) {
chrigelburri 0:31f7be68e52d 146 Desired.increment(speed, omega, period);
chrigelburri 0:31f7be68e52d 147 } else {
chrigelburri 0:31f7be68e52d 148 speed = 0.0f;
chrigelburri 0:31f7be68e52d 149 omega = 0.0f;
chrigelburri 0:31f7be68e52d 150 Desired.setState(&Actual);
chrigelburri 0:31f7be68e52d 151 }
chrigelburri 0:31f7be68e52d 152
chrigelburri 0:31f7be68e52d 153 /* position calculation */
chrigelburri 0:31f7be68e52d 154
chrigelburri 0:31f7be68e52d 155 /* Set the state of speed from Left und Right Wheel*/
chrigelburri 0:31f7be68e52d 156 stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
chrigelburri 0:31f7be68e52d 157 stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
chrigelburri 0:31f7be68e52d 158
chrigelburri 0:31f7be68e52d 159 /* translational speed of the Robot (average) */
chrigelburri 0:31f7be68e52d 160 Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
chrigelburri 0:31f7be68e52d 161
chrigelburri 0:31f7be68e52d 162 /* rotational speed of the Robot */
chrigelburri 0:31f7be68e52d 163 Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE;
chrigelburri 0:31f7be68e52d 164
chrigelburri 0:31f7be68e52d 165 /* rotational theta of the Robot */
chrigelburri 0:31f7be68e52d 166 Actual.theta += Actual.omega * period;
chrigelburri 0:31f7be68e52d 167 if(Actual.theta <= -PI) {
chrigelburri 0:31f7be68e52d 168 Actual.theta += 2* PI;
chrigelburri 0:31f7be68e52d 169 } else if (Actual.theta > PI) {
chrigelburri 0:31f7be68e52d 170 Actual.theta -= 2* PI;
chrigelburri 0:31f7be68e52d 171 } else {
chrigelburri 0:31f7be68e52d 172 //nothing
chrigelburri 0:31f7be68e52d 173 }
chrigelburri 0:31f7be68e52d 174 Actual.theta += Actual.omega * period;
chrigelburri 0:31f7be68e52d 175 Actual.thetaCompass = compass->getFilteredAngle();
chrigelburri 0:31f7be68e52d 176
chrigelburri 0:31f7be68e52d 177 /* translational X and Y Position. integrate the speed with the time */
chrigelburri 0:31f7be68e52d 178 // Actual.xposition += (Actual.speed * period * sin(Actual.theta));
chrigelburri 0:31f7be68e52d 179 // Actual.yposition += (Actual.speed * period * cos(Actual.theta));
chrigelburri 0:31f7be68e52d 180
chrigelburri 0:31f7be68e52d 181 /* translational X and Y Position. integrate the speed with the time theta from compass */
chrigelburri 0:31f7be68e52d 182 Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass));
chrigelburri 0:31f7be68e52d 183 Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass));
chrigelburri 0:31f7be68e52d 184
chrigelburri 0:31f7be68e52d 185
chrigelburri 0:31f7be68e52d 186
chrigelburri 0:31f7be68e52d 187
chrigelburri 0:31f7be68e52d 188 /* postition control calculate */
chrigelburri 0:31f7be68e52d 189
chrigelburri 0:31f7be68e52d 190 rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
chrigelburri 0:31f7be68e52d 191
chrigelburri 0:31f7be68e52d 192
chrigelburri 0:31f7be68e52d 193
chrigelburri 0:31f7be68e52d 194
chrigelburri 0:31f7be68e52d 195
chrigelburri 0:31f7be68e52d 196 /* motor control */
chrigelburri 0:31f7be68e52d 197 if (isEnabled()) {
chrigelburri 0:31f7be68e52d 198
chrigelburri 0:31f7be68e52d 199 motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) );
chrigelburri 0:31f7be68e52d 200 motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) );
chrigelburri 0:31f7be68e52d 201
chrigelburri 0:31f7be68e52d 202
chrigelburri 0:31f7be68e52d 203 } else {
chrigelburri 0:31f7be68e52d 204
chrigelburri 0:31f7be68e52d 205 motorControllerLeft->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 206 motorControllerRight->setVelocity(0.0f);
chrigelburri 0:31f7be68e52d 207
chrigelburri 0:31f7be68e52d 208 }
chrigelburri 0:31f7be68e52d 209 }
chrigelburri 0:31f7be68e52d 210