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
Revision 5:48a258f6335e, committed 2013-03-21
- Comitter:
- chrigelburri
- Date:
- Thu Mar 21 08:56:53 2013 +0000
- Parent:
- 4:3a97923ff2d4
- Child:
- 6:48eeb41188dd
- Commit message:
- vor dem Gr?nden m?nd?li;
Changed in this revision
--- a/RobotControl/RobotControl.cpp Fri Mar 08 09:54:34 2013 +0000 +++ b/RobotControl/RobotControl.cpp Thu Mar 21 08:56:53 2013 +0000 @@ -61,26 +61,41 @@ this->omega = omega; } -void RobotControl::setxPosition(float xposition) +void RobotControl::setDesiredxPosition(float xposition) { Desired.xposition = xposition; } -void RobotControl::setyPosition(float yposition) +void RobotControl::setDesiredyPosition(float yposition) { Desired.yposition = yposition; } -void RobotControl::setTheta(float theta) +void RobotControl::setDesiredTheta(float theta) { Desired.theta = theta; } -void RobotControl::setPositionAngle(float xposition, float yposition, float theta) +float RobotControl::getDesiredxPosition() +{ + return Desired.xposition; +} + +float RobotControl::getDesiredyPosition() { - setxPosition(xposition); - setyPosition(yposition); - setTheta(theta); + 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() @@ -243,8 +258,8 @@ 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 ); + 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) ); } else {
--- a/RobotControl/RobotControl.h Fri Mar 08 09:54:34 2013 +0000 +++ b/RobotControl/RobotControl.h Thu Mar 21 08:56:53 2013 +0000 @@ -103,19 +103,37 @@ * Sets the desired X-position of the robot. * @param xposition the desired position, given in [m] */ - void setxPosition(float xposition); + void setDesiredxPosition(float xposition); /** * Sets the desired Y-position of the robot. * @param yposition the desired position, given in [m] */ - void setyPosition(float yposition); + void setDesiredyPosition(float yposition); /** * Sets the desired θ of the robot. * @param theta the desired θ, given in [rad] */ - void setTheta(float theta); + void setDesiredTheta(float theta); + + /** + * Get the desired X-position of the robot. + * @return xposition the desired position, given in [m] + */ + float getDesiredxPosition(); + + /** + * Get the desired Y-position of the robot. + * @return yposition the desired position, given in [m] + */ + float getDesiredyPosition(); + + /** + * Get the desired θ of the robot. + * @return theta the desired θ, given in [rad] + */ + float getDesiredTheta(); /** * Sets the desired Position and θ. @@ -123,7 +141,7 @@ * @param yposition the desired position, given in [m] * @param theta the desired θ, given in [rad] */ - void setPositionAngle(float xposition, float yposition, float theta); + void setDesiredPositionAndAngle(float xposition, float yposition, float theta); /** * Gets the desired θ of the goal point.
--- a/StateDefines/State.cpp Fri Mar 08 09:54:34 2013 +0000 +++ b/StateDefines/State.cpp Thu Mar 21 08:56:53 2013 +0000 @@ -43,17 +43,17 @@ "X-AxisError[m]\t" "AngleError[grad]\t" "AngleCar[grad]\t" - "SetpointX-Axis[m]\t" + "SetpointX-Axis[m]\t" //15 "SetpointY-Axis[m]\t" - "SetpointAngel[grad]\t" + "SetpointAngel[grad]\t" //17 "RightDistanceSensor[m]\t" "CompassAngle[grad]\t" "CompassX-Axis\t" //20 "CompassY-Axis\t" "State\t" "distanceToGoal[m]\t" //23 - "angleToGoal[rad]\t" - "thetaFromTheGoal[rad]\n"); + "angleToGoal[grad]\t" + "thetaFromTheGoal[grad]\n"); } } @@ -159,6 +159,9 @@ s->xAxisError = robotControl->getxPositionError(); s->yAxisError = robotControl->getyPositionError(); s->angleError = robotControl->getThetaError() * 180 / PI; + s->setxAxis = robotControl->getDesiredxPosition(); + s->setyAxis = robotControl->getDesiredyPosition(); + s->setAngle = robotControl->getDesiredTheta() * 180 / PI; // s->compassAngle = compass->getFilteredAngle() * 180 / PI; s->rho = robotControl->getDistanceError();
--- a/StateDefines/defines.h Fri Mar 08 09:54:34 2013 +0000 +++ b/StateDefines/defines.h Thu Mar 21 08:56:53 2013 +0000 @@ -8,7 +8,7 @@ // maxon motor #339282 EC 45 flat 30W and GEAR #define POLE_PAIRS 8u // Number of of pole pairs -#define GEAR 1.0f // Gear on the motor +#define GEAR 1/11.6f // Gear on the motor 1/11.6f #define PULSES_PER_STEP 6u // Pulses per step Hallsensor have 6 steps // Physical Dimension of the car @@ -22,8 +22,8 @@ #define STATE_RIGHT 8u // Bit3 = right ESCON in error state // ESCON Constands -#define ESCON_SET_FACTOR 100.0f // Speed Factor how set in the ESCON -#define ESCON_GET_FACTOR 100.0f // Speed Factor how get in the ESCON +#define ESCON_SET_FACTOR 1200.0f // Speed Factor how set in the ESCON +#define ESCON_GET_FACTOR 1400.0f // Speed Factor how get in the ESCON // Start Defintition #define START_X_OFFSET -0.8f // Sets the start X-point [m] @@ -38,7 +38,7 @@ #define K1 1.0f * GAIN #define K2 3.0f * GAIN #define K3 2.0f * GAIN -#define MIN_DISTANCE_ERROR 0.03 // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m] +#define MIN_DISTANCE_ERROR 0.03 // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m] // LiPo Batterie #define BAT_MULTIPLICATOR 21.633333333f // R2 / (R1 + R2) = 0.153 R2= 10k , R1 = 1.8k 1/0.153 = 6.555 ---> 3.3 * 6.555 = 21.6333333f
--- a/main.cpp Fri Mar 08 09:54:34 2013 +0000 +++ b/main.cpp Thu Mar 21 08:56:53 2013 +0000 @@ -100,37 +100,40 @@ state.startTimerFromZero(); state.start(); - robotControl.setPositionAngle(-1.20f, 1.50f, 3*PI/4); + robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); + wait(0.1); + + robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f, 3*PI/4); while(!(robotControl.getDistanceError() <= 0.4)) { state.savePlotFile(s); }; - robotControl.setPositionAngle(-1.20f, 2.6f, PI/4); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; - - robotControl.setPositionAngle(-0.45f, 3.4f, PI/2); + robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4); while(!(robotControl.getDistanceError() <= 0.4)) { state.savePlotFile(s); }; - robotControl.setPositionAngle(-1.0f, 3.6f, PI); + robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4); while(!(robotControl.getDistanceError() <= 0.4)) { state.savePlotFile(s); }; - robotControl.setPositionAngle(-1.5f, 3.9f, PI); + robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, PI); + while(!(robotControl.getDistanceError() <= 0.2)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI); while(!(robotControl.getDistanceError() <= 0.03)) { state.savePlotFile(s); }; - - robotControl.setPositionAngle(-2.5f, 3.0f, -PI/2); + + robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2); while(!(robotControl.getDistanceError() <= 0.4)) { state.savePlotFile(s); }; - - robotControl.setPositionAngle(-1.75f, 1.30f, -PI/2); + + robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2); while(!(robotControl.getDistanceError() <= 0.04)) { state.savePlotFile(s); };