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
RobotControl/RobotControl.cpp
- Committer:
- chrigelburri
- Date:
- 2013-04-07
- Revision:
- 12:235e318a414f
- Parent:
- 11:775ebb69d5e1
- Child:
- 14:6a45a9f940a8
File content as of revision 12:235e318a414f:
#include "RobotControl.h" using namespace std; RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, float period) : Task(period) { /* get peripherals */ this->motorControllerLeft = motorControllerLeft; this->motorControllerRight = motorControllerRight; 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); motorControllerLeft->setVelocity(0.0f); motorControllerRight->setVelocity(0.0f); } RobotControl::~RobotControl() { } float RobotControl::PiRange(float theta) { if(theta <= -PI) { return theta += 2*PI; } else if (theta > PI) { return theta -= 2*PI; } else { return theta; } } void RobotControl::setEnable(bool enable) { motorControllerLeft->enable(enable); motorControllerRight->enable(enable); } bool RobotControl::isEnabled() { return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled()); } void RobotControl::setDesiredSpeed(float speed) { this->speed = speed; } void RobotControl::setDesiredOmega(float omega) { this->omega = omega; } void RobotControl::setDesiredxPosition(float xposition) { Desired.xposition = xposition; } void RobotControl::setDesiredyPosition(float yposition) { Desired.yposition = yposition; } void RobotControl::setDesiredTheta(float theta) { Desired.theta = theta; } float RobotControl::getDesiredxPosition() { return Desired.xposition; } float RobotControl::getDesiredyPosition() { return Desired.yposition; } float RobotControl::getDesiredTheta() { return Desired.theta; } void RobotControl::setDesiredPositionAndAngle(float xposition, float yposition, float theta) { setDesiredxPosition(xposition); setDesiredyPosition(yposition); setDesiredTheta(theta); } float RobotControl::getTheta() { return Desired.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(); } float RobotControl::getDistanceError() { return sqrt( ( getxPositionError() * getxPositionError() ) + (getyPositionError() * getyPositionError() ) ); } float RobotControl::getThetaErrorToGoal() { return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta()); } float RobotControl::getThetaGoal() { return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta()); } void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta) { Actual.setState(xZeroPos, yZeroPos, theta, 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() { // When the Motor is note enable set the desired speed to the acutal speed. // only used by setting the speed and omega via the WII control. if (!isEnabled()) { 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_LEFT * PI * GEAR; stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ; //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 integrate the omega with the time Actual.theta += Actual.omega * period; Actual.theta = PiRange(Actual.theta); //translational X and Y Position. integrate the speed with the time Actual.xposition += (Actual.speed * period * cos(Actual.theta)); Actual.yposition += (Actual.speed * period * sin(Actual.theta)); //motor control if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) { //postition control speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() ); omega = K2 * getThetaErrorToGoal() + K1 * ( ( sin(getThetaErrorToGoal() ) * cos(getThetaErrorToGoal() ) ) / getThetaErrorToGoal() ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() ); motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS_LEFT * PI * GEAR) ); motorControllerRight->setVelocity( -( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS_RIGHT * PI * GEAR) ); } else { motorControllerLeft->setVelocity(0.0f); motorControllerRight->setVelocity(0.0f); } }