Diff: StateDefines/State.cpp

Revision:

12:235e318a414f

Parent:

11:775ebb69d5e1

Child:

13:a7c30ee09bae

--- a/StateDefines/State.cpp	Fri Apr 05 10:58:42 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,175 +0,0 @@
-#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();
-}