Christian Burri
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 
Christian Burri
Diff: RobotControl/RobotControl.cpp
- Revision:
- 12:235e318a414f
- Parent:
- 11:775ebb69d5e1
- Child:
- 14:6a45a9f940a8
--- 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;
- }
-}