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@2:d8e1613dc38b, 2013-03-03 (annotated)

Committer:
chrigelburri
Date:
Sun Mar 03 16:26:47 2013 +0000
Revision:
2:d8e1613dc38b
Parent:
1:6cd533a712c6
Child:
4:3a97923ff2d4
Viereck Fahren; Code aufger?umt und eine setter methode progammiert f?r sollwert

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 2:d8e1613dc38b 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 State::~State() {}
chrigelburri 0:31f7be68e52d 24
chrigelburri 0:31f7be68e52d 25 void State::initPlotFile(void)
chrigelburri 0:31f7be68e52d 26 {
chrigelburri 0:31f7be68e52d 27 logp = fopen("/local/plots.txt", "w"); // only write
chrigelburri 0:31f7be68e52d 28 if(logp == NULL) {
chrigelburri 0:31f7be68e52d 29 exit(1);
chrigelburri 0:31f7be68e52d 30 } else {
chrigelburri 0:31f7be68e52d 31 fprintf(logp,
chrigelburri 0:31f7be68e52d 32 "Time[ms]\t"
chrigelburri 0:31f7be68e52d 33 "BatteryVoltage[V]\t"
chrigelburri 0:31f7be68e52d 34 "NumberOfPulsesLeft\t"
chrigelburri 0:31f7be68e52d 35 "NumberOfPulsesRight\t"
chrigelburri 0:31f7be68e52d 36 "VelocityLeft[m/s]\t"
chrigelburri 0:31f7be68e52d 37 "VelocityRight[m/s]\t"
chrigelburri 0:31f7be68e52d 38 "VelocityCar[m/s]\t"
chrigelburri 0:31f7be68e52d 39 "VelocityRotation[grad/s]\t"
chrigelburri 0:31f7be68e52d 40 "X-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 41 "Y-AxisCo-ordinate[m]\t"
chrigelburri 0:31f7be68e52d 42 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 43 "X-AxisError[m]\t"
chrigelburri 0:31f7be68e52d 44 "AngleError[grad]\t"
chrigelburri 0:31f7be68e52d 45 "AngleCar[grad]\t"
chrigelburri 0:31f7be68e52d 46 "SetpointX-Axis[m]\t"
chrigelburri 0:31f7be68e52d 47 "SetpointY-Axis[m]\t"
chrigelburri 0:31f7be68e52d 48 "SetpointAngel[grad]\t"
chrigelburri 0:31f7be68e52d 49 "RightDistanceSensor[m]\t"
chrigelburri 0:31f7be68e52d 50 "CompassAngle[grad]\t"
chrigelburri 2:d8e1613dc38b 51 "CompassX-Axis\t" //20
chrigelburri 0:31f7be68e52d 52 "CompassY-Axis\t"
chrigelburri 2:d8e1613dc38b 53 "State\t"
chrigelburri 2:d8e1613dc38b 54 "distanceToGoal[m]\t" //23
chrigelburri 2:d8e1613dc38b 55 "angleToGoal[rad]\t"
chrigelburri 2:d8e1613dc38b 56 "thetaFromTheGoal[rad]\n");
chrigelburri 0:31f7be68e52d 57 }
chrigelburri 0:31f7be68e52d 58 }
chrigelburri 0:31f7be68e52d 59
chrigelburri 0:31f7be68e52d 60
chrigelburri 0:31f7be68e52d 61 void State::savePlotFile(state_t s)
chrigelburri 0:31f7be68e52d 62 {
chrigelburri 0:31f7be68e52d 63 char buf[256];
chrigelburri 0:31f7be68e52d 64
chrigelburri 2:d8e1613dc38b 65 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\t%f\t%f\t%f",
chrigelburri 0:31f7be68e52d 66 s.millis,
chrigelburri 0:31f7be68e52d 67 s.voltageBattery,
chrigelburri 0:31f7be68e52d 68 s.leftPulses,
chrigelburri 0:31f7be68e52d 69 s.rightPulses,
chrigelburri 0:31f7be68e52d 70 s.leftVelocity,
chrigelburri 0:31f7be68e52d 71 s.rightVelocity,
chrigelburri 0:31f7be68e52d 72 s.velocity,
chrigelburri 0:31f7be68e52d 73 s.omega,
chrigelburri 0:31f7be68e52d 74 s.xAxis,
chrigelburri 0:31f7be68e52d 75 s.yAxis,
chrigelburri 0:31f7be68e52d 76 s.xAxisError,
chrigelburri 0:31f7be68e52d 77 s.yAxisError,
chrigelburri 0:31f7be68e52d 78 s.angleError,
chrigelburri 0:31f7be68e52d 79 s.angle,
chrigelburri 0:31f7be68e52d 80 s.setxAxis,
chrigelburri 0:31f7be68e52d 81 s.setyAxis,
chrigelburri 0:31f7be68e52d 82 s.setAngle,
chrigelburri 0:31f7be68e52d 83 s.rightDist,
chrigelburri 0:31f7be68e52d 84 s.compassAngle,
chrigelburri 0:31f7be68e52d 85 s.compassxAxis,
chrigelburri 0:31f7be68e52d 86 s.compassyAxis,
chrigelburri 2:d8e1613dc38b 87 s.state,
chrigelburri 2:d8e1613dc38b 88 s.rho,
chrigelburri 2:d8e1613dc38b 89 s.lamda,
chrigelburri 2:d8e1613dc38b 90 s.delta
chrigelburri 0:31f7be68e52d 91 );
chrigelburri 0:31f7be68e52d 92
chrigelburri 0:31f7be68e52d 93 if (logp)
chrigelburri 0:31f7be68e52d 94 fprintf(logp, buf);
chrigelburri 0:31f7be68e52d 95 fprintf(logp, "\n"); // new line
chrigelburri 0:31f7be68e52d 96 }
chrigelburri 0:31f7be68e52d 97
chrigelburri 0:31f7be68e52d 98 void State::savePlotText(char text[])
chrigelburri 0:31f7be68e52d 99 {
chrigelburri 0:31f7be68e52d 100 fprintf(logp, text);
chrigelburri 0:31f7be68e52d 101 }
chrigelburri 0:31f7be68e52d 102
chrigelburri 0:31f7be68e52d 103 void State::closePlotFile(void)
chrigelburri 0:31f7be68e52d 104 {
chrigelburri 0:31f7be68e52d 105 fclose(logp);
chrigelburri 0:31f7be68e52d 106 }
chrigelburri 0:31f7be68e52d 107
chrigelburri 0:31f7be68e52d 108 float State::readBattery()
chrigelburri 0:31f7be68e52d 109 {
chrigelburri 0:31f7be68e52d 110 return battery->read()*BAT_MULTIPLICATOR;
chrigelburri 0:31f7be68e52d 111 }
chrigelburri 0:31f7be68e52d 112
chrigelburri 0:31f7be68e52d 113 void State::setBatteryBit()
chrigelburri 0:31f7be68e52d 114 {
chrigelburri 0:31f7be68e52d 115 if(s->voltageBattery < BAT_MIN) {
chrigelburri 0:31f7be68e52d 116 s->state |= STATE_UNDER;
chrigelburri 0:31f7be68e52d 117 } else {
chrigelburri 0:31f7be68e52d 118 s->state &= (~STATE_UNDER);
chrigelburri 0:31f7be68e52d 119 }
chrigelburri 0:31f7be68e52d 120 }
chrigelburri 0:31f7be68e52d 121
chrigelburri 0:31f7be68e52d 122 void State::setEnableLeftBit()
chrigelburri 0:31f7be68e52d 123 {
chrigelburri 0:31f7be68e52d 124 if(motorControllerLeft->isEnabled()) {
chrigelburri 0:31f7be68e52d 125 s->state &= (~STATE_LEFT);
chrigelburri 0:31f7be68e52d 126 } else {
chrigelburri 0:31f7be68e52d 127 s->state |= STATE_LEFT;
chrigelburri 0:31f7be68e52d 128 }
chrigelburri 0:31f7be68e52d 129 }
chrigelburri 0:31f7be68e52d 130
chrigelburri 0:31f7be68e52d 131 void State::setEnableRightBit()
chrigelburri 0:31f7be68e52d 132 {
chrigelburri 0:31f7be68e52d 133 if(motorControllerRight->isEnabled()) {
chrigelburri 0:31f7be68e52d 134 s->state &= (~STATE_RIGHT);
chrigelburri 0:31f7be68e52d 135 } else {
chrigelburri 0:31f7be68e52d 136 s->state |= STATE_RIGHT;
chrigelburri 0:31f7be68e52d 137 }
chrigelburri 0:31f7be68e52d 138 }
chrigelburri 0:31f7be68e52d 139
chrigelburri 0:31f7be68e52d 140 void State::startTimerFromZero()
chrigelburri 0:31f7be68e52d 141 {
chrigelburri 0:31f7be68e52d 142 timer.reset();
chrigelburri 0:31f7be68e52d 143 timer.start();
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 2:d8e1613dc38b 162 // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
chrigelburri 0:31f7be68e52d 163
chrigelburri 2:d8e1613dc38b 164 s->rho = robotControl->getDistanceError();
chrigelburri 2:d8e1613dc38b 165 s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
chrigelburri 2:d8e1613dc38b 166 s->delta = robotControl->getThetaGoal() * 180 / PI;
chrigelburri 0:31f7be68e52d 167
chrigelburri 0:31f7be68e52d 168 setBatteryBit();
chrigelburri 0:31f7be68e52d 169 setEnableLeftBit();
chrigelburri 0:31f7be68e52d 170 setEnableRightBit();
chrigelburri 0:31f7be68e52d 171
chrigelburri 0:31f7be68e52d 172 }