Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> 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: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

State/State.cpp@27:a13ede88e75f, 2013-05-20 (annotated)

Committer:
chrigelburri
Date:
Mon May 20 11:51:33 2013 +0000
Revision:
27:a13ede88e75f
Parent:
24:08241be546ba 
mit led methode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 0:31f7be68e52d 1 #include "State.h"
chrigelburri 0:31f7be68e52d 2
chrigelburri 0:31f7be68e52d 3 using namespace std;
chrigelburri 0:31f7be68e52d 4
chrigelburri 0:31f7be68e52d 5 // File
chrigelburri 0:31f7be68e52d 6 FILE *logp;
chrigelburri 0:31f7be68e52d 7
chrigelburri 0:31f7be68e52d 8 // LocalFileSystem
chrigelburri 0:31f7be68e52d 9 LocalFileSystem local("local");
chrigelburri 0:31f7be68e52d 10
chrigelburri 11:775ebb69d5e1 11 State::State(state_t* s,
chrigelburri 11:775ebb69d5e1 12 RobotControl* robotControl,
chrigelburri 11:775ebb69d5e1 13 MaxonESCON* motorControllerLeft,
chrigelburri 11:775ebb69d5e1 14 MaxonESCON* motorControllerRight,
chrigelburri 11:775ebb69d5e1 15 PinName batteryPin,
chrigelburri 11:775ebb69d5e1 16 float period)
chrigelburri 11:775ebb69d5e1 17 : Task(period) ,
chrigelburri 27:a13ede88e75f 18 battery(batteryPin),
chrigelburri 27:a13ede88e75f 19 led1(LED1),
chrigelburri 27:a13ede88e75f 20 led2(LED2),
chrigelburri 27:a13ede88e75f 21 led3(LED3),
chrigelburri 27:a13ede88e75f 22 led4(LED4)
chrigelburri 0:31f7be68e52d 23 {
chrigelburri 0:31f7be68e52d 24 /* get peripherals */
chrigelburri 0:31f7be68e52d 25 this->s = s;
chrigelburri 0:31f7be68e52d 26 this->robotControl = robotControl;
chrigelburri 0:31f7be68e52d 27 this->motorControllerLeft = motorControllerLeft;
chrigelburri 0:31f7be68e52d 28 this->motorControllerRight = motorControllerRight;
chrigelburri 0:31f7be68e52d 29 this->battery =battery;
chrigelburri 0:31f7be68e52d 30 this->period = period;
chrigelburri 27:a13ede88e75f 31
chrigelburri 0:31f7be68e52d 32 }
chrigelburri 0:31f7be68e52d 33
chrigelburri 0:31f7be68e52d 34 State::~State() {}
chrigelburri 0:31f7be68e52d 35
chrigelburri 0:31f7be68e52d 36 void State::initPlotFile(void)
chrigelburri 0:31f7be68e52d 37 {
chrigelburri 0:31f7be68e52d 38 logp = fopen("/local/plots.txt", "w"); // only write
chrigelburri 0:31f7be68e52d 39 if(logp == NULL) {
chrigelburri 0:31f7be68e52d 40 exit(1);
chrigelburri 0:31f7be68e52d 41 } else {
chrigelburri 0:31f7be68e52d 42 fprintf(logp,
chrigelburri 0:31f7be68e52d 43 "Time[ms]\t"
chrigelburri 0:31f7be68e52d 44 "BatteryVoltage[V]\t"
chrigelburri 0:31f7be68e52d 45 "NumberOfPulsesLeft\t"
chrigelburri 0:31f7be68e52d 46 "NumberOfPulsesRight\t"
chrigelburri 0:31f7be68e52d 47 "VelocityLeft[m/s]\t"
chrigelburri 0:31f7be68e52d 48 "VelocityRight[m/s]\t"
chrigelburri 4:3a97923ff2d4 49 "VelocityCar[m/s]\t" //7
chrigelburri 0:31f7be68e52d 50 "VelocityRotation[grad/s]\t"
chrigelburri 0:31f7be68e52d 51 "X-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 52 "Y-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 53 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 54 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 55 "AngleError[grad]\t"
chrigelburri 0:31f7be68e52d 56 "AngleCar[grad]\t"
chrigelburri 5:48a258f6335e 57 "SetpointX-Axis[m]\t" //15
chrigelburri 0:31f7be68e52d 58 "SetpointY-Axis[m]\t"
chrigelburri 5:48a258f6335e 59 "SetpointAngel[grad]\t" //17
chrigelburri 8:696c2f9dfc62 60 "SetpointVelocitiy[m/s]\t"
chrigelburri 8:696c2f9dfc62 61 "SetpointVelocitiyRotations[rad/s]\t"
chrigelburri 11:775ebb69d5e1 62 "State\t" //20
chrigelburri 11:775ebb69d5e1 63 "distanceToGoal[m]\t" //21
chrigelburri 5:48a258f6335e 64 "angleToGoal[grad]\t"
chrigelburri 5:48a258f6335e 65 "thetaFromTheGoal[grad]\n");
chrigelburri 0:31f7be68e52d 66 }
chrigelburri 0:31f7be68e52d 67 }
chrigelburri 0:31f7be68e52d 68
chrigelburri 0:31f7be68e52d 69
chrigelburri 0:31f7be68e52d 70 void State::savePlotFile(state_t s)
chrigelburri 0:31f7be68e52d 71 {
chrigelburri 0:31f7be68e52d 72 char buf[256];
chrigelburri 0:31f7be68e52d 73
chrigelburri 6:48eeb41188dd 74 sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t"
chrigelburri 11:775ebb69d5e1 75 "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f",
chrigelburri 0:31f7be68e52d 76 s.millis,
chrigelburri 0:31f7be68e52d 77 s.voltageBattery,
chrigelburri 0:31f7be68e52d 78 s.leftPulses,
chrigelburri 0:31f7be68e52d 79 s.rightPulses,
chrigelburri 0:31f7be68e52d 80 s.leftVelocity,
chrigelburri 0:31f7be68e52d 81 s.rightVelocity,
chrigelburri 0:31f7be68e52d 82 s.velocity,
chrigelburri 0:31f7be68e52d 83 s.omega,
chrigelburri 0:31f7be68e52d 84 s.xAxis,
chrigelburri 0:31f7be68e52d 85 s.yAxis,
chrigelburri 0:31f7be68e52d 86 s.xAxisError,
chrigelburri 0:31f7be68e52d 87 s.yAxisError,
chrigelburri 0:31f7be68e52d 88 s.angleError,
chrigelburri 0:31f7be68e52d 89 s.angle,
chrigelburri 0:31f7be68e52d 90 s.setxAxis,
chrigelburri 0:31f7be68e52d 91 s.setyAxis,
chrigelburri 0:31f7be68e52d 92 s.setAngle,
chrigelburri 8:696c2f9dfc62 93 s.setVelocity,
chrigelburri 8:696c2f9dfc62 94 s.setOmega,
chrigelburri 2:d8e1613dc38b 95 s.state,
chrigelburri 2:d8e1613dc38b 96 s.rho,
chrigelburri 2:d8e1613dc38b 97 s.lamda,
chrigelburri 2:d8e1613dc38b 98 s.delta
chrigelburri 0:31f7be68e52d 99 );
chrigelburri 0:31f7be68e52d 100
chrigelburri 0:31f7be68e52d 101 if (logp)
chrigelburri 0:31f7be68e52d 102 fprintf(logp, buf);
chrigelburri 0:31f7be68e52d 103 fprintf(logp, "\n"); // new line
chrigelburri 0:31f7be68e52d 104 }
chrigelburri 0:31f7be68e52d 105
chrigelburri 0:31f7be68e52d 106 void State::closePlotFile(void)
chrigelburri 0:31f7be68e52d 107 {
chrigelburri 0:31f7be68e52d 108 fclose(logp);
chrigelburri 0:31f7be68e52d 109 }
chrigelburri 0:31f7be68e52d 110
chrigelburri 0:31f7be68e52d 111 float State::readBattery()
chrigelburri 0:31f7be68e52d 112 {
chrigelburri 11:775ebb69d5e1 113 return battery.read()*BAT_MULTIPLICATOR;
chrigelburri 0:31f7be68e52d 114 }
chrigelburri 0:31f7be68e52d 115
chrigelburri 0:31f7be68e52d 116 void State::setBatteryBit()
chrigelburri 0:31f7be68e52d 117 {
chrigelburri 0:31f7be68e52d 118 if(s->voltageBattery < BAT_MIN) {
chrigelburri 0:31f7be68e52d 119 s->state |= STATE_UNDER;
chrigelburri 0:31f7be68e52d 120 } else {
chrigelburri 0:31f7be68e52d 121 s->state &= (~STATE_UNDER);
chrigelburri 0:31f7be68e52d 122 }
chrigelburri 0:31f7be68e52d 123 }
chrigelburri 0:31f7be68e52d 124
chrigelburri 0:31f7be68e52d 125 void State::setEnableLeftBit()
chrigelburri 0:31f7be68e52d 126 {
chrigelburri 0:31f7be68e52d 127 if(motorControllerLeft->isEnabled()) {
chrigelburri 0:31f7be68e52d 128 s->state &= (~STATE_LEFT);
chrigelburri 0:31f7be68e52d 129 } else {
chrigelburri 0:31f7be68e52d 130 s->state |= STATE_LEFT;
chrigelburri 0:31f7be68e52d 131 }
chrigelburri 0:31f7be68e52d 132 }
chrigelburri 0:31f7be68e52d 133
chrigelburri 0:31f7be68e52d 134 void State::setEnableRightBit()
chrigelburri 0:31f7be68e52d 135 {
chrigelburri 0:31f7be68e52d 136 if(motorControllerRight->isEnabled()) {
chrigelburri 0:31f7be68e52d 137 s->state &= (~STATE_RIGHT);
chrigelburri 0:31f7be68e52d 138 } else {
chrigelburri 0:31f7be68e52d 139 s->state |= STATE_RIGHT;
chrigelburri 0:31f7be68e52d 140 }
chrigelburri 0:31f7be68e52d 141 }
chrigelburri 0:31f7be68e52d 142
chrigelburri 0:31f7be68e52d 143 void State::startTimerFromZero()
chrigelburri 0:31f7be68e52d 144 {
chrigelburri 0:31f7be68e52d 145 timer.reset();
chrigelburri 0:31f7be68e52d 146 timer.start();
chrigelburri 0:31f7be68e52d 147 }
chrigelburri 0:31f7be68e52d 148
chrigelburri 24:08241be546ba 149 float State::dim( int idx, float f )
chrigelburri 24:08241be546ba 150 {
chrigelburri 24:08241be546ba 151 return (sin(f + float(idx)* 1.0) + 1.0) / 2.0; // transform value from -1.0 .. 1.0 to 0.0 .. 1.0
chrigelburri 24:08241be546ba 152 }
chrigelburri 24:08241be546ba 153
chrigelburri 27:a13ede88e75f 154 void State::ledArray(float wait)
chrigelburri 27:a13ede88e75f 155 {
chrigelburri 27:a13ede88e75f 156 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 27:a13ede88e75f 157 led1 = dim( 0, f );
chrigelburri 27:a13ede88e75f 158 led2 = dim( 1, f );
chrigelburri 27:a13ede88e75f 159 led3 = dim( 2, f );
chrigelburri 27:a13ede88e75f 160 led4 = dim( 3, f );
chrigelburri 27:a13ede88e75f 161
chrigelburri 27:a13ede88e75f 162 wait_ms(wait);
chrigelburri 27:a13ede88e75f 163 }
chrigelburri 27:a13ede88e75f 164 wait_ms(wait/0.2);
chrigelburri 27:a13ede88e75f 165 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 27:a13ede88e75f 166 led1 = dim( 3, f );
chrigelburri 27:a13ede88e75f 167 led2 = dim( 2, f );
chrigelburri 27:a13ede88e75f 168 led3 = dim( 1, f );
chrigelburri 27:a13ede88e75f 169 led4 = dim( 0, f );
chrigelburri 27:a13ede88e75f 170 wait_ms(wait);
chrigelburri 27:a13ede88e75f 171 }
chrigelburri 27:a13ede88e75f 172 wait_ms(wait/0.2);
chrigelburri 27:a13ede88e75f 173 }
chrigelburri 27:a13ede88e75f 174
chrigelburri 0:31f7be68e52d 175 void State::run()
chrigelburri 0:31f7be68e52d 176 {
chrigelburri 0:31f7be68e52d 177 s->millis = timer.read_ms();
chrigelburri 0:31f7be68e52d 178 s->voltageBattery = readBattery();
chrigelburri 0:31f7be68e52d 179 s->leftPulses = - motorControllerLeft->getPulses();
chrigelburri 0:31f7be68e52d 180 s->rightPulses = motorControllerRight->getPulses();
chrigelburri 6:48eeb41188dd 181 s->leftVelocity = motorControllerLeft->getActualSpeed() *
chrigelburri 8:696c2f9dfc62 182 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
chrigelburri 6:48eeb41188dd 183 s->rightVelocity = - motorControllerRight->getActualSpeed()*
chrigelburri 8:696c2f9dfc62 184 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
chrigelburri 0:31f7be68e52d 185 s->velocity = robotControl->getActualSpeed();
chrigelburri 0:31f7be68e52d 186 s->omega = robotControl->getActualOmega();
chrigelburri 0:31f7be68e52d 187 s->xAxis = robotControl->getxActualPosition();
chrigelburri 0:31f7be68e52d 188 s->yAxis = robotControl->getyActualPosition();
chrigelburri 0:31f7be68e52d 189 s->angle = robotControl->getActualTheta() * 180 / PI;
chrigelburri 0:31f7be68e52d 190 s->xAxisError = robotControl->getxPositionError();
chrigelburri 0:31f7be68e52d 191 s->yAxisError = robotControl->getyPositionError();
chrigelburri 0:31f7be68e52d 192 s->angleError = robotControl->getThetaError() * 180 / PI;
chrigelburri 5:48a258f6335e 193 s->setxAxis = robotControl->getDesiredxPosition();
chrigelburri 5:48a258f6335e 194 s->setyAxis = robotControl->getDesiredyPosition();
chrigelburri 5:48a258f6335e 195 s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
chrigelburri 8:696c2f9dfc62 196 s->setVelocity = robotControl->getDesiredSpeed();
chrigelburri 8:696c2f9dfc62 197 s->setOmega = robotControl->getDesiredOmega();
chrigelburri 0:31f7be68e52d 198
chrigelburri 2:d8e1613dc38b 199 s->rho = robotControl->getDistanceError();
chrigelburri 2:d8e1613dc38b 200 s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
chrigelburri 2:d8e1613dc38b 201 s->delta = robotControl->getThetaGoal() * 180 / PI;
chrigelburri 0:31f7be68e52d 202
chrigelburri 0:31f7be68e52d 203 setBatteryBit();
chrigelburri 0:31f7be68e52d 204 setEnableLeftBit();
chrigelburri 0:31f7be68e52d 205 setEnableRightBit();
chrigelburri 0:31f7be68e52d 206 }