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@6:48eeb41188dd, 2013-03-23 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 23 13:52:48 2013 +0000
Revision:
6:48eeb41188dd
Parent:
5:48a258f6335e
Child:
8:696c2f9dfc62
mit link und rechten Radradius

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 4:3a97923ff2d4 38 "VelocityCar[m/s]\t" //7
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 5:48a258f6335e 46 "SetpointX-Axis[m]\t" //15
chrigelburri 0:31f7be68e52d 47 "SetpointY-Axis[m]\t"
chrigelburri 5:48a258f6335e 48 "SetpointAngel[grad]\t" //17
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 5:48a258f6335e 55 "angleToGoal[grad]\t"
chrigelburri 5:48a258f6335e 56 "thetaFromTheGoal[grad]\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 6:48eeb41188dd 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"
chrigelburri 6:48eeb41188dd 66 "%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 67 s.millis,
chrigelburri 0:31f7be68e52d 68 s.voltageBattery,
chrigelburri 0:31f7be68e52d 69 s.leftPulses,
chrigelburri 0:31f7be68e52d 70 s.rightPulses,
chrigelburri 0:31f7be68e52d 71 s.leftVelocity,
chrigelburri 0:31f7be68e52d 72 s.rightVelocity,
chrigelburri 0:31f7be68e52d 73 s.velocity,
chrigelburri 0:31f7be68e52d 74 s.omega,
chrigelburri 0:31f7be68e52d 75 s.xAxis,
chrigelburri 0:31f7be68e52d 76 s.yAxis,
chrigelburri 0:31f7be68e52d 77 s.xAxisError,
chrigelburri 0:31f7be68e52d 78 s.yAxisError,
chrigelburri 0:31f7be68e52d 79 s.angleError,
chrigelburri 0:31f7be68e52d 80 s.angle,
chrigelburri 0:31f7be68e52d 81 s.setxAxis,
chrigelburri 0:31f7be68e52d 82 s.setyAxis,
chrigelburri 0:31f7be68e52d 83 s.setAngle,
chrigelburri 0:31f7be68e52d 84 s.rightDist,
chrigelburri 0:31f7be68e52d 85 s.compassAngle,
chrigelburri 0:31f7be68e52d 86 s.compassxAxis,
chrigelburri 0:31f7be68e52d 87 s.compassyAxis,
chrigelburri 2:d8e1613dc38b 88 s.state,
chrigelburri 2:d8e1613dc38b 89 s.rho,
chrigelburri 2:d8e1613dc38b 90 s.lamda,
chrigelburri 2:d8e1613dc38b 91 s.delta
chrigelburri 0:31f7be68e52d 92 );
chrigelburri 0:31f7be68e52d 93
chrigelburri 0:31f7be68e52d 94 if (logp)
chrigelburri 0:31f7be68e52d 95 fprintf(logp, buf);
chrigelburri 0:31f7be68e52d 96 fprintf(logp, "\n"); // new line
chrigelburri 0:31f7be68e52d 97 }
chrigelburri 0:31f7be68e52d 98
chrigelburri 0:31f7be68e52d 99 void State::savePlotText(char text[])
chrigelburri 0:31f7be68e52d 100 {
chrigelburri 0:31f7be68e52d 101 fprintf(logp, text);
chrigelburri 0:31f7be68e52d 102 }
chrigelburri 0:31f7be68e52d 103
chrigelburri 0:31f7be68e52d 104 void State::closePlotFile(void)
chrigelburri 0:31f7be68e52d 105 {
chrigelburri 0:31f7be68e52d 106 fclose(logp);
chrigelburri 0:31f7be68e52d 107 }
chrigelburri 0:31f7be68e52d 108
chrigelburri 0:31f7be68e52d 109 float State::readBattery()
chrigelburri 0:31f7be68e52d 110 {
chrigelburri 0:31f7be68e52d 111 return battery->read()*BAT_MULTIPLICATOR;
chrigelburri 0:31f7be68e52d 112 }
chrigelburri 0:31f7be68e52d 113
chrigelburri 0:31f7be68e52d 114 void State::setBatteryBit()
chrigelburri 0:31f7be68e52d 115 {
chrigelburri 0:31f7be68e52d 116 if(s->voltageBattery < BAT_MIN) {
chrigelburri 0:31f7be68e52d 117 s->state |= STATE_UNDER;
chrigelburri 0:31f7be68e52d 118 } else {
chrigelburri 0:31f7be68e52d 119 s->state &= (~STATE_UNDER);
chrigelburri 0:31f7be68e52d 120 }
chrigelburri 0:31f7be68e52d 121 }
chrigelburri 0:31f7be68e52d 122
chrigelburri 0:31f7be68e52d 123 void State::setEnableLeftBit()
chrigelburri 0:31f7be68e52d 124 {
chrigelburri 0:31f7be68e52d 125 if(motorControllerLeft->isEnabled()) {
chrigelburri 0:31f7be68e52d 126 s->state &= (~STATE_LEFT);
chrigelburri 0:31f7be68e52d 127 } else {
chrigelburri 0:31f7be68e52d 128 s->state |= STATE_LEFT;
chrigelburri 0:31f7be68e52d 129 }
chrigelburri 0:31f7be68e52d 130 }
chrigelburri 0:31f7be68e52d 131
chrigelburri 0:31f7be68e52d 132 void State::setEnableRightBit()
chrigelburri 0:31f7be68e52d 133 {
chrigelburri 0:31f7be68e52d 134 if(motorControllerRight->isEnabled()) {
chrigelburri 0:31f7be68e52d 135 s->state &= (~STATE_RIGHT);
chrigelburri 0:31f7be68e52d 136 } else {
chrigelburri 0:31f7be68e52d 137 s->state |= STATE_RIGHT;
chrigelburri 0:31f7be68e52d 138 }
chrigelburri 0:31f7be68e52d 139 }
chrigelburri 0:31f7be68e52d 140
chrigelburri 0:31f7be68e52d 141 void State::startTimerFromZero()
chrigelburri 0:31f7be68e52d 142 {
chrigelburri 0:31f7be68e52d 143 timer.reset();
chrigelburri 0:31f7be68e52d 144 timer.start();
chrigelburri 0:31f7be68e52d 145 }
chrigelburri 0:31f7be68e52d 146
chrigelburri 0:31f7be68e52d 147 void State::run()
chrigelburri 0:31f7be68e52d 148 {
chrigelburri 0:31f7be68e52d 149 s->millis = timer.read_ms();
chrigelburri 0:31f7be68e52d 150 s->voltageBattery = readBattery();
chrigelburri 0:31f7be68e52d 151 s->leftPulses = - motorControllerLeft->getPulses();
chrigelburri 0:31f7be68e52d 152 s->rightPulses = motorControllerRight->getPulses();
chrigelburri 6:48eeb41188dd 153 s->leftVelocity = motorControllerLeft->getActualSpeed() *
chrigelburri 6:48eeb41188dd 154 2.0f * WHEEL_RADIUS_LEFT * PI;
chrigelburri 6:48eeb41188dd 155 s->rightVelocity = - motorControllerRight->getActualSpeed()*
chrigelburri 6:48eeb41188dd 156 2.0f * WHEEL_RADIUS_RIGHT * PI;
chrigelburri 0:31f7be68e52d 157 s->velocity = robotControl->getActualSpeed();
chrigelburri 0:31f7be68e52d 158 s->omega = robotControl->getActualOmega();
chrigelburri 0:31f7be68e52d 159 s->xAxis = robotControl->getxActualPosition();
chrigelburri 0:31f7be68e52d 160 s->yAxis = robotControl->getyActualPosition();
chrigelburri 0:31f7be68e52d 161 s->angle = robotControl->getActualTheta() * 180 / PI;
chrigelburri 0:31f7be68e52d 162 s->xAxisError = robotControl->getxPositionError();
chrigelburri 0:31f7be68e52d 163 s->yAxisError = robotControl->getyPositionError();
chrigelburri 0:31f7be68e52d 164 s->angleError = robotControl->getThetaError() * 180 / PI;
chrigelburri 5:48a258f6335e 165 s->setxAxis = robotControl->getDesiredxPosition();
chrigelburri 5:48a258f6335e 166 s->setyAxis = robotControl->getDesiredyPosition();
chrigelburri 5:48a258f6335e 167 s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
chrigelburri 2:d8e1613dc38b 168 // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
chrigelburri 0:31f7be68e52d 169
chrigelburri 2:d8e1613dc38b 170 s->rho = robotControl->getDistanceError();
chrigelburri 2:d8e1613dc38b 171 s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
chrigelburri 2:d8e1613dc38b 172 s->delta = robotControl->getThetaGoal() * 180 / PI;
chrigelburri 0:31f7be68e52d 173
chrigelburri 0:31f7be68e52d 174 setBatteryBit();
chrigelburri 0:31f7be68e52d 175 setEnableLeftBit();
chrigelburri 0:31f7be68e52d 176 setEnableRightBit();
chrigelburri 0:31f7be68e52d 177 }