Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

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>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

main.cpp

Committer:
chrigelburri
Date:
2013-04-04
Revision:
10:09ddb819fdcb
Parent:
9:d3cdcdef9719
Child:
11:775ebb69d5e1

File content as of revision 10:09ddb819fdcb:

/*! \mainpage Index Page
 *
 * @author Christian Burri
 *
 * @section LICENSE
 *
 * Copyright &copy; 2013 HSLU Pren Team #1 Cruising Crêpe
 * All rights reserved.
 *
 * @section DESCRIPTION
 *
 * This Programm is for a 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> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
 * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
 * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
 * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
 * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
 * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
 * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
 *
 * The postition control is based on polar coordiantes.
 * For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
 *
 */

#include "mbed.h"
#include "math.h"
#include "defines.h"
#include "State.h"
#include "HMC5883L.h"
#include "HMC6352.h"
#include "RobotControl.h"
#include "Ping.h"
#include "PowerControl/EthernetPowerControl.h"
//#include "android.h"

//Android
//AdkTerm AdkTerm;

/**
 * LiPo Batterie for check an undervoltage.
 */
AnalogIn battery(p15);

// compass
//HMC5883L compass(p9, p10, PERIOD_COMPASS);       // sda, sdl (I2C)
//HMC6352 compass(p9, p10, PERIOD_COMPASS);        // sda, sdl (I2C)

//Hallsensor
//hall1, hall2, hall3
Hallsensor hallLeft(p18, p17, p16);
//hall1, hall2, hall3
Hallsensor hallRight(p27, p28, p29);

// Motors
//enb, ready, pwm, actualSpeed, Hallsensor object
MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
//enb, ready, pwm, actualSpeed, Hallsensor object
MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);

// Robot Control
RobotControl robotControl (&leftMotor, &rightMotor, /*&compass,*/ PERIOD_ROBOTCONTROL);

// Logging & State
state_t s; // stuct state
State state(&s, &robotControl, &leftMotor, &rightMotor, /*&compass,*/ &battery, PERIOD_STATE);

// PC USB communications
Serial pc(USBTX, USBRX);

DigitalOut myled(LED1);


// LiPo Batterie
float batterie_voltage;

int main()
{
    /** Normal mbed power level for this setup is around 690mW
     * assuming 5V used on Vin pin
     * If you don't need networking...
     * Power down Ethernet interface - saves around 175mW
     * Also need to unplug network cable - just a cable sucks power
     */
    PHY_PowerDown();

    //  robotControl.start();
    //  compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //  compass.start();

    state.initPlotFile();
    state.startTimerFromZero();
    state.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.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
    //  robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
    // wait(0.1);

    //////////////////////////////////////////
    /*
        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(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 >= 32000)) {
            state.savePlotFile(s);
        };
    */




    ///////////////////////stay
    /*
            robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
        while(!(s.millis >= 25000)) {
            state.savePlotFile(s);
        };
        */
    //////////////////////////stay







    ////////////////////////////////////////////////////////////////////////

    /*

        //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);
        };


        robotControl.setDesiredPositionAndAngle(0.0f, 2.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.8f, 1.0f,  PI);
        while(!(robotControl.getDistanceError() <= 0.05)) {
            state.savePlotFile(s);
        };


        robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f,  PI);
        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.50f, 1.50f,  3*PI/4);
    while(!(robotControl.getDistanceError() <= 0.9)) {
        state.savePlotFile(s);
    };

    robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f,  PI/4);
    while(!(robotControl.getDistanceError() <= 0.7)) {
        state.savePlotFile(s);
    };

    robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f,  PI/2);
    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.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() )

            if( AdkTerm.getx() == 99) {
                break;
            }
        }
    */

    state.savePlotFile(s);
    state.closePlotFile();
    state.stop();
    robotControl.setEnable(false);
}