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
main.cpp
- Committer:
- chrigelburri
- Date:
- 2013-06-10
- Revision:
- 39:a4fd6206da89
- Parent:
- 38:d76e488e725f
File content as of revision 39:a4fd6206da89:
/*! \mainpage Index Page * @author Christian Burri * @author Arno Galliker * * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe * All rights reserved. * * @brief * * This program is for an autonomous robot for the competition * at the Hochschule Luzern. * We are one of the 32 teams. In the team #1 is: * - Bauernfeind Julia <B>WI</B> * - Büttler Pirmin <B>WI</B> * - Amberg Reto <B>I</B> * - Galliker Arno <B>I</B> * - Amrein Marcel <B>M</B> * - Flühler Ramon <B>M</B> * - Burri Christian <B>ET</B> * Or see: <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> * * The connection to an android smartphone is realise with the library MicroBridge(Android ADB) from Junichi Katsu. * For more information see here: <a href="http://mbed.org/users/jksoft/code/MicroBridge/">http://mbed.org/users/jksoft/code/MicroBridge/</a> * * The rest of the classes are only based on standard library from mbed. * For more information see here: <a href="http://mbed.org/users/mbed_official/code/mbed/">http://mbed.org/users/mbed_official/code/mbed/</a> * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /** * @file main.cpp */ #include "defines.h" #include "State.h" #include "RobotControl.h" #include "androidADB.h" PwmOut led[4] = {LED1, LED2, LED3, LED4}; /** * @name Hallsensor * @{ */ /** * @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16 */ Hallsensor hallLeft(p18, p17, p16); /** * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29 */ Hallsensor hallRight(p27, p28, p29); /*! @} */ /** * @name Motors and Robot Control * @{ */ /** * @brief <code>leftMotor</code> object with pin26, pin25, pin24, * pin19 and <code>hallsensorLeft</code> object */ MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft); /** * @brief <code>rightMotor</code> object with pin23, pin22, pin21, * pin20 and <code>hallsensorRight</code> object */ MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight); /** * @brief <code>robotControl</code> object with <code>leftMotor</code>, * <code>rightMotor</code> and the sampling rate for the run method */ RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL); /*! @} */ /** * @name Logging & State * @{ */ /** * @brief Define the struct for the State and the Logging */ state_t s; /** * @brief <code>state</code> object with <code>robotControl</code>, * <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code> * and the sampling rate for the run method */ State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE); /*! @} */ /** * @name Communication * @{ */ /*! @} */ /** * @brief Main function. Start the Programm here. */ int main() { /** * Check at first the Battery voltage. Starts when the voltage * is greater than the min is. * start the timer for the Logging to the file * and start the Task for logging. **/ state.start(); 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); } for(int i = 0; i <= 3; i ++) { led[i] = state.dim( 0, 0.0f ); } wait(0.5); state.initPlotFile(); state.startTimerFromZero(); robotControl.start(); /* /** * Clear all Errors of the ESCON Module, with a disabled to enable event. */ robotControl.setEnable(false); wait(0.01); robotControl.setEnable(true); wait(0.1); robotControl.setEnable(false); /** * Initialise the ADB subsystem. * Set Theta for to go to Startloop. */ init(); setDesiredTheta(400.0f); while(getDesiredTheta() < (4 * PI)) { 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); wait(PERIOD_ANDROID/2); } else { robotControl.setEnable(true); robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta()); state.savePlotFile(s); wait(PERIOD_ANDROID/2); state.savePlotFile(s); } writeActualPosition(robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery); wait(PERIOD_ANDROID/2); ADB::poll(); } /** * Sets the acutal value for a fast stop. * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm. */ robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getActualTheta()); robotControl.stop(); leftMotor.setVelocity(0.0f); rightMotor.setVelocity(0.0f); wait(2); /** * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm. */ state.closePlotFile(); state.stop(); robotControl.setEnable(false); /** * 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); } }