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

Revision:
0:31f7be68e52d
Child:
1:6cd533a712c6
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateDefines/State.cpp	Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,169 @@
+#include "State.h"
+
+using namespace std;
+
+// File
+FILE *logp;
+
+// LocalFileSystem
+LocalFileSystem local("local");
+
+State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, AnalogIn* battery, float period) : Task(period)
+{
+    /* get peripherals */
+    this->s = s;
+    this->robotControl = robotControl;
+    this->motorControllerLeft = motorControllerLeft;
+    this->motorControllerRight = motorControllerRight;
+    this->compass = compass;
+    this->battery =battery;
+    this->period = period;
+
+}
+
+
+State::~State() {}
+
+void State::initPlotFile(void)
+{
+    logp = fopen("/local/plots.txt", "w"); // only write
+    if(logp == NULL) {
+        exit(1);
+    } else {
+        fprintf(logp,
+                "Time[ms]\t"
+                "BatteryVoltage[V]\t"
+                "NumberOfPulsesLeft\t"
+                "NumberOfPulsesRight\t"
+                "VelocityLeft[m/s]\t"
+                "VelocityRight[m/s]\t"
+                "VelocityCar[m/s]\t"
+                "VelocityRotation[grad/s]\t"
+                "X-AxisCo-ordinate[m]\t"
+                "Y-AxisCo-ordinate[m]\t"
+                "X-AxisError[m]\t"
+                "X-AxisError[m]\t"
+                "AngleError[grad]\t"
+                "AngleCar[grad]\t"
+                "SetpointX-Axis[m]\t"
+                "SetpointY-Axis[m]\t"
+                "SetpointAngel[grad]\t"
+                "RightDistanceSensor[m]\t"
+                "CompassAngle[grad]\t"
+                "CompassX-Axis\t"
+                "CompassY-Axis\t"
+                "State\n");
+    }
+}
+
+
+void State::savePlotFile(state_t s)
+{
+    char buf[256];
+
+    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",
+            s.millis,
+            s.voltageBattery,
+            s.leftPulses,
+            s.rightPulses,
+            s.leftVelocity,
+            s.rightVelocity,
+            s.velocity,
+            s.omega,
+            s.xAxis,
+            s.yAxis,
+            s.xAxisError,
+            s.yAxisError,
+            s.angleError,
+            s.angle,
+            s.setxAxis,
+            s.setyAxis,
+            s.setAngle,
+            s.rightDist,
+            s.compassAngle,
+            s.compassxAxis,
+            s.compassyAxis,
+            s.state
+           );
+
+    if (logp)
+        fprintf(logp, buf);
+    fprintf(logp, "\n"); // new line
+
+}
+
+void State::savePlotText(char text[])
+{
+    fprintf(logp, text);
+}
+
+
+void State::closePlotFile(void)
+{
+    fclose(logp);
+
+}
+
+float State::readBattery()
+{
+    return battery->read()*BAT_MULTIPLICATOR;
+}
+
+void State::setBatteryBit()
+{
+    if(s->voltageBattery < BAT_MIN) {
+        s->state |= STATE_UNDER;
+    } else {
+        s->state &= (~STATE_UNDER);
+    }
+}
+
+void State::setEnableLeftBit()
+{
+    if(motorControllerLeft->isEnabled()) {
+        s->state &= (~STATE_LEFT);
+    } else {
+        s->state |= STATE_LEFT;
+    }
+}
+
+void State::setEnableRightBit()
+{
+    if(motorControllerRight->isEnabled()) {
+        s->state &= (~STATE_RIGHT);
+    } else {
+        s->state |= STATE_RIGHT;
+    }
+}
+
+void State::startTimerFromZero()
+{
+    timer.reset();
+    timer.start();
+}
+
+
+void State::run()
+{
+    s->millis = timer.read_ms();
+    s->voltageBattery = readBattery();
+    s->leftPulses = - motorControllerLeft->getPulses();
+    s->rightPulses = motorControllerRight->getPulses();
+    s->leftVelocity = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+    s->rightVelocity = - motorControllerRight->getActualSpeed()* 2.0f * WHEEL_RADIUS * PI;
+    s->velocity = robotControl->getActualSpeed();
+    s->omega =  robotControl->getActualOmega();
+    s->xAxis = robotControl->getxActualPosition();
+    s->yAxis = robotControl->getyActualPosition();
+    s->angle = robotControl->getActualTheta() * 180 / PI;
+    s->xAxisError = robotControl->getxPositionError();
+    s->yAxisError = robotControl->getyPositionError();
+    s->angleError = robotControl->getThetaError() * 180 / PI;
+    s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+
+
+    setBatteryBit();
+    setEnableLeftBit();
+    setEnableRightBit();
+
+}