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 8:696c2f9dfc62, committed 2013-03-30
- Comitter:
- chrigelburri
- Date:
- Sat Mar 30 06:55:08 2013 +0000
- Parent:
- 7:34be8b3a979c
- Child:
- 9:d3cdcdef9719
- Commit message:
- vor dem tuning. nicht merh 50% duty cicle und 1.65V mitte
Changed in this revision
--- a/Actuators/MaxonESCON.cpp Tue Mar 26 08:29:43 2013 +0000 +++ b/Actuators/MaxonESCON.cpp Sat Mar 30 06:55:08 2013 +0000 @@ -36,17 +36,17 @@ { speed = speed / ESCON_SET_FACTOR * 60.0f; if(speed > 1 ) { - _pwm = 0.9f; + _pwm = 0.89f; } else if(speed < -1) { - _pwm = 0.1f; + _pwm = 0.11f; } else { - _pwm = 0.4f*speed + 0.5f; + _pwm = 0.4f*speed + (0.5f * (SET_SPEED_PATCH)); } } float MaxonESCON::getActualSpeed(void) { - return (_actualSpeed.read()* 2.0f - 1.0f) * ESCON_GET_FACTOR / 60.0f; + return (_actualSpeed.read()* 2.0f - 1.0f *(GET_SPEED_PATCH)) * ESCON_GET_FACTOR / 60.0f; } void MaxonESCON::period(float period)
--- a/RobotControl/RobotControl.cpp Tue Mar 26 08:29:43 2013 +0000 +++ b/RobotControl/RobotControl.cpp Sat Mar 30 06:55:08 2013 +0000 @@ -24,6 +24,9 @@ motorControllerLeft->setPulses(0); motorControllerRight->setPulses(0); + motorControllerLeft->setVelocity(0.0f); + motorControllerRight->setVelocity(0.0f); + Desired.setAcceleration(ACCELERATION); Desired.setThetaAcceleration(THETA_ACCELERATION); } @@ -208,13 +211,13 @@ stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR; stateRight.speed = - motorControllerRight->getActualSpeed() * - 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR; + 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ; /* translational speed of the Robot (average) */ - Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f; + Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f; /* rotational speed of the Robot */ - Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE; + Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE; /* rotational theta of the Robot integrate the omega with the time*/ Actual.theta += Actual.omega * period;
--- a/StateDefines/State.cpp Tue Mar 26 08:29:43 2013 +0000 +++ b/StateDefines/State.cpp Sat Mar 30 06:55:08 2013 +0000 @@ -46,8 +46,8 @@ "SetpointX-Axis[m]\t" //15 "SetpointY-Axis[m]\t" "SetpointAngel[grad]\t" //17 - "RightDistanceSensor[m]\t" - "CompassAngle[grad]\t" + "SetpointVelocitiy[m/s]\t" + "SetpointVelocitiyRotations[rad/s]\t" "CompassX-Axis\t" //20 "CompassY-Axis\t" "State\t" @@ -81,8 +81,8 @@ s.setxAxis, s.setyAxis, s.setAngle, - s.rightDist, - s.compassAngle, + s.setVelocity, + s.setOmega, s.compassxAxis, s.compassyAxis, s.state, @@ -151,9 +151,9 @@ s->leftPulses = - motorControllerLeft->getPulses(); s->rightPulses = motorControllerRight->getPulses(); s->leftVelocity = motorControllerLeft->getActualSpeed() * - 2.0f * WHEEL_RADIUS_LEFT * PI; + 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR; s->rightVelocity = - motorControllerRight->getActualSpeed()* - 2.0f * WHEEL_RADIUS_RIGHT * PI; + 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR; s->velocity = robotControl->getActualSpeed(); s->omega = robotControl->getActualOmega(); s->xAxis = robotControl->getxActualPosition(); @@ -166,6 +166,8 @@ s->setyAxis = robotControl->getDesiredyPosition(); s->setAngle = robotControl->getDesiredTheta() * 180 / PI; // s->compassAngle = compass->getFilteredAngle() * 180 / PI; + s->setVelocity = robotControl->getDesiredSpeed(); + s->setOmega = robotControl->getDesiredOmega(); s->rho = robotControl->getDistanceError(); s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
--- a/StateDefines/defines.h Tue Mar 26 08:29:43 2013 +0000 +++ b/StateDefines/defines.h Sat Mar 30 06:55:08 2013 +0000 @@ -12,11 +12,10 @@ #define PULSES_PER_STEP 6u // Pulses per electrical step Hallsensor, have 6 steps // Physical Dimension of the car -#define WHEEL_RADIUS_DIFF 0.00054 // positiv zu weit links, given in [m] 0.00059 -#define WHEEL_RADIUS_LEFT 0.03930f // radius of the left wheel, given in [m] 0.03945f -#define WHEEL_RADIUS_RIGHT (WHEEL_RADIUS_LEFT + WHEEL_RADIUS_DIFF) // radius of the left wheel, given in [m] -#define WHEEL_DISTANCE 0.1860f // Distance of the wheel, given in [m] 0.1865f - +#define WHEEL_RADIUS_DIFF -0.00015f // positiv zu weit rechts, given in [m] +#define WHEEL_RADIUS_LEFT 0.040280f // radius of the left wheel, given in [m] 0.040479f +#define WHEEL_RADIUS_RIGHT (WHEEL_RADIUS_LEFT - WHEEL_RADIUS_DIFF) // radius of the left wheel, given in [m] +#define WHEEL_DISTANCE 0.1875f // Distance of the wheel, given in [m] Grösser --> mehr drehen // State Bits of the car #define STATE_STOP 1u // Bit0 = stop pressed @@ -25,8 +24,12 @@ #define STATE_RIGHT 8u // Bit3 = right ESCON in error state // ESCON Constands -#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 +#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 + +//Error patch of the drift of Analog input and pwn output +#define SET_SPEED_PATCH (1+0.0029f) // patch factor of set speed +#define GET_SPEED_PATCH (1+0.0019f) // patch factor of get speed // Start Defintition #define START_X_OFFSET -0.8f // Sets the start X-point [m] @@ -41,7 +44,7 @@ #define K1 1.0f * GAIN #define K2 3.0f * GAIN #define K3 2.0f * GAIN // deafult 2.0f -#define MIN_DISTANCE_ERROR -0.02f // 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.005f // 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 @@ -55,13 +58,6 @@ // Compass Maxima und Minima for the Filter #define SET_MAXIMAS_MINIMAS true // For Set the maximas und minimas when false the object search the maximas minimas by your own -/*#define COMP_X_MAX 344.464996f // Maximum X-Range -#define COMP_Y_MAX 238.751282f // Maximum Y-Range -#define COMP_Z_MAX -266.899994f // Maximum Z-Range not used in this side -#define COMP_X_MIN -90.412109f // Minimum X-Range -#define COMP_Y_MIN -220.834808f // Minimum Y-Range -#define COMP_Z_MIN -356.000000f // Minimun Z-Range not used in this side -*/ #define COMP_X_MAX 391.219910f // Maximum X-Range #define COMP_Y_MAX 382.915161f // Maximum Y-Range #define COMP_Z_MAX -237.855042f // Maximum Z-Range not used in this side @@ -112,10 +108,10 @@ float setyAxis; /** Setpoint Angel [°] */ float setAngle; - /** Right Distance Sensor [m] */ - float rightDist; - /** Compass Angle [°] */ - float compassAngle; + /** Setpoint velocitiy [m/s] */ + float setVelocity; + /** Setpoint rotation velocitiy [rad/s] */ + float setOmega; /** Compass X-Axis */ float compassxAxis; /** Compass Y-Axis */
--- a/main.cpp Tue Mar 26 08:29:43 2013 +0000 +++ b/main.cpp Sat Mar 30 06:55:08 2013 +0000 @@ -1,6 +1,6 @@ /** * \mainpage Index Page - * + * * @file main.cpp * @author Christian Burri * @@ -91,140 +91,110 @@ // compass.start(); state.initPlotFile(); + state.startTimerFromZero(); + state.start(); - robotControl.start(); robotControl.setEnable(false); wait(0.1); robotControl.setEnable(true); wait(0.1); robotControl.setAllToZero(0, 0, PI/2 ); - // robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); + robotControl.start(); + // robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); + + + // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); + // robotControl.setDesiredPositionAndAngle(0, 0, PI/2); + // wait(0.1); + + ////////////////////////////////////////// - leftMotor.setPulses(0); - rightMotor.setPulses(0); + robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; - state.startTimerFromZero(); - state.start(); + robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f, -PI/2); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; - // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); - // robotControl.setDesiredPositionAndAngle(0, 0, PI/2); - // wait(0.1); - - ////////////////////////////////////////// + robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f, 0); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(0.0f, -0.8f, PI/2); + while(!(robotControl.getDistanceError() <= 0.05)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(0.0f, -0.2f, PI/2); + while(!(robotControl.getDistanceError() <= 0.05)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2); + while(!(s.millis >= 65000)) { + state.savePlotFile(s); + }; + + + + + + ///////////////////////stay /* - robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI); - while(!(robotControl.getDistanceError() <= 0.1)) { + robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2); + while(!(s.millis >= 25000)) { state.savePlotFile(s); }; + */ + //////////////////////////stay - robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.1)) { - state.savePlotFile(s); - }; + + + + + + + //////////////////////////////////////////////////////////////////////// + + - robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f, 0); - while(!(robotControl.getDistanceError() <= 0.1)) { - state.savePlotFile(s); - }; - - robotControl.setDesiredPositionAndAngle(0.0f, -0.8f, PI/2); - while(!(robotControl.getDistanceError() <= 0.05)) { - state.savePlotFile(s); - }; - - robotControl.setDesiredPositionAndAngle(0.0f, -0.2f, PI/2); + //Zum Umfang einstellen 2m fahren + /* + robotControl.setDesiredPositionAndAngle(0.0f, 4.0f, PI/2); + while(!(s.millis >= 30000)) { + state.savePlotFile(s); + }; + */ + + + + ///////////////oder//////////////////e + + + // Zum radabstand einstellen + + /* + robotControl.setDesiredPositionAndAngle(-0.7f, 1.0f, PI); while(!(robotControl.getDistanceError() <= 0.05)) { state.savePlotFile(s); }; - robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2); - while(!(s.millis >= 65000)) { - state.savePlotFile(s); - }; - */ - - ///////////////////////stay - - robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2); - while(!(s.millis >= 65000)) { + robotControl.setDesiredPositionAndAngle(-0.8f, 1.0f, PI); + while(!(robotControl.getDistanceError() <= 0.05)) { state.savePlotFile(s); }; - - //////////////////////////stay - - - - - - - - - - - //////////////////////////////////////////////////////////////////////// - - - - //Zum Umfang einstellen 2m fahren - - robotControl.setDesiredPositionAndAngle(0.0f, 2.0f, PI/2); - while(!(s.millis >= 30000)) { - state.savePlotFile(s); - }; - - - - - ///////////////oder////////////////// - // Zum radabstand einstellen - - /* - robotControl.setDesiredPositionAndAngle(-0.94f, 0.68f, PI); - while(!(s.millis >= 30000)) { - state.savePlotFile(s); - } - /* - - /* - int sek = 0; - int step = 5000; - int i = 0; - int totalTurns = 2; - - sek += step; - - while(i <= totalTurns) { - robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI); - while(!(s.millis >= sek)) { + robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f, PI); + while(!(s.millis >= 30000)) { state.savePlotFile(s); - }; - sek += step; + } + */ - robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, -PI/2); - while(!(s.millis >= sek)) { - state.savePlotFile(s); - }; - sek += step; - - robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, 0); - while(!(s.millis >= sek)) { - state.savePlotFile(s); - }; - sek += step; - - robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI/2); - while(!(s.millis >= sek)) { - state.savePlotFile(s); - }; - sek += step; - - i++; - } - - -*/ //////////////////////////////////////////////////////// @@ -232,51 +202,51 @@ // Epä Parkour fahrä -/* - robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); - wait(0.1); + /* + 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.setDesiredPositionAndAngle(-1.20f, 1.50f, 3*PI/4); + while(!(robotControl.getDistanceError() <= 0.4)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4); + while(!(robotControl.getDistanceError() <= 0.4)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4); + while(!(robotControl.getDistanceError() <= 0.4)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, PI); - while(!(robotControl.getDistanceError() <= 0.2)) { - state.savePlotFile(s); - }; + 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.1)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2); + while(!(robotControl.getDistanceError() <= 0.4)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.06)) { - state.savePlotFile(s); - }; - + robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2); + while(!(robotControl.getDistanceError() <= 0.06)) { + state.savePlotFile(s); + }; + - */ - - - - + */ + + + + /* printf("here we go... \n"); AdkTerm.setupDevice();