![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
State/State.cpp
- Committer:
- chrigelburri
- Date:
- 2013-05-19
- Revision:
- 24:08241be546ba
- Parent:
- 12:235e318a414f
- Child:
- 27:a13ede88e75f
- Child:
- 29:937c74ff040e
File content as of revision 24:08241be546ba:
#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(); } 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::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(); }