Christian Burri
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 
Christian Burri
Diff: main.cpp
- Revision:
- 1:6cd533a712c6
- Parent:
- 0:31f7be68e52d
- Child:
- 2:d8e1613dc38b
--- a/main.cpp Thu Feb 07 17:43:19 2013 +0000
+++ b/main.cpp Sat Mar 02 09:39:34 2013 +0000
@@ -1,87 +1,114 @@
-/*
- * 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()
-{
-
- /** 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();
-
- robotControl.setEnable(false);
- wait(1);
- robotControl.setEnable(true);
- wait(1);
-
-
-}
+/*
+ * 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");
+
+}