File content as of revision 27:a13ede88e75f:

#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),
      led1(LED1),
      led2(LED2),
      led3(LED3),
      led4(LED4)
{
    /* 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();
}

float State::dim( int idx, float f )
{
    return (sin(f + float(idx)* 1.0) + 1.0) / 2.0;   // transform value from -1.0 .. 1.0 to 0.0 .. 1.0
}

void State::ledArray(float wait)
{
    for (float f = 0.1f; f < 6.3f; f += 0.1f) {
        led1 = dim( 0, f );
        led2 = dim( 1, f );
        led3 = dim( 2, f );
        led4 = dim( 3, f );

        wait_ms(wait);
    }
    wait_ms(wait/0.2);
    for (float f = 0.1f; f < 6.3f; f += 0.1f) {
        led1 = dim( 3, f );
        led2 = dim( 2, f );
        led3 = dim( 1, f );
        led4 = dim( 0, f );
        wait_ms(wait);
    }
    wait_ms(wait/0.2);
}

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();
}