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:
- 12:235e318a414f
- Parent:
- 11:775ebb69d5e1
- Child:
- 14:6a45a9f940a8
diff -r 775ebb69d5e1 -r 235e318a414f RobotControl/RobotControl.cpp --- a/RobotControl/RobotControl.cpp Fri Apr 05 10:58:42 2013 +0000 +++ b/RobotControl/RobotControl.cpp Sun Apr 07 08:31:51 2013 +0000 @@ -31,6 +31,17 @@ } +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); @@ -175,67 +186,61 @@ void RobotControl::run() { -// kann glaub wegggggg - if (isEnabled()) { - } else { + // 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); } - ////bis hieerrrrr - /* position calculation */ - - /* Set the state of speed from Left und Right Wheel*/ + // 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) */ + //translational speed of the Robot (average) Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f; - /* rotational speed of the Robot */ + //rotational speed of the Robot Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE; - /* rotational theta of the Robot integrate the omega with the time*/ + //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 */ + //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 */ + //motor control if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) { - /* postition control */ - + //postition control speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() ); omega = K2 * getThetaErrorToGoal() + - K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / - (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() ); + 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) ); + 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); - } } - -float RobotControl::PiRange(float theta) -{ - if(theta <= -PI) { - return theta += 2*PI; - } else if (theta > PI) { - return theta -= 2*PI; - } else { - return theta; - } -}