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
RobotControl/RobotControl.cpp
- Committer:
- chrigelburri
- Date:
- 2013-02-07
- Revision:
- 0:31f7be68e52d
- Child:
- 1:6cd533a712c6
File content as of revision 0:31f7be68e52d:
#include "RobotControl.h" using namespace std; RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period) : Task(period) { /* get peripherals */ this->motorControllerLeft = motorControllerLeft; this->motorControllerRight = motorControllerRight; this->compass = compass; this->period = period; /* initialize peripherals */ motorControllerLeft->enable(false); motorControllerRight->enable(false); /* initialize remaining state values */ speed = 0.0f; omega = 0.0f; motorControllerLeft->setPulses(0); motorControllerRight->setPulses(0); Desired.setAcceleration(ACCELERATION); Desired.setThetaAcceleration(THETA_ACCELERATION); } RobotControl::~RobotControl() { } void RobotControl::setEnable(bool enable) { motorControllerLeft->enable(enable); motorControllerRight->enable(enable); } bool RobotControl::isEnabled() { return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled()); } void RobotControl::setAcceleration(float acc) { Desired.setAcceleration(acc); } void RobotControl::setThetaAcceleration(float acc) { Desired.setThetaAcceleration(acc); } void RobotControl::setDesiredSpeed(float speed) { this->speed = speed; } void RobotControl::setDesiredOmega(float omega) { this->omega = omega; } void RobotControl::setxPosition(float position) { Desired.xposition = position; } void RobotControl::setyPosition(float position) { Desired.yposition = position; } void RobotControl::setTheta(float theta) { Desired.theta = theta; } float RobotControl::getDesiredSpeed() { return speed; } float RobotControl::getActualSpeed() { return Actual.speed; } float RobotControl::getDesiredOmega() { return omega; } float RobotControl::getActualOmega() { return Actual.omega; } float RobotControl::getxActualPosition() { return Actual.getxPosition(); } float RobotControl::getxPositionError() { return Desired.getxPosition()-Actual.getxPosition(); } float RobotControl::getyActualPosition() { return Actual.getyPosition(); } float RobotControl::getyPositionError() { return Desired.getyPosition()-Actual.getyPosition(); } float RobotControl::getActualTheta() { return Actual.getTheta(); } float RobotControl::getThetaError() { return Desired.getTheta()-Actual.getTheta(); } void RobotControl::setAllToZero(float xZeroPos, float yZeroPos) { Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f); Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f); speed = 0.0f; omega = 0.0f; } void RobotControl::run() { /* motion planning */ if (isEnabled()) { Desired.increment(speed, omega, period); } else { speed = 0.0f; omega = 0.0f; Desired.setState(&Actual); } /* position calculation */ /* Set the state of speed from Left und Right Wheel*/ stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI; stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI; /* translational speed of the Robot (average) */ Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f; /* rotational speed of the Robot */ Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE; /* rotational theta of the Robot */ Actual.theta += Actual.omega * period; if(Actual.theta <= -PI) { Actual.theta += 2* PI; } else if (Actual.theta > PI) { Actual.theta -= 2* PI; } else { //nothing } Actual.theta += Actual.omega * period; Actual.thetaCompass = compass->getFilteredAngle(); /* translational X and Y Position. integrate the speed with the time */ // Actual.xposition += (Actual.speed * period * sin(Actual.theta)); // Actual.yposition += (Actual.speed * period * cos(Actual.theta)); /* translational X and Y Position. integrate the speed with the time theta from compass */ Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass)); Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass)); /* postition control calculate */ rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2)); /* motor control */ if (isEnabled()) { motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) ); motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) ); } else { motorControllerLeft->setVelocity(0.0f); motorControllerRight->setVelocity(0.0f); } }