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@1:6cd533a712c6, 2013-03-02 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 02 09:39:34 2013 +0000
Revision:
1:6cd533a712c6
Parent:
0:31f7be68e52d
Child:
2:d8e1613dc38b
Pos Regler funktioniert getestet im leerlauf;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 1:6cd533a712c6 1 /*
chrigelburri 1:6cd533a712c6 2 * main.cpp
chrigelburri 1:6cd533a712c6 3 * Copyright (c) 2012, Pren Team1 HSLU T&A
chrigelburri 1:6cd533a712c6 4 * All rights reserved.
chrigelburri 1:6cd533a712c6 5 */
chrigelburri 1:6cd533a712c6 6
chrigelburri 1:6cd533a712c6 7 ///////////////////////////////////////////////////////////////////////////////////////////////////////
chrigelburri 1:6cd533a712c6 8 // INCLUDES
chrigelburri 1:6cd533a712c6 9 ///////////////////////////////////////////////////////////////////////////////////////////////////////
chrigelburri 1:6cd533a712c6 10 #include "mbed.h"
chrigelburri 1:6cd533a712c6 11 #include "math.h"
chrigelburri 1:6cd533a712c6 12 #include "defines.h"
chrigelburri 1:6cd533a712c6 13 #include "State.h"
chrigelburri 1:6cd533a712c6 14 #include "HMC5883L.h"
chrigelburri 1:6cd533a712c6 15 #include "HMC6352.h"
chrigelburri 1:6cd533a712c6 16 #include "RobotControl.h"
chrigelburri 1:6cd533a712c6 17 #include "Ping.h"
chrigelburri 1:6cd533a712c6 18 #include "PowerControl/EthernetPowerControl.h"
chrigelburri 1:6cd533a712c6 19 #include "Android.h"
chrigelburri 1:6cd533a712c6 20
chrigelburri 1:6cd533a712c6 21
chrigelburri 1:6cd533a712c6 22 // LiPo Batterie
chrigelburri 1:6cd533a712c6 23 AnalogIn battery(p15); // Battery check
chrigelburri 1:6cd533a712c6 24
chrigelburri 1:6cd533a712c6 25 // compass
chrigelburri 1:6cd533a712c6 26 //HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
chrigelburri 1:6cd533a712c6 27 //HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
chrigelburri 1:6cd533a712c6 28
chrigelburri 1:6cd533a712c6 29
chrigelburri 1:6cd533a712c6 30 // ultrasonic sensor
chrigelburri 1:6cd533a712c6 31 //Ping ultrasonic(p30);
chrigelburri 1:6cd533a712c6 32
chrigelburri 1:6cd533a712c6 33 //Hallsensor
chrigelburri 1:6cd533a712c6 34 //hall1, hall2, hall3
chrigelburri 1:6cd533a712c6 35 Hallsensor hallLeft(p18, p17, p16);
chrigelburri 1:6cd533a712c6 36 //hall1, hall2, hall3
chrigelburri 1:6cd533a712c6 37 Hallsensor hallRight(p27, p28, p29);
chrigelburri 1:6cd533a712c6 38
chrigelburri 1:6cd533a712c6 39 // Motors
chrigelburri 1:6cd533a712c6 40 //enb, ready, pwm, actualSpeed, Hallsensor object
chrigelburri 1:6cd533a712c6 41 MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
chrigelburri 1:6cd533a712c6 42 //enb, ready, pwm, actualSpeed, Hallsensor object
chrigelburri 1:6cd533a712c6 43 MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
chrigelburri 1:6cd533a712c6 44
chrigelburri 1:6cd533a712c6 45 // Robot Control
chrigelburri 1:6cd533a712c6 46 RobotControl robotControl (&leftMotor, &rightMotor, /*&compass,*/ PERIOD_ROBOTCONTROL);
chrigelburri 1:6cd533a712c6 47
chrigelburri 1:6cd533a712c6 48 // Logging & State
chrigelburri 1:6cd533a712c6 49 state_t s; // stuct state
chrigelburri 1:6cd533a712c6 50 State state(&s, &robotControl, &leftMotor, &rightMotor, /*&compass,*/ &battery, PERIOD_STATE);
chrigelburri 1:6cd533a712c6 51
chrigelburri 1:6cd533a712c6 52 // Communication
chrigelburri 1:6cd533a712c6 53
chrigelburri 1:6cd533a712c6 54 //Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);
chrigelburri 1:6cd533a712c6 55
chrigelburri 1:6cd533a712c6 56 // PC USB communications
chrigelburri 1:6cd533a712c6 57 Serial pc(USBTX, USBRX);
chrigelburri 1:6cd533a712c6 58
chrigelburri 1:6cd533a712c6 59 DigitalOut myled(LED1);
chrigelburri 1:6cd533a712c6 60
chrigelburri 1:6cd533a712c6 61 //float magout[3] = {0};
chrigelburri 1:6cd533a712c6 62
chrigelburri 1:6cd533a712c6 63 // LiPo Batterie
chrigelburri 1:6cd533a712c6 64 float batterie_voltage;
chrigelburri 1:6cd533a712c6 65
chrigelburri 1:6cd533a712c6 66 int main()
chrigelburri 1:6cd533a712c6 67 {
chrigelburri 1:6cd533a712c6 68 pc.printf("blabal");
chrigelburri 1:6cd533a712c6 69 /** Normal mbed power level for this setup is around 690mW
chrigelburri 1:6cd533a712c6 70 * assuming 5V used on Vin pin
chrigelburri 1:6cd533a712c6 71 * If you don't need networking...
chrigelburri 1:6cd533a712c6 72 * Power down Ethernet interface - saves around 175mW
chrigelburri 1:6cd533a712c6 73 * Also need to unplug network cable - just a cable sucks power
chrigelburri 1:6cd533a712c6 74 */
chrigelburri 1:6cd533a712c6 75 PHY_PowerDown();
chrigelburri 1:6cd533a712c6 76 state.startTimerFromZero();
chrigelburri 1:6cd533a712c6 77 state.initPlotFile();
chrigelburri 1:6cd533a712c6 78
chrigelburri 1:6cd533a712c6 79 // robotControl.start();
chrigelburri 1:6cd533a712c6 80 // compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
chrigelburri 1:6cd533a712c6 81 // compass.start();
chrigelburri 1:6cd533a712c6 82 robotControl.start();
chrigelburri 1:6cd533a712c6 83 robotControl.setEnable(false);
chrigelburri 1:6cd533a712c6 84 wait(0.01);
chrigelburri 1:6cd533a712c6 85 robotControl.setEnable(true);
chrigelburri 1:6cd533a712c6 86 wait(0.01);
chrigelburri 1:6cd533a712c6 87 robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 1:6cd533a712c6 88 leftMotor.setPulses(0);
chrigelburri 1:6cd533a712c6 89 rightMotor.setPulses(0);
chrigelburri 1:6cd533a712c6 90 state.start();
chrigelburri 1:6cd533a712c6 91 pc.printf("start");
chrigelburri 1:6cd533a712c6 92
chrigelburri 1:6cd533a712c6 93 robotControl.setxPosition(-1.0);
chrigelburri 1:6cd533a712c6 94 robotControl.setyPosition(1.0);
chrigelburri 1:6cd533a712c6 95 robotControl.setTheta( PI);
chrigelburri 1:6cd533a712c6 96 while(!(s.millis >= 15000)) {
chrigelburri 1:6cd533a712c6 97 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 98 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);
chrigelburri 1:6cd533a712c6 99 };
chrigelburri 1:6cd533a712c6 100 pc.printf("nextpoint");
chrigelburri 1:6cd533a712c6 101 robotControl.setxPosition(-2.0);
chrigelburri 1:6cd533a712c6 102 robotControl.setyPosition(3.0);
chrigelburri 1:6cd533a712c6 103 robotControl.setTheta( PI/2 );
chrigelburri 1:6cd533a712c6 104 while(!(s.millis >= 30000)) {
chrigelburri 1:6cd533a712c6 105 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 106 pc.printf("rho: %f, gamma: %f, delta: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal() * 180/PI, robotControl.getThetaGoal()*180/PI);
chrigelburri 1:6cd533a712c6 107 };
chrigelburri 1:6cd533a712c6 108 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 109 state.closePlotFile();
chrigelburri 1:6cd533a712c6 110 state.stop();
chrigelburri 1:6cd533a712c6 111 robotControl.setEnable(false);
chrigelburri 1:6cd533a712c6 112 pc.printf("\n");
chrigelburri 1:6cd533a712c6 113
chrigelburri 1:6cd533a712c6 114 }