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 22:bfec16575c91, committed 2013-05-19
- Comitter:
- chrigelburri
- Date:
- Sun May 19 07:35:16 2013 +0000
- Parent:
- 21:48248c5b8992
- Child:
- 23:4d8173c5183b
- Child:
- 24:08241be546ba
- Commit message:
- Eingestellt und mal so punkte gesetzt...
Changed in this revision
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/defines.h Fri May 17 06:39:44 2013 +0000 +++ b/defines.h Sun May 19 07:35:16 2013 +0000 @@ -42,12 +42,12 @@ /** * @brief Value for the diffrerenz between left an right, given in [m] */ -#define WHEEL_RADIUS_DIFF 0.0000f +#define WHEEL_RADIUS_DIFF 0.0001f /** - * @brief Radius of the left wheel, given in [m] + * @brief Radius of the left wheel, given in [m] kleiner --> weiter */ -#define WHEEL_RADIUS_LEFT 0.040280f +#define WHEEL_RADIUS_LEFT 0.040190f //0.040190f /** * @brief Radius of the left wheel, given in [m] @@ -57,7 +57,7 @@ /** * @brief Distance of the wheel, given in [m] Greater --> turn more */ -#define WHEEL_DISTANCE 0.1873f // 0.1870f +#define WHEEL_DISTANCE 0.17350f // 0.17500f /** * @brief Sets the start X-point, given in [m] @@ -110,7 +110,7 @@ /** * @brief Speed Factor how get in the ESCON Studio */ -#define ESCON_GET_FACTOR 2452.4f +#define ESCON_GET_FACTOR 2500.0f /** * @brief Error patch of the drift of Analog input and pwn output for set speed @@ -131,12 +131,12 @@ /** * @brief Main Gain for k1, k2 and k3 */ -#define GAIN 0.8f +#define GAIN 0.65f // 1.8f 0.65 /** * @brief Gain k1 default 1.0f */ -#define K1 0.8f * GAIN +#define K1 1.45f * GAIN //0.8 1.2 /** * @brief Gain k2 default 3.0f @@ -152,7 +152,7 @@ * @brief 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.01f +#define MIN_DISTANCE_ERROR 0.001f /*! @} */ /**
--- a/main.cpp Fri May 17 06:39:44 2013 +0000 +++ b/main.cpp Sun May 19 07:35:16 2013 +0000 @@ -114,14 +114,25 @@ int main() { + int garagenumber = 1; + float x = -2.954f + 0.308 * garagenumber; + float ypre = 1.30f; + float ygoal = 0.80f; + /** - * Initialze the filt PLOTS.txt, + * Check at first the Battery voltage. Starts when the voltages greater as the min is. * start the timer for the Logging to the file * and start the Task for logging **/ + state.start(); + state.initPlotFile(); + state.closePlotFile(); + while(s.voltageBattery < BAT_MIN) {} + state.stop(); + wait(0.5); + state.start(); state.initPlotFile(); state.startTimerFromZero(); - state.start(); /** * Clear all Errors of the ESCON Module, with a disabled to enable event @@ -134,99 +145,95 @@ /** * Set the startposition and start the Task for controlling the roboter. */ - robotControl.setAllToZero(0, 0, PI/2 ); - //robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); + // robotControl.setAllToZero(0, 0, PI/2 ); + robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); robotControl.start(); - - // 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); + state.savePlotFile(s); - robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f, 3*PI/4); - while(!(robotControl.getDistanceError() <= 0.9)) { - state.savePlotFile(s); - }; + /** + * Sets the desired Points. + */ + robotControl.setDesiredPositionAndAngle(-1.35f, 1.85f, 3*PI/8); + while(!(robotControl.getDistanceError() <= 0.7)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f, PI/4); - while(!(robotControl.getDistanceError() <= 0.7)) { - state.savePlotFile(s); - }; - - robotControl.setDesiredPositionAndAngle(-0.5f, 3.0f, PI/2); //0.75 - while(!(robotControl.getDistanceError() <= 0.7)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-0.75f, 2.85f, 3*PI/8); + while(!(robotControl.getDistanceError() <= 0.7)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4); - while(!(robotControl.getDistanceError() <= 0.55)) { - 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.05)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI); + while(!(robotControl.getDistanceError() <= 0.05)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-2.6f, 3.0f, -PI/2); - while(!(robotControl.getDistanceError() <= 1.0)) { - state.savePlotFile(s); - }; - - robotControl.setDesiredPositionAndAngle(-2.0f, 2.0f, -PI/2); - while(!(robotControl.getDistanceError() <= 1.0)) { - state.savePlotFile(s); - }; + robotControl.setDesiredPositionAndAngle(-2.1f, 3.0f, -PI/2); + while(!(robotControl.getDistanceError() <= 1.0)) { + state.savePlotFile(s); + }; - robotControl.setDesiredPositionAndAngle(-2.65f, 1.30f, -PI/2); - while(!(robotControl.getDistanceError() <= 0.4)) { - state.savePlotFile(s); - }; - robotControl.setDesiredPositionAndAngle(-2.65f, 0.80f, -PI/2); - while(!(s.millis >= 32000)) { - state.savePlotFile(s); - } - */ + robotControl.setDesiredPositionAndAngle(-2.0f, 1.8f, -PI/2); + while(!(robotControl.getDistanceError() <= 1.0)) { + state.savePlotFile(s); + }; + + robotControl.setDesiredPositionAndAngle(x, ypre, -PI/2); + while(!(robotControl.getDistanceError() <= 0.4)) { + state.savePlotFile(s); + }; + robotControl.setDesiredPositionAndAngle(x, ygoal, -PI/2); + while(!(s.millis >= 19000)) { + state.savePlotFile(s); + } - pc.baud(460800); - pc.printf("********************* MicroBridge 4568 ********************************\n\r"); - init(); + /* + pc.baud(460800); + pc.printf("********************* MicroBridge 4568 ********************************\n\r"); - // Initialise the ADB subsystem. - - pc.printf("connection isOpen\n"); + init(); - while(getDesiredTheta() < (4 * PI)) { + // Initialise the ADB subsystem. - ADB::poll(); + pc.printf("connection isOpen\n"); - if (getDesiredTheta() > (2 * PI)) { - //robotControl.setAllToZero(0, 0, PI/2 ); - robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); - robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); - } - else - { - robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta()); + while(getDesiredTheta() < (4 * PI)) { + + ADB::poll(); - - state.savePlotFile(s); - //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b) - } - - writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery); - //connection->write(sizeof(str),(unsigned char*)&str); - - wait(0.25); - - } + if (getDesiredTheta() > (2 * PI)) { + //robotControl.setAllToZero(0, 0, PI/2 ); + robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); + robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); + } + else + { + robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta()); + state.savePlotFile(s); + //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b) + } + + writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery); + //connection->write(sizeof(str),(unsigned char*)&str); + + wait(0.25); + + } + + */ /** * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm */