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

StateDefines/State.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 "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 1:6cd533a712c6 11 State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period) : Task(period)
chrigelburri 0:31f7be68e52d 12 {
chrigelburri 0:31f7be68e52d 13 /* get peripherals */
chrigelburri 0:31f7be68e52d 14 this->s = s;
chrigelburri 0:31f7be68e52d 15 this->robotControl = robotControl;
chrigelburri 0:31f7be68e52d 16 this->motorControllerLeft = motorControllerLeft;
chrigelburri 0:31f7be68e52d 17 this->motorControllerRight = motorControllerRight;
chrigelburri 1:6cd533a712c6 18 // this->compass = compass;
chrigelburri 0:31f7be68e52d 19 this->battery =battery;
chrigelburri 0:31f7be68e52d 20 this->period = period;
chrigelburri 0:31f7be68e52d 21
chrigelburri 0:31f7be68e52d 22 }
chrigelburri 0:31f7be68e52d 23
chrigelburri 0:31f7be68e52d 24
chrigelburri 0:31f7be68e52d 25 State::~State() {}
chrigelburri 0:31f7be68e52d 26
chrigelburri 0:31f7be68e52d 27 void State::initPlotFile(void)
chrigelburri 0:31f7be68e52d 28 {
chrigelburri 0:31f7be68e52d 29 logp = fopen("/local/plots.txt", "w"); // only write
chrigelburri 0:31f7be68e52d 30 if(logp == NULL) {
chrigelburri 0:31f7be68e52d 31 exit(1);
chrigelburri 0:31f7be68e52d 32 } else {
chrigelburri 0:31f7be68e52d 33 fprintf(logp,
chrigelburri 0:31f7be68e52d 34 "Time[ms]\t"
chrigelburri 0:31f7be68e52d 35 "BatteryVoltage[V]\t"
chrigelburri 0:31f7be68e52d 36 "NumberOfPulsesLeft\t"
chrigelburri 0:31f7be68e52d 37 "NumberOfPulsesRight\t"
chrigelburri 0:31f7be68e52d 38 "VelocityLeft[m/s]\t"
chrigelburri 0:31f7be68e52d 39 "VelocityRight[m/s]\t"
chrigelburri 0:31f7be68e52d 40 "VelocityCar[m/s]\t"
chrigelburri 0:31f7be68e52d 41 "VelocityRotation[grad/s]\t"
chrigelburri 0:31f7be68e52d 42 "X-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 43 "Y-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 44 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 45 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 46 "AngleError[grad]\t"
chrigelburri 0:31f7be68e52d 47 "AngleCar[grad]\t"
chrigelburri 0:31f7be68e52d 48 "SetpointX-Axis[m]\t"
chrigelburri 0:31f7be68e52d 49 "SetpointY-Axis[m]\t"
chrigelburri 0:31f7be68e52d 50 "SetpointAngel[grad]\t"
chrigelburri 0:31f7be68e52d 51 "RightDistanceSensor[m]\t"
chrigelburri 0:31f7be68e52d 52 "CompassAngle[grad]\t"
chrigelburri 0:31f7be68e52d 53 "CompassX-Axis\t"
chrigelburri 0:31f7be68e52d 54 "CompassY-Axis\t"
chrigelburri 0:31f7be68e52d 55 "State\n");
chrigelburri 0:31f7be68e52d 56 }
chrigelburri 0:31f7be68e52d 57 }
chrigelburri 0:31f7be68e52d 58
chrigelburri 0:31f7be68e52d 59
chrigelburri 0:31f7be68e52d 60 void State::savePlotFile(state_t s)
chrigelburri 0:31f7be68e52d 61 {
chrigelburri 0:31f7be68e52d 62 char buf[256];
chrigelburri 0:31f7be68e52d 63
chrigelburri 0:31f7be68e52d 64 sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d",
chrigelburri 0:31f7be68e52d 65 s.millis,
chrigelburri 0:31f7be68e52d 66 s.voltageBattery,
chrigelburri 0:31f7be68e52d 67 s.leftPulses,
chrigelburri 0:31f7be68e52d 68 s.rightPulses,
chrigelburri 0:31f7be68e52d 69 s.leftVelocity,
chrigelburri 0:31f7be68e52d 70 s.rightVelocity,
chrigelburri 0:31f7be68e52d 71 s.velocity,
chrigelburri 0:31f7be68e52d 72 s.omega,
chrigelburri 0:31f7be68e52d 73 s.xAxis,
chrigelburri 0:31f7be68e52d 74 s.yAxis,
chrigelburri 0:31f7be68e52d 75 s.xAxisError,
chrigelburri 0:31f7be68e52d 76 s.yAxisError,
chrigelburri 0:31f7be68e52d 77 s.angleError,
chrigelburri 0:31f7be68e52d 78 s.angle,
chrigelburri 0:31f7be68e52d 79 s.setxAxis,
chrigelburri 0:31f7be68e52d 80 s.setyAxis,
chrigelburri 0:31f7be68e52d 81 s.setAngle,
chrigelburri 0:31f7be68e52d 82 s.rightDist,
chrigelburri 0:31f7be68e52d 83 s.compassAngle,
chrigelburri 0:31f7be68e52d 84 s.compassxAxis,
chrigelburri 0:31f7be68e52d 85 s.compassyAxis,
chrigelburri 0:31f7be68e52d 86 s.state
chrigelburri 0:31f7be68e52d 87 );
chrigelburri 0:31f7be68e52d 88
chrigelburri 0:31f7be68e52d 89 if (logp)
chrigelburri 0:31f7be68e52d 90 fprintf(logp, buf);
chrigelburri 0:31f7be68e52d 91 fprintf(logp, "\n"); // new line
chrigelburri 0:31f7be68e52d 92
chrigelburri 0:31f7be68e52d 93 }
chrigelburri 0:31f7be68e52d 94
chrigelburri 0:31f7be68e52d 95 void State::savePlotText(char text[])
chrigelburri 0:31f7be68e52d 96 {
chrigelburri 0:31f7be68e52d 97 fprintf(logp, text);
chrigelburri 0:31f7be68e52d 98 }
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
chrigelburri 0:31f7be68e52d 107 float State::readBattery()
chrigelburri 0:31f7be68e52d 108 {
chrigelburri 0:31f7be68e52d 109 return battery->read()*BAT_MULTIPLICATOR;
chrigelburri 0:31f7be68e52d 110 }
chrigelburri 0:31f7be68e52d 111
chrigelburri 0:31f7be68e52d 112 void State::setBatteryBit()
chrigelburri 0:31f7be68e52d 113 {
chrigelburri 0:31f7be68e52d 114 if(s->voltageBattery < BAT_MIN) {
chrigelburri 0:31f7be68e52d 115 s->state |= STATE_UNDER;
chrigelburri 0:31f7be68e52d 116 } else {
chrigelburri 0:31f7be68e52d 117 s->state &= (~STATE_UNDER);
chrigelburri 0:31f7be68e52d 118 }
chrigelburri 0:31f7be68e52d 119 }
chrigelburri 0:31f7be68e52d 120
chrigelburri 0:31f7be68e52d 121 void State::setEnableLeftBit()
chrigelburri 0:31f7be68e52d 122 {
chrigelburri 0:31f7be68e52d 123 if(motorControllerLeft->isEnabled()) {
chrigelburri 0:31f7be68e52d 124 s->state &= (~STATE_LEFT);
chrigelburri 0:31f7be68e52d 125 } else {
chrigelburri 0:31f7be68e52d 126 s->state |= STATE_LEFT;
chrigelburri 0:31f7be68e52d 127 }
chrigelburri 0:31f7be68e52d 128 }
chrigelburri 0:31f7be68e52d 129
chrigelburri 0:31f7be68e52d 130 void State::setEnableRightBit()
chrigelburri 0:31f7be68e52d 131 {
chrigelburri 0:31f7be68e52d 132 if(motorControllerRight->isEnabled()) {
chrigelburri 0:31f7be68e52d 133 s->state &= (~STATE_RIGHT);
chrigelburri 0:31f7be68e52d 134 } else {
chrigelburri 0:31f7be68e52d 135 s->state |= STATE_RIGHT;
chrigelburri 0:31f7be68e52d 136 }
chrigelburri 0:31f7be68e52d 137 }
chrigelburri 0:31f7be68e52d 138
chrigelburri 0:31f7be68e52d 139 void State::startTimerFromZero()
chrigelburri 0:31f7be68e52d 140 {
chrigelburri 0:31f7be68e52d 141 timer.reset();
chrigelburri 0:31f7be68e52d 142 timer.start();
chrigelburri 0:31f7be68e52d 143 }
chrigelburri 0:31f7be68e52d 144
chrigelburri 0:31f7be68e52d 145
chrigelburri 0:31f7be68e52d 146 void State::run()
chrigelburri 0:31f7be68e52d 147 {
chrigelburri 0:31f7be68e52d 148 s->millis = timer.read_ms();
chrigelburri 0:31f7be68e52d 149 s->voltageBattery = readBattery();
chrigelburri 0:31f7be68e52d 150 s->leftPulses = - motorControllerLeft->getPulses();
chrigelburri 0:31f7be68e52d 151 s->rightPulses = motorControllerRight->getPulses();
chrigelburri 0:31f7be68e52d 152 s->leftVelocity = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
chrigelburri 0:31f7be68e52d 153 s->rightVelocity = - motorControllerRight->getActualSpeed()* 2.0f * WHEEL_RADIUS * PI;
chrigelburri 0:31f7be68e52d 154 s->velocity = robotControl->getActualSpeed();
chrigelburri 0:31f7be68e52d 155 s->omega = robotControl->getActualOmega();
chrigelburri 0:31f7be68e52d 156 s->xAxis = robotControl->getxActualPosition();
chrigelburri 0:31f7be68e52d 157 s->yAxis = robotControl->getyActualPosition();
chrigelburri 0:31f7be68e52d 158 s->angle = robotControl->getActualTheta() * 180 / PI;
chrigelburri 0:31f7be68e52d 159 s->xAxisError = robotControl->getxPositionError();
chrigelburri 0:31f7be68e52d 160 s->yAxisError = robotControl->getyPositionError();
chrigelburri 0:31f7be68e52d 161 s->angleError = robotControl->getThetaError() * 180 / PI;
chrigelburri 1:6cd533a712c6 162 // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
chrigelburri 0:31f7be68e52d 163
chrigelburri 0:31f7be68e52d 164
chrigelburri 0:31f7be68e52d 165 setBatteryBit();
chrigelburri 0:31f7be68e52d 166 setEnableLeftBit();
chrigelburri 0:31f7be68e52d 167 setEnableRightBit();
chrigelburri 0:31f7be68e52d 168
chrigelburri 0:31f7be68e52d 169 }