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:
- 2:d8e1613dc38b
- Parent:
- 1:6cd533a712c6
- Child:
- 3:92ba0254af87
--- a/RobotControl/RobotControl.cpp Sat Mar 02 09:39:34 2013 +0000 +++ b/RobotControl/RobotControl.cpp Sun Mar 03 16:26:47 2013 +0000 @@ -23,7 +23,6 @@ Desired.setAcceleration(ACCELERATION); Desired.setThetaAcceleration(THETA_ACCELERATION); - } RobotControl::~RobotControl() @@ -77,6 +76,13 @@ Desired.theta = theta; } +void RobotControl::setPositionAngle(float xposition, float yposition, float theta) +{ + setxPosition(xposition); + setyPosition(yposition); + setTheta(theta); +} + float RobotControl::getTheta() { return Desired.theta; @@ -139,12 +145,32 @@ float RobotControl::getThetaErrorToGoal() { - return atan2(getyPositionError(),getxPositionError()) - getActualTheta(); // PI ist weg weil auch negative Zahlen zugelassen werden. + float temp; + temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta(); + + if(temp <= -PI) { + temp += 2* PI; + } else if (temp > PI) { + temp -= 2* PI; + } else { + //nothing + } + return temp; } float RobotControl::getThetaGoal() { - return atan2(getyPositionError(),getxPositionError()) - getTheta(); + float temp; + temp = atan2(getyPositionError(),getxPositionError()) - getTheta(); + + if(temp <= -PI) { + temp += 2* PI; + } else if (temp > PI) { + temp -= 2* PI; + } else { + //nothing + } + return temp; } void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta) @@ -157,17 +183,16 @@ omega = 0.0f; } - void RobotControl::run() { ///// DAs kan glaub raus ab hier - +/////////////////////////////////////////////////////////7 /* motion planning */ if (isEnabled()) { ///// DAs kan glaub raus bis hier Desired.increment(speed, omega, period); - ///// DAs kan glaub raus vis hier + ///// DAs kan glaub raus bis hier } else { speed = 0.0f; @@ -201,19 +226,19 @@ Actual.xposition += (Actual.speed * period * cos(Actual.theta)); Actual.yposition += (Actual.speed * period * sin(Actual.theta)); - // Actual.thetaCompass = compass->getFilteredAngle(); /* translational X and Y Position. integrate the speed with the time theta from compass */ // Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass)); // Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass)); - + /* 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() ); + omega = K2 * getThetaErrorToGoal() + + K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() ); motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) * GEAR ); motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) * GEAR ); @@ -225,4 +250,3 @@ } } -