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
Diff: main.cpp
- Revision:
- 27:a13ede88e75f
- Parent:
- 26:a201dcd4e618
diff -r a201dcd4e618 -r a13ede88e75f main.cpp --- a/main.cpp Mon May 20 09:41:47 2013 +0000 +++ b/main.cpp Mon May 20 11:51:33 2013 +0000 @@ -39,16 +39,6 @@ #include "RobotControl.h" #include "androidADB.h" -/** - * @name LED - * @{ - */ - -/** -* @brief The Array of the four LED on the mbed board. -*/ -PwmOut led[4] = { LED1, LED2, LED3, LED4 }; -/*! @} */ /** * @name Hallsensor @@ -115,7 +105,6 @@ /*! @} */ - // @todo PC USB communications DAs wird danach gelöscht Serial pc(USBTX, USBRX); @@ -125,11 +114,6 @@ int main() { - int garagenumber = 2; - float x = -2.954f + 0.308 * garagenumber; - float ypre = 1.30f; - float ygoal = 0.60f; - /** * Check at first the Battery voltage. Starts when the voltages greater as the min is. * start the timer for the Logging to the file @@ -139,29 +123,15 @@ state.initPlotFile(); state.closePlotFile(); while(s.voltageBattery < BAT_MIN) { - for (float f = 0.1f; f < 6.3f; f += 0.1f) { - for(int i = 0; i <= 3; i ++) { - led[i] = state.dim( i, f ); - } - wait_ms(20); - } - wait(0.05); - for (float f = 0.1f; f < 6.3f; f += 0.1f) { - for(int i = 0; i <= 3; i ++) { - led[i] = state.dim( 3-i, f ); - } - wait_ms(20); - } - wait(0.05); + state.ledArray(20.0f); } - for(int i = 0; i <= 3; i ++) { - led[i] = state.dim( 0, 0.0f ); - } + state.stop(); wait(0.5); state.start(); state.initPlotFile(); state.startTimerFromZero(); + robotControl.start(); /** * Clear all Errors of the ESCON Module, with a disabled to enable event @@ -170,58 +140,55 @@ wait(0.01); robotControl.setEnable(true); wait(0.1); + robotControl.setEnable(false); - /** - * 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.start(); - robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); - - wait(0.02); - state.savePlotFile(s); + // Verbindung zum putty kann spöter gelöscht werden...... + pc.baud(460800); + pc.printf("********************* MicroBridge 4568 ********************************\n\r"); + + + // Initialise the ADB subsystem. + init(); + setDesiredTheta(400.0f); + pc.printf("connection isOpen\n"); + + while(getDesiredTheta() < (4 * PI)) { + + ADB::poll(); + if (getDesiredTheta() > (2 * PI)) { + // Runs at first till the Startbutton has pressed + state.startTimerFromZero(); + robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 ); + robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2); + robotControl.setEnable(false); + } else { + robotControl.setEnable(true); + 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); + + } /** - * Sets the desired Points. + * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm */ - robotControl.setDesiredPositionAndAngle(-1.35f, 1.85f, 3*PI/8); - 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); - }; -//QR - robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI); - while(!(robotControl.getDistanceError() <= 0.075)) { - 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(!(robotControl.getDistanceError() <= 0.2)) { - state.savePlotFile(s); - } - - -/// STOP Setze actueler Wert - robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getActualTheta()); + robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(), + robotControl.getyActualPosition(), + robotControl.getActualTheta()); robotControl.stop(); state.savePlotFile(s); leftMotor.setVelocity(0.0f); @@ -230,44 +197,6 @@ state.savePlotFile(s); }; - - - /* - pc.baud(460800); - pc.printf("********************* MicroBridge 4568 ********************************\n\r"); - - init(); - - // Initialise the ADB subsystem. - - pc.printf("connection isOpen\n"); - - while(getDesiredTheta() < (4 * PI)) { - - ADB::poll(); - - 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 */ @@ -281,20 +210,7 @@ * Fast PWM sample for the end */ while(1) { - for (float f = 0.1f; f < 6.3f; f += 0.1f) { - for(int i = 0; i <= 3; i ++) { - led[i] = state.dim( i, f ); - } - wait_ms(5); - } - wait(0.1); - for (float f = 0.1f; f < 6.3f; f += 0.1f) { - for(int i = 0; i <= 3; i ++) { - led[i] = state.dim( 3-i, f ); - } - wait_ms(5); - } - wait(0.05); + state.ledArray(20.0f); } }