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
Diff: RobotControl/RobotControl.cpp
- Revision:
- 3:92ba0254af87
- Parent:
- 2:d8e1613dc38b
- Child:
- 5:48a258f6335e
diff -r d8e1613dc38b -r 92ba0254af87 RobotControl/RobotControl.cpp --- a/RobotControl/RobotControl.cpp Sun Mar 03 16:26:47 2013 +0000 +++ b/RobotControl/RobotControl.cpp Thu Mar 07 09:47:07 2013 +0000 @@ -145,7 +145,8 @@ float RobotControl::getThetaErrorToGoal() { - float temp; + return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta()); + /*float temp; temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta(); if(temp <= -PI) { @@ -155,14 +156,14 @@ } else { //nothing } - return temp; + return temp;*/ } float RobotControl::getThetaGoal() { - float temp; - temp = atan2(getyPositionError(),getxPositionError()) - getTheta(); + return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta()); + /* if(temp <= -PI) { temp += 2* PI; } else if (temp > PI) { @@ -170,7 +171,7 @@ } else { //nothing } - return temp; + return temp;*/ } void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta) @@ -214,14 +215,16 @@ /* rotational theta of the Robot integrate the omega with the time*/ 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 = PiRange(Actual.theta); + /* + if(Actual.theta <= -PI) { + Actual.theta += 2* PI; + } else if (Actual.theta > PI) { + Actual.theta -= 2* PI; + } else { + //nothing + } + */ /* 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)); @@ -250,3 +253,15 @@ } } + +float RobotControl::PiRange(float theta) +{ + if(theta <= -PI) { + return theta += 2*PI; + } else if (theta > PI) { + return theta -= 2*PI; + } else { + return theta; + } +} +