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@10:09ddb819fdcb, 2013-04-04 (annotated)

Committer:
chrigelburri
Date:
Thu Apr 04 06:43:43 2013 +0000
Revision:
10:09ddb819fdcb
Parent:
9:d3cdcdef9719
Child:
11:775ebb69d5e1
vor dem sauberen kommentieren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 10:09ddb819fdcb 1 /*! \mainpage Index Page
chrigelburri 8:696c2f9dfc62 2 *
chrigelburri 2:d8e1613dc38b 3 * @author Christian Burri
chrigelburri 2:d8e1613dc38b 4 *
chrigelburri 2:d8e1613dc38b 5 * @section LICENSE
chrigelburri 2:d8e1613dc38b 6 *
chrigelburri 2:d8e1613dc38b 7 * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 1:6cd533a712c6 8 * All rights reserved.
chrigelburri 2:d8e1613dc38b 9 *
chrigelburri 2:d8e1613dc38b 10 * @section DESCRIPTION
chrigelburri 2:d8e1613dc38b 11 *
chrigelburri 3:92ba0254af87 12 * This Programm is for a autonomous robot for the competition
chrigelburri 4:3a97923ff2d4 13 * at the Hochschule Luzern.
chrigelburri 3:92ba0254af87 14 * We are one of the 32 teams. In the team #1 is:
chrigelburri 4:3a97923ff2d4 15 * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 16 * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 17 * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 18 * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 19 * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 20 * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
chrigelburri 3:92ba0254af87 21 * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 22 *
chrigelburri 4:3a97923ff2d4 23 * The postition control is based on polar coordiantes.
chrigelburri 3:92ba0254af87 24 * 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>
chrigelburri 2:d8e1613dc38b 25 *
chrigelburri 1:6cd533a712c6 26 */
chrigelburri 1:6cd533a712c6 27
chrigelburri 1:6cd533a712c6 28 #include "mbed.h"
chrigelburri 1:6cd533a712c6 29 #include "math.h"
chrigelburri 1:6cd533a712c6 30 #include "defines.h"
chrigelburri 1:6cd533a712c6 31 #include "State.h"
chrigelburri 1:6cd533a712c6 32 #include "HMC5883L.h"
chrigelburri 1:6cd533a712c6 33 #include "HMC6352.h"
chrigelburri 1:6cd533a712c6 34 #include "RobotControl.h"
chrigelburri 1:6cd533a712c6 35 #include "Ping.h"
chrigelburri 1:6cd533a712c6 36 #include "PowerControl/EthernetPowerControl.h"
chrigelburri 6:48eeb41188dd 37 //#include "android.h"
chrigelburri 6:48eeb41188dd 38
chrigelburri 6:48eeb41188dd 39 //Android
chrigelburri 6:48eeb41188dd 40 //AdkTerm AdkTerm;
chrigelburri 1:6cd533a712c6 41
chrigelburri 10:09ddb819fdcb 42 /**
chrigelburri 10:09ddb819fdcb 43 * LiPo Batterie for check an undervoltage.
chrigelburri 10:09ddb819fdcb 44 */
chrigelburri 10:09ddb819fdcb 45 AnalogIn battery(p15);
chrigelburri 1:6cd533a712c6 46
chrigelburri 1:6cd533a712c6 47 // compass
chrigelburri 1:6cd533a712c6 48 //HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
chrigelburri 1:6cd533a712c6 49 //HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
chrigelburri 1:6cd533a712c6 50
chrigelburri 1:6cd533a712c6 51 //Hallsensor
chrigelburri 1:6cd533a712c6 52 //hall1, hall2, hall3
chrigelburri 1:6cd533a712c6 53 Hallsensor hallLeft(p18, p17, p16);
chrigelburri 1:6cd533a712c6 54 //hall1, hall2, hall3
chrigelburri 1:6cd533a712c6 55 Hallsensor hallRight(p27, p28, p29);
chrigelburri 1:6cd533a712c6 56
chrigelburri 1:6cd533a712c6 57 // Motors
chrigelburri 1:6cd533a712c6 58 //enb, ready, pwm, actualSpeed, Hallsensor object
chrigelburri 1:6cd533a712c6 59 MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
chrigelburri 1:6cd533a712c6 60 //enb, ready, pwm, actualSpeed, Hallsensor object
chrigelburri 1:6cd533a712c6 61 MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
chrigelburri 1:6cd533a712c6 62
chrigelburri 1:6cd533a712c6 63 // Robot Control
chrigelburri 1:6cd533a712c6 64 RobotControl robotControl (&leftMotor, &rightMotor, /*&compass,*/ PERIOD_ROBOTCONTROL);
chrigelburri 1:6cd533a712c6 65
chrigelburri 1:6cd533a712c6 66 // Logging & State
chrigelburri 1:6cd533a712c6 67 state_t s; // stuct state
chrigelburri 1:6cd533a712c6 68 State state(&s, &robotControl, &leftMotor, &rightMotor, /*&compass,*/ &battery, PERIOD_STATE);
chrigelburri 1:6cd533a712c6 69
chrigelburri 1:6cd533a712c6 70 // PC USB communications
chrigelburri 1:6cd533a712c6 71 Serial pc(USBTX, USBRX);
chrigelburri 1:6cd533a712c6 72
chrigelburri 1:6cd533a712c6 73 DigitalOut myled(LED1);
chrigelburri 1:6cd533a712c6 74
chrigelburri 1:6cd533a712c6 75
chrigelburri 1:6cd533a712c6 76 // LiPo Batterie
chrigelburri 1:6cd533a712c6 77 float batterie_voltage;
chrigelburri 1:6cd533a712c6 78
chrigelburri 1:6cd533a712c6 79 int main()
chrigelburri 1:6cd533a712c6 80 {
chrigelburri 1:6cd533a712c6 81 /** Normal mbed power level for this setup is around 690mW
chrigelburri 10:09ddb819fdcb 82 * assuming 5V used on Vin pin
chrigelburri 10:09ddb819fdcb 83 * If you don't need networking...
chrigelburri 10:09ddb819fdcb 84 * Power down Ethernet interface - saves around 175mW
chrigelburri 10:09ddb819fdcb 85 * Also need to unplug network cable - just a cable sucks power
chrigelburri 10:09ddb819fdcb 86 */
chrigelburri 1:6cd533a712c6 87 PHY_PowerDown();
chrigelburri 2:d8e1613dc38b 88
chrigelburri 2:d8e1613dc38b 89 // robotControl.start();
chrigelburri 1:6cd533a712c6 90 // compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
chrigelburri 1:6cd533a712c6 91 // compass.start();
chrigelburri 4:3a97923ff2d4 92
chrigelburri 2:d8e1613dc38b 93 state.initPlotFile();
chrigelburri 8:696c2f9dfc62 94 state.startTimerFromZero();
chrigelburri 8:696c2f9dfc62 95 state.start();
chrigelburri 2:d8e1613dc38b 96
chrigelburri 1:6cd533a712c6 97 robotControl.setEnable(false);
chrigelburri 6:48eeb41188dd 98 wait(0.1);
chrigelburri 1:6cd533a712c6 99 robotControl.setEnable(true);
chrigelburri 6:48eeb41188dd 100 wait(0.1);
chrigelburri 9:d3cdcdef9719 101 //robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 10:09ddb819fdcb 102 robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 8:696c2f9dfc62 103 robotControl.start();
chrigelburri 8:696c2f9dfc62 104
chrigelburri 8:696c2f9dfc62 105
chrigelburri 8:696c2f9dfc62 106 // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 8:696c2f9dfc62 107 // robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
chrigelburri 8:696c2f9dfc62 108 // wait(0.1);
chrigelburri 8:696c2f9dfc62 109
chrigelburri 8:696c2f9dfc62 110 //////////////////////////////////////////
chrigelburri 9:d3cdcdef9719 111 /*
chrigelburri 9:d3cdcdef9719 112 robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI);
chrigelburri 9:d3cdcdef9719 113 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 9:d3cdcdef9719 114 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 115 };
chrigelburri 2:d8e1613dc38b 116
chrigelburri 9:d3cdcdef9719 117 robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f, -PI/2);
chrigelburri 9:d3cdcdef9719 118 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 9:d3cdcdef9719 119 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 120 };
chrigelburri 8:696c2f9dfc62 121
chrigelburri 9:d3cdcdef9719 122 robotControl.setDesiredPositionAndAngle(0.0f, -0.8f, PI/2);
chrigelburri 9:d3cdcdef9719 123 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 9:d3cdcdef9719 124 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 125 };
chrigelburri 8:696c2f9dfc62 126
chrigelburri 9:d3cdcdef9719 127 robotControl.setDesiredPositionAndAngle(0.0f, -0.2f, PI/2);
chrigelburri 9:d3cdcdef9719 128 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 9:d3cdcdef9719 129 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 130 };
chrigelburri 8:696c2f9dfc62 131
chrigelburri 9:d3cdcdef9719 132 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 9:d3cdcdef9719 133 while(!(s.millis >= 32000)) {
chrigelburri 9:d3cdcdef9719 134 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 135 };
chrigelburri 9:d3cdcdef9719 136 */
chrigelburri 8:696c2f9dfc62 137
chrigelburri 8:696c2f9dfc62 138
chrigelburri 8:696c2f9dfc62 139
chrigelburri 8:696c2f9dfc62 140
chrigelburri 8:696c2f9dfc62 141 ///////////////////////stay
chrigelburri 7:34be8b3a979c 142 /*
chrigelburri 8:696c2f9dfc62 143 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 8:696c2f9dfc62 144 while(!(s.millis >= 25000)) {
chrigelburri 6:48eeb41188dd 145 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 146 };
chrigelburri 8:696c2f9dfc62 147 */
chrigelburri 8:696c2f9dfc62 148 //////////////////////////stay
chrigelburri 5:48a258f6335e 149
chrigelburri 8:696c2f9dfc62 150
chrigelburri 8:696c2f9dfc62 151
chrigelburri 8:696c2f9dfc62 152
chrigelburri 8:696c2f9dfc62 153
chrigelburri 8:696c2f9dfc62 154
chrigelburri 8:696c2f9dfc62 155
chrigelburri 8:696c2f9dfc62 156 ////////////////////////////////////////////////////////////////////////
chrigelburri 8:696c2f9dfc62 157
chrigelburri 9:d3cdcdef9719 158 /*
chrigelburri 9:d3cdcdef9719 159
chrigelburri 9:d3cdcdef9719 160 //Zum Umfang einstellen 2m fahren
chrigelburri 9:d3cdcdef9719 161 robotControl.setDesiredPositionAndAngle(0.0f, 0.5f, PI/2);
chrigelburri 9:d3cdcdef9719 162 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 9:d3cdcdef9719 163 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 164 };
chrigelburri 9:d3cdcdef9719 165
chrigelburri 9:d3cdcdef9719 166 robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI/2);
chrigelburri 9:d3cdcdef9719 167 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 9:d3cdcdef9719 168 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 169 };
chrigelburri 9:d3cdcdef9719 170 robotControl.setDesiredPositionAndAngle(0.0f, 1.5f, PI/2);
chrigelburri 9:d3cdcdef9719 171 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 9:d3cdcdef9719 172 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 173 };
chrigelburri 8:696c2f9dfc62 174
chrigelburri 6:48eeb41188dd 175
chrigelburri 9:d3cdcdef9719 176 robotControl.setDesiredPositionAndAngle(0.0f, 2.0f, PI/2);
chrigelburri 9:d3cdcdef9719 177 while(!(s.millis >= 30000)) {
chrigelburri 9:d3cdcdef9719 178 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 179 };
chrigelburri 8:696c2f9dfc62 180
chrigelburri 9:d3cdcdef9719 181 */
chrigelburri 8:696c2f9dfc62 182
chrigelburri 8:696c2f9dfc62 183
chrigelburri 8:696c2f9dfc62 184 ///////////////oder//////////////////e
chrigelburri 8:696c2f9dfc62 185
chrigelburri 8:696c2f9dfc62 186
chrigelburri 8:696c2f9dfc62 187 // Zum radabstand einstellen
chrigelburri 8:696c2f9dfc62 188
chrigelburri 8:696c2f9dfc62 189 /*
chrigelburri 8:696c2f9dfc62 190 robotControl.setDesiredPositionAndAngle(-0.7f, 1.0f, PI);
chrigelburri 7:34be8b3a979c 191 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 192 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 193 };
chrigelburri 2:d8e1613dc38b 194
chrigelburri 8:696c2f9dfc62 195 robotControl.setDesiredPositionAndAngle(-0.8f, 1.0f, PI);
chrigelburri 8:696c2f9dfc62 196 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 197 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 198 };
chrigelburri 6:48eeb41188dd 199
chrigelburri 6:48eeb41188dd 200
chrigelburri 8:696c2f9dfc62 201 robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f, PI);
chrigelburri 8:696c2f9dfc62 202 while(!(s.millis >= 30000)) {
chrigelburri 6:48eeb41188dd 203 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 204 }
chrigelburri 9:d3cdcdef9719 205
chrigelburri 8:696c2f9dfc62 206 */
chrigelburri 6:48eeb41188dd 207
chrigelburri 6:48eeb41188dd 208
chrigelburri 6:48eeb41188dd 209 ////////////////////////////////////////////////////////
chrigelburri 6:48eeb41188dd 210
chrigelburri 6:48eeb41188dd 211
chrigelburri 6:48eeb41188dd 212
chrigelburri 6:48eeb41188dd 213 // Epä Parkour fahrä
chrigelburri 5:48a258f6335e 214
chrigelburri 9:d3cdcdef9719 215 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 9:d3cdcdef9719 216 wait(0.1);
chrigelburri 6:48eeb41188dd 217
chrigelburri 9:d3cdcdef9719 218 robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f, 3*PI/4);
chrigelburri 9:d3cdcdef9719 219 while(!(robotControl.getDistanceError() <= 0.9)) {
chrigelburri 9:d3cdcdef9719 220 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 221 };
chrigelburri 6:48eeb41188dd 222
chrigelburri 9:d3cdcdef9719 223 robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f, PI/4);
chrigelburri 9:d3cdcdef9719 224 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 9:d3cdcdef9719 225 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 226 };
chrigelburri 5:48a258f6335e 227
chrigelburri 9:d3cdcdef9719 228 robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f, PI/2);
chrigelburri 9:d3cdcdef9719 229 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 9:d3cdcdef9719 230 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 231 };
chrigelburri 6:48eeb41188dd 232
chrigelburri 9:d3cdcdef9719 233 robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4);
chrigelburri 9:d3cdcdef9719 234 while(!(robotControl.getDistanceError() <= 0.55)) {
chrigelburri 9:d3cdcdef9719 235 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 236 };
chrigelburri 8:696c2f9dfc62 237
chrigelburri 4:3a97923ff2d4 238
chrigelburri 9:d3cdcdef9719 239 robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
chrigelburri 9:d3cdcdef9719 240 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 9:d3cdcdef9719 241 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 242 };
chrigelburri 9:d3cdcdef9719 243
chrigelburri 9:d3cdcdef9719 244 robotControl.setDesiredPositionAndAngle(-2.4f, 3.0f, -PI/2);
chrigelburri 9:d3cdcdef9719 245 while(!(robotControl.getDistanceError() <= 1.0)) {
chrigelburri 9:d3cdcdef9719 246 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 247 };
chrigelburri 9:d3cdcdef9719 248
chrigelburri 9:d3cdcdef9719 249 robotControl.setDesiredPositionAndAngle(-1.74f, 1.30f, -PI/2);
chrigelburri 9:d3cdcdef9719 250 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 9:d3cdcdef9719 251 state.savePlotFile(s);
chrigelburri 9:d3cdcdef9719 252 };
chrigelburri 9:d3cdcdef9719 253 robotControl.setDesiredPositionAndAngle(-1.75f, 0.80f, -PI/2);
chrigelburri 9:d3cdcdef9719 254 while(!(s.millis >= 32000)) {
chrigelburri 9:d3cdcdef9719 255 state.savePlotFile(s);
chrigelburri 10:09ddb819fdcb 256 }
chrigelburri 8:696c2f9dfc62 257
chrigelburri 8:696c2f9dfc62 258
chrigelburri 8:696c2f9dfc62 259
chrigelburri 8:696c2f9dfc62 260
chrigelburri 6:48eeb41188dd 261
chrigelburri 10:09ddb819fdcb 262 /*
chrigelburri 10:09ddb819fdcb 263 printf("here we go... \n");
chrigelburri 10:09ddb819fdcb 264 AdkTerm.setupDevice();
chrigelburri 10:09ddb819fdcb 265 printf("Android Development Kit: start\r\n");
chrigelburri 10:09ddb819fdcb 266 USBInit();
chrigelburri 10:09ddb819fdcb 267 while (!(s.millis >= 60000)) {
chrigelburri 10:09ddb819fdcb 268 USBLoop();
chrigelburri 9:d3cdcdef9719 269
chrigelburri 10:09ddb819fdcb 270 printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() )
chrigelburri 6:48eeb41188dd 271
chrigelburri 10:09ddb819fdcb 272 if( AdkTerm.getx() == 99) {
chrigelburri 10:09ddb819fdcb 273 break;
chrigelburri 6:48eeb41188dd 274 }
chrigelburri 10:09ddb819fdcb 275 }
chrigelburri 10:09ddb819fdcb 276 */
chrigelburri 2:d8e1613dc38b 277
chrigelburri 10:09ddb819fdcb 278 state.savePlotFile(s);
chrigelburri 10:09ddb819fdcb 279 state.closePlotFile();
chrigelburri 10:09ddb819fdcb 280 state.stop();
chrigelburri 10:09ddb819fdcb 281 robotControl.setEnable(false);
chrigelburri 10:09ddb819fdcb 282 }