Diff: State/State.cpp

Revision:

12:235e318a414f

Parent:

11:775ebb69d5e1

Child:

23:4d8173c5183b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/State/State.cpp	Sun Apr 07 08:31:51 2013 +0000
@@ -0,0 +1,175 @@
+#include "State.h"
+
+using namespace std;
+
+// File
+FILE *logp;
+
+// LocalFileSystem
+LocalFileSystem local("local");
+
+State::State(state_t* s,
+             RobotControl* robotControl,
+             MaxonESCON* motorControllerLeft,
+             MaxonESCON* motorControllerRight,
+             PinName batteryPin,
+             float period)
+    : Task(period) ,
+      battery(batteryPin)
+{
+    /* get peripherals */
+    this->s = s;
+    this->robotControl = robotControl;
+    this->motorControllerLeft = motorControllerLeft;
+    this->motorControllerRight = motorControllerRight;
+    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" //7
+                "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" //15
+                "SetpointY-Axis[m]\t"
+                "SetpointAngel[grad]\t" //17
+                "SetpointVelocitiy[m/s]\t"
+                "SetpointVelocitiyRotations[rad/s]\t"
+                "State\t" //20
+                "distanceToGoal[m]\t" //21
+                "angleToGoal[grad]\t"
+                "thetaFromTheGoal[grad]\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%d\t%f\t%f\t%f",
+            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.setVelocity,
+            s.setOmega,
+            s.state,
+            s.rho,
+            s.lamda,
+            s.delta
+           );
+
+    if (logp)
+        fprintf(logp, buf);
+    fprintf(logp, "\n"); // new line
+}
+
+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_LEFT * PI * GEAR;
+    s->rightVelocity = - motorControllerRight->getActualSpeed()*
+                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
+    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->setxAxis = robotControl->getDesiredxPosition();
+    s->setyAxis = robotControl->getDesiredyPosition();
+    s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
+    s->setVelocity = robotControl->getDesiredSpeed();
+    s->setOmega = robotControl->getDesiredOmega();
+
+    s->rho = robotControl->getDistanceError();
+    s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
+    s->delta = robotControl->getThetaGoal() * 180 / PI;
+
+    setBatteryBit();
+    setEnableLeftBit();
+    setEnableRightBit();
+}