This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> 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: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
Fork of autonomousRobotAndroid by
Diff: RobotControl/RobotControl.cpp
- Revision:
- 0:31f7be68e52d
- Child:
- 1:6cd533a712c6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RobotControl/RobotControl.cpp Thu Feb 07 17:43:19 2013 +0000 @@ -0,0 +1,210 @@ +#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); + + } +} +