State.cpp

00001 #include "State.h"
00002 
00003 using namespace std;
00004 
00005 // File
00006 FILE *logp;
00007 
00008 // LocalFileSystem
00009 LocalFileSystem local("local");
00010 
00011 State::State(state_t* s,
00012              RobotControl* robotControl,
00013              MaxonESCON* motorControllerLeft,
00014              MaxonESCON* motorControllerRight,
00015              PinName batteryPin,
00016              float period)
00017     : Task(period) ,
00018       battery(batteryPin)
00019 {
00020     /* get peripherals */
00021     this->s = s;
00022     this->robotControl = robotControl;
00023     this->motorControllerLeft = motorControllerLeft;
00024     this->motorControllerRight = motorControllerRight;
00025     this->battery =battery;
00026     this->period = period;
00027 }
00028 
00029 State::~State() {}
00030 
00031 void State::initPlotFile(void)
00032 {
00033     logp = fopen("/local/plots.txt", "w"); // only write
00034     if(logp == NULL) {
00035         exit(1);
00036     } else {
00037         fprintf(logp,
00038                 "Time[ms]\t"
00039                 "BatteryVoltage[V]\t"
00040                 "NumberOfPulsesLeft\t"
00041                 "NumberOfPulsesRight\t"
00042                 "VelocityLeft[m/s]\t"
00043                 "VelocityRight[m/s]\t"
00044                 "VelocityCar[m/s]\t" //7
00045                 "VelocityRotation[grad/s]\t"
00046                 "X-AxisCo-ordinate[m]\t"
00047                 "Y-AxisCo-ordinate[m]\t"
00048                 "X-AxisError[m]\t"
00049                 "X-AxisError[m]\t"
00050                 "AngleError[grad]\t"
00051                 "AngleCar[grad]\t"
00052                 "SetpointX-Axis[m]\t" //15
00053                 "SetpointY-Axis[m]\t"
00054                 "SetpointAngel[grad]\t" //17
00055                 "SetpointVelocitiy[m/s]\t"
00056                 "SetpointVelocitiyRotations[rad/s]\t"
00057                 "State\t" //20
00058                 "distanceToGoal[m]\t" //21
00059                 "angleToGoal[grad]\t"
00060                 "thetaFromTheGoal[grad]\n");
00061     }
00062 }
00063 
00064 
00065 void State::savePlotFile(state_t s)
00066 {
00067     char buf[256];
00068 
00069     sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t"
00070             "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f",
00071             s.millis,
00072             s.voltageBattery,
00073             s.leftPulses,
00074             s.rightPulses,
00075             s.leftVelocity,
00076             s.rightVelocity,
00077             s.velocity,
00078             s.omega,
00079             s.xAxis,
00080             s.yAxis,
00081             s.xAxisError,
00082             s.yAxisError,
00083             s.angleError,
00084             s.angle,
00085             s.setxAxis,
00086             s.setyAxis,
00087             s.setAngle,
00088             s.setVelocity,
00089             s.setOmega,
00090             s.state,
00091             s.rho,
00092             s.lamda,
00093             s.delta
00094            );
00095 
00096     if (logp)
00097         fprintf(logp, buf);
00098     fprintf(logp, "\n"); // new line
00099 }
00100 
00101 void State::closePlotFile(void)
00102 {
00103     fclose(logp);
00104 }
00105 
00106 float State::readBattery()
00107 {
00108     return battery.read()*BAT_MULTIPLICATOR;
00109 }
00110 
00111 void State::setBatteryBit()
00112 {
00113     if(s->voltageBattery < BAT_MIN) {
00114         s->state |= STATE_UNDER;
00115     } else {
00116         s->state &= (~STATE_UNDER);
00117     }
00118 }
00119 
00120 void State::setEnableLeftBit()
00121 {
00122     if(motorControllerLeft->isEnabled()) {
00123         s->state &= (~STATE_LEFT);
00124     } else {
00125         s->state |= STATE_LEFT;
00126     }
00127 }
00128 
00129 void State::setEnableRightBit()
00130 {
00131     if(motorControllerRight->isEnabled()) {
00132         s->state &= (~STATE_RIGHT);
00133     } else {
00134         s->state |= STATE_RIGHT;
00135     }
00136 }
00137 
00138 void State::startTimerFromZero()
00139 {
00140     timer.reset();
00141     timer.start();
00142 }
00143 
00144 float State::dim( int idx, float f )
00145 {
00146     return (sin(f + float(idx)* 1.0) + 1.0) / 2.0;   // transform value from -1.0 .. 1.0 to 0.0 .. 1.0
00147 }
00148 
00149 void State::run()
00150 {
00151     s->millis = timer.read_ms();
00152     s->voltageBattery = readBattery();
00153     s->leftPulses = - motorControllerLeft->getPulses();
00154     s->rightPulses = motorControllerRight->getPulses();
00155     s->leftVelocity = motorControllerLeft->getActualSpeed() *
00156                       2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
00157     s->rightVelocity = - motorControllerRight->getActualSpeed()*
00158                        2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
00159     s->velocity = robotControl->getActualSpeed();
00160     s->omega =  robotControl->getActualOmega();
00161     s->xAxis = robotControl->getxActualPosition();
00162     s->yAxis = robotControl->getyActualPosition();
00163     s->angle = robotControl->getActualTheta() * 180 / PI;
00164     s->xAxisError = robotControl->getxPositionError();
00165     s->yAxisError = robotControl->getyPositionError();
00166     s->angleError = robotControl->getThetaError() * 180 / PI;
00167     s->setxAxis = robotControl->getDesiredxPosition();
00168     s->setyAxis = robotControl->getDesiredyPosition();
00169     s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
00170     s->setVelocity = robotControl->getDesiredSpeed();
00171     s->setOmega = robotControl->getDesiredOmega();
00172 
00173     s->rho = robotControl->getDistanceError();
00174     s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
00175     s->delta = robotControl->getThetaGoal() * 180 / PI;
00176 
00177     setBatteryBit();
00178     setEnableLeftBit();
00179     setEnableRightBit();
00180     //savePlotFile(*s);
00181     
00182 }