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

State/State.cpp@24:08241be546ba, 2013-05-19 (annotated)

Committer:
chrigelburri
Date:
Sun May 19 14:54:49 2013 +0000
Revision:
24:08241be546ba
Parent:
12:235e318a414f
Child:
27:a13ede88e75f
Child:
29:937c74ff040e
LED lauft :-) cool

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