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
Fork of autonomous Robot Android by
Diff: StateDefines/State.cpp
- Revision:
- 0:31f7be68e52d
- Child:
- 1:6cd533a712c6
diff -r 000000000000 -r 31f7be68e52d StateDefines/State.cpp --- /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(); + +}