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 9:d3cdcdef9719, committed 2013-03-30
- Comitter:
- chrigelburri
- Date:
- Sat Mar 30 16:35:22 2013 +0000
- Parent:
- 8:696c2f9dfc62
- Child:
- 10:09ddb819fdcb
- Commit message:
- Find Garage 4?????
Changed in this revision
StateDefines/defines.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/StateDefines/defines.h Sat Mar 30 06:55:08 2013 +0000 +++ b/StateDefines/defines.h Sat Mar 30 16:35:22 2013 +0000 @@ -12,10 +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.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_DIFF 0.0000f // positiv zu weit rechts, given in [m] +#define WHEEL_RADIUS_LEFT 0.040280f // radius of the left wheel, given in [m] 0.040280f #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 +#define WHEEL_DISTANCE 0.2000f // Distance of the wheel, given in [m] Grösser --> mehr drehen // State Bits of the car #define STATE_STOP 1u // Bit0 = stop pressed @@ -24,12 +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 1500.0f // Speed Factor how set in the ESCON +#define ESCON_GET_FACTOR 1600.4f // 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 +#define SET_SPEED_PATCH (1+0.00262f) // 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] @@ -40,9 +40,9 @@ #define THETA_ACCELERATION 1.0f // maximum rotational acceleration, given in [rad/s2] // position controller -#define GAIN 0.30f // Main Gain -#define K1 1.0f * GAIN -#define K2 3.0f * GAIN +#define GAIN 0.8f // Main Gain +#define K1 0.8f * GAIN +#define K2 3.0f * GAIN // deafult 3.0f #define K3 2.0f * GAIN // deafult 2.0f #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]
--- a/main.cpp Sat Mar 30 06:55:08 2013 +0000 +++ b/main.cpp Sat Mar 30 16:35:22 2013 +0000 @@ -98,9 +98,9 @@ wait(0.1); robotControl.setEnable(true); wait(0.1); - robotControl.setAllToZero(0, 0, PI/2 ); + //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); @@ -108,37 +108,32 @@ // wait(0.1); ////////////////////////////////////////// - - robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI); - while(!(robotControl.getDistanceError() <= 0.1)) { - state.savePlotFile(s); - }; + /* + robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; - 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(-1.00f, 1.0f, -PI/2); + 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.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.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); - }; - + robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2); + while(!(s.millis >= 32000)) { + state.savePlotFile(s); + }; + */ @@ -160,16 +155,30 @@ //////////////////////////////////////////////////////////////////////// + /* + + //Zum Umfang einstellen 2m fahren + robotControl.setDesiredPositionAndAngle(0.0f, 0.5f, PI/2); + while(!(robotControl.getDistanceError() <= 0.2)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI/2); + while(!(robotControl.getDistanceError() <= 0.2)) { + state.savePlotFile(s); + }; + robotControl.setDesiredPositionAndAngle(0.0f, 1.5f, PI/2); + while(!(robotControl.getDistanceError() <= 0.2)) { + state.savePlotFile(s); + }; - //Zum Umfang einstellen 2m fahren - /* - robotControl.setDesiredPositionAndAngle(0.0f, 4.0f, PI/2); - while(!(s.millis >= 30000)) { - state.savePlotFile(s); - }; - */ + robotControl.setDesiredPositionAndAngle(0.0f, 2.0f, PI/2); + while(!(s.millis >= 30000)) { + state.savePlotFile(s); + }; + */ ///////////////oder//////////////////e @@ -193,78 +202,81 @@ while(!(s.millis >= 30000)) { state.savePlotFile(s); } + */ - //////////////////////////////////////////////////////// // Epä Parkour fahrä - /* - 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(START_X_OFFSET, START_Y_OFFSET, PI/2); + wait(0.1); - 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(-1.50f, 1.50f, 3*PI/4); + while(!(robotControl.getDistanceError() <= 0.9)) { + 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.20f, 2.1f, PI/4); + while(!(robotControl.getDistanceError() <= 0.7)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f, PI/2); + while(!(robotControl.getDistanceError() <= 0.7)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.06)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4); + while(!(robotControl.getDistanceError() <= 0.55)) { + state.savePlotFile(s); + }; - */ + robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(-2.4f, 3.0f, -PI/2); + while(!(robotControl.getDistanceError() <= 1.0)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(-1.74f, 1.30f, -PI/2); + while(!(robotControl.getDistanceError() <= 0.1)) { + state.savePlotFile(s); + }; + robotControl.setDesiredPositionAndAngle(-1.75f, 0.80f, -PI/2); + while(!(s.millis >= 32000)) { + state.savePlotFile(s); +} - /* - printf("here we go... \n"); - AdkTerm.setupDevice(); - printf("Android Development Kit: start\r\n"); - USBInit(); - while (!(s.millis >= 60000)) { - USBLoop(); - printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() ) + /* + printf("here we go... \n"); + AdkTerm.setupDevice(); + printf("Android Development Kit: start\r\n"); + USBInit(); + while (!(s.millis >= 60000)) { + USBLoop(); + + printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() ) - if( AdkTerm.getx() == 99) { - break; + if( AdkTerm.getx() == 99) { + break; + } } - } - */ + */ - state.savePlotFile(s); - state.closePlotFile(); - state.stop(); - robotControl.setEnable(false); -} + state.savePlotFile(s); + state.closePlotFile(); + state.stop(); + robotControl.setEnable(false); + }