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