Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

main.cpp

Committer:
chrigelburri
Date:
2013-03-02
Revision:
1:6cd533a712c6
Parent:
0:31f7be68e52d
Child:
2:d8e1613dc38b

File content as of revision 1:6cd533a712c6:

/*
 * main.cpp
 * Copyright (c) 2012, Pren Team1 HSLU T&A
 * All rights reserved.
 */

///////////////////////////////////////////////////////////////////////////////////////////////////////
// INCLUDES
///////////////////////////////////////////////////////////////////////////////////////////////////////
#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"


// LiPo Batterie
AnalogIn battery(p15);           // Battery check

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


// ultrasonic sensor
//Ping ultrasonic(p30);

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

// Communication

//Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);

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

DigitalOut myled(LED1);

//float magout[3] = {0};

// LiPo Batterie
float batterie_voltage;

int main()
{
    pc.printf("blabal");
    /** 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();
    state.startTimerFromZero();
    state.initPlotFile();
    
          //  robotControl.start();
    //  compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //  compass.start();
    robotControl.start();
    robotControl.setEnable(false);
    wait(0.01);
    robotControl.setEnable(true);
    wait(0.01);
    robotControl.setAllToZero(0, 0, PI/2 );
    leftMotor.setPulses(0);
    rightMotor.setPulses(0);
    state.start();
    pc.printf("start");
    
    robotControl.setxPosition(-1.0);
    robotControl.setyPosition(1.0);
    robotControl.setTheta( PI);
    while(!(s.millis >= 15000)) {
        state.savePlotFile(s);
        pc.printf("rhho: %f, gamma: %f, delta: %f thetaacutal: %f      %f   %f  atan2: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal() * 180/PI, robotControl.getThetaGoal()*180/PI, robotControl.getActualTheta()*180/PI, robotControl.getyPositionError(), robotControl.getxPositionError(), atan2(robotControl.getyPositionError(),robotControl.getxPositionError()) * 180/PI);
    };
    pc.printf("nextpoint");
    robotControl.setxPosition(-2.0);
    robotControl.setyPosition(3.0);
    robotControl.setTheta( PI/2 );
    while(!(s.millis >= 30000)) {
        state.savePlotFile(s);
         pc.printf("rho: %f, gamma: %f, delta: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal()  * 180/PI, robotControl.getThetaGoal()*180/PI);
    };
            state.savePlotFile(s);
    state.closePlotFile();
    state.stop();
    robotControl.setEnable(false);
    pc.printf("\n");

}