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@7:34be8b3a979c, 2013-03-26 (annotated)

Committer:
chrigelburri
Date:
Tue Mar 26 08:29:43 2013 +0000
Revision:
7:34be8b3a979c
Parent:
6:48eeb41188dd
Child:
8:696c2f9dfc62
Grob einstellung vollzogen, korrektur vom linken und rechten rausche noch nicht gemacht

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 2:d8e1613dc38b 1 /**
chrigelburri 6:48eeb41188dd 2 * \mainpage Index Page
chrigelburri 6:48eeb41188dd 3 *
chrigelburri 3:92ba0254af87 4 * @file main.cpp
chrigelburri 2:d8e1613dc38b 5 * @author Christian Burri
chrigelburri 2:d8e1613dc38b 6 *
chrigelburri 2:d8e1613dc38b 7 * @section LICENSE
chrigelburri 2:d8e1613dc38b 8 *
chrigelburri 2:d8e1613dc38b 9 * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 1:6cd533a712c6 10 * All rights reserved.
chrigelburri 2:d8e1613dc38b 11 *
chrigelburri 2:d8e1613dc38b 12 * @section DESCRIPTION
chrigelburri 2:d8e1613dc38b 13 *
chrigelburri 3:92ba0254af87 14 * This Programm is for a autonomous robot for the competition
chrigelburri 4:3a97923ff2d4 15 * at the Hochschule Luzern.
chrigelburri 3:92ba0254af87 16 * We are one of the 32 teams. In the team #1 is:
chrigelburri 4:3a97923ff2d4 17 * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 18 * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 19 * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 20 * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 21 * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 22 * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
chrigelburri 3:92ba0254af87 23 * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 24 *
chrigelburri 4:3a97923ff2d4 25 * The postition control is based on polar coordiantes.
chrigelburri 3:92ba0254af87 26 * 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 27 *
chrigelburri 1:6cd533a712c6 28 */
chrigelburri 1:6cd533a712c6 29
chrigelburri 1:6cd533a712c6 30 #include "mbed.h"
chrigelburri 1:6cd533a712c6 31 #include "math.h"
chrigelburri 1:6cd533a712c6 32 #include "defines.h"
chrigelburri 1:6cd533a712c6 33 #include "State.h"
chrigelburri 1:6cd533a712c6 34 #include "HMC5883L.h"
chrigelburri 1:6cd533a712c6 35 #include "HMC6352.h"
chrigelburri 1:6cd533a712c6 36 #include "RobotControl.h"
chrigelburri 1:6cd533a712c6 37 #include "Ping.h"
chrigelburri 1:6cd533a712c6 38 #include "PowerControl/EthernetPowerControl.h"
chrigelburri 6:48eeb41188dd 39 //#include "android.h"
chrigelburri 6:48eeb41188dd 40
chrigelburri 6:48eeb41188dd 41 //Android
chrigelburri 6:48eeb41188dd 42 //AdkTerm AdkTerm;
chrigelburri 1:6cd533a712c6 43
chrigelburri 1:6cd533a712c6 44 // LiPo Batterie
chrigelburri 1:6cd533a712c6 45 AnalogIn battery(p15); // Battery check
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 1:6cd533a712c6 82 * assuming 5V used on Vin pin
chrigelburri 1:6cd533a712c6 83 * If you don't need networking...
chrigelburri 1:6cd533a712c6 84 * Power down Ethernet interface - saves around 175mW
chrigelburri 1:6cd533a712c6 85 * Also need to unplug network cable - just a cable sucks power
chrigelburri 1:6cd533a712c6 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 2:d8e1613dc38b 94
chrigelburri 1:6cd533a712c6 95 robotControl.start();
chrigelburri 1:6cd533a712c6 96 robotControl.setEnable(false);
chrigelburri 6:48eeb41188dd 97 wait(0.1);
chrigelburri 1:6cd533a712c6 98 robotControl.setEnable(true);
chrigelburri 6:48eeb41188dd 99 wait(0.1);
chrigelburri 6:48eeb41188dd 100 robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 6:48eeb41188dd 101 // robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 2:d8e1613dc38b 102
chrigelburri 1:6cd533a712c6 103 leftMotor.setPulses(0);
chrigelburri 1:6cd533a712c6 104 rightMotor.setPulses(0);
chrigelburri 2:d8e1613dc38b 105
chrigelburri 2:d8e1613dc38b 106 state.startTimerFromZero();
chrigelburri 1:6cd533a712c6 107 state.start();
chrigelburri 2:d8e1613dc38b 108
chrigelburri 6:48eeb41188dd 109 // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 6:48eeb41188dd 110 // robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
chrigelburri 7:34be8b3a979c 111 // wait(0.1);
chrigelburri 6:48eeb41188dd 112
chrigelburri 6:48eeb41188dd 113 //////////////////////////////////////////
chrigelburri 7:34be8b3a979c 114 /*
chrigelburri 6:48eeb41188dd 115 robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI);
chrigelburri 6:48eeb41188dd 116 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 6:48eeb41188dd 117 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 118 };
chrigelburri 5:48a258f6335e 119
chrigelburri 6:48eeb41188dd 120 robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f, -PI/2);
chrigelburri 6:48eeb41188dd 121 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 6:48eeb41188dd 122 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 123 };
chrigelburri 6:48eeb41188dd 124
chrigelburri 6:48eeb41188dd 125 robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f, 0);
chrigelburri 6:48eeb41188dd 126 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 6:48eeb41188dd 127 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 128 };
chrigelburri 7:34be8b3a979c 129
chrigelburri 7:34be8b3a979c 130 robotControl.setDesiredPositionAndAngle(0.0f, -0.8f, PI/2);
chrigelburri 7:34be8b3a979c 131 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 132 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 133 };
chrigelburri 7:34be8b3a979c 134
chrigelburri 7:34be8b3a979c 135 robotControl.setDesiredPositionAndAngle(0.0f, -0.2f, PI/2);
chrigelburri 7:34be8b3a979c 136 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 137 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 138 };
chrigelburri 2:d8e1613dc38b 139
chrigelburri 6:48eeb41188dd 140 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 7:34be8b3a979c 141 while(!(s.millis >= 65000)) {
chrigelburri 6:48eeb41188dd 142 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 143 };
chrigelburri 7:34be8b3a979c 144 */
chrigelburri 6:48eeb41188dd 145
chrigelburri 7:34be8b3a979c 146 ///////////////////////stay
chrigelburri 6:48eeb41188dd 147
chrigelburri 7:34be8b3a979c 148 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 7:34be8b3a979c 149 while(!(s.millis >= 65000)) {
chrigelburri 7:34be8b3a979c 150 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 151 };
chrigelburri 7:34be8b3a979c 152
chrigelburri 7:34be8b3a979c 153 //////////////////////////stay
chrigelburri 6:48eeb41188dd 154
chrigelburri 6:48eeb41188dd 155
chrigelburri 6:48eeb41188dd 156
chrigelburri 6:48eeb41188dd 157
chrigelburri 6:48eeb41188dd 158
chrigelburri 6:48eeb41188dd 159
chrigelburri 6:48eeb41188dd 160
chrigelburri 6:48eeb41188dd 161
chrigelburri 6:48eeb41188dd 162
chrigelburri 6:48eeb41188dd 163
chrigelburri 6:48eeb41188dd 164 ////////////////////////////////////////////////////////////////////////
chrigelburri 6:48eeb41188dd 165
chrigelburri 6:48eeb41188dd 166
chrigelburri 2:d8e1613dc38b 167
chrigelburri 7:34be8b3a979c 168 //Zum Umfang einstellen 2m fahren
chrigelburri 7:34be8b3a979c 169
chrigelburri 7:34be8b3a979c 170 robotControl.setDesiredPositionAndAngle(0.0f, 2.0f, PI/2);
chrigelburri 7:34be8b3a979c 171 while(!(s.millis >= 30000)) {
chrigelburri 2:d8e1613dc38b 172 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 173 };
chrigelburri 6:48eeb41188dd 174
chrigelburri 2:d8e1613dc38b 175
chrigelburri 7:34be8b3a979c 176
chrigelburri 6:48eeb41188dd 177
chrigelburri 6:48eeb41188dd 178 ///////////////oder//////////////////
chrigelburri 6:48eeb41188dd 179
chrigelburri 6:48eeb41188dd 180
chrigelburri 6:48eeb41188dd 181 // Zum radabstand einstellen
chrigelburri 7:34be8b3a979c 182
chrigelburri 6:48eeb41188dd 183 /*
chrigelburri 7:34be8b3a979c 184 robotControl.setDesiredPositionAndAngle(-0.94f, 0.68f, PI);
chrigelburri 7:34be8b3a979c 185 while(!(s.millis >= 30000)) {
chrigelburri 7:34be8b3a979c 186 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 187 }
chrigelburri 7:34be8b3a979c 188 /*
chrigelburri 7:34be8b3a979c 189
chrigelburri 7:34be8b3a979c 190 /*
chrigelburri 7:34be8b3a979c 191 int sek = 0;
chrigelburri 7:34be8b3a979c 192 int step = 5000;
chrigelburri 6:48eeb41188dd 193 int i = 0;
chrigelburri 7:34be8b3a979c 194 int totalTurns = 2;
chrigelburri 6:48eeb41188dd 195
chrigelburri 7:34be8b3a979c 196 sek += step;
chrigelburri 7:34be8b3a979c 197
chrigelburri 7:34be8b3a979c 198 while(i <= totalTurns) {
chrigelburri 6:48eeb41188dd 199 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI);
chrigelburri 6:48eeb41188dd 200 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 201 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 202 };
chrigelburri 6:48eeb41188dd 203 sek += step;
chrigelburri 6:48eeb41188dd 204
chrigelburri 6:48eeb41188dd 205 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, -PI/2);
chrigelburri 6:48eeb41188dd 206 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 207 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 208 };
chrigelburri 6:48eeb41188dd 209 sek += step;
chrigelburri 5:48a258f6335e 210
chrigelburri 6:48eeb41188dd 211 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, 0);
chrigelburri 6:48eeb41188dd 212 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 213 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 214 };
chrigelburri 6:48eeb41188dd 215 sek += step;
chrigelburri 6:48eeb41188dd 216
chrigelburri 6:48eeb41188dd 217 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI/2);
chrigelburri 6:48eeb41188dd 218 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 219 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 220 };
chrigelburri 6:48eeb41188dd 221 sek += step;
chrigelburri 6:48eeb41188dd 222
chrigelburri 6:48eeb41188dd 223 i++;
chrigelburri 6:48eeb41188dd 224 }
chrigelburri 7:34be8b3a979c 225
chrigelburri 7:34be8b3a979c 226
chrigelburri 6:48eeb41188dd 227 */
chrigelburri 6:48eeb41188dd 228
chrigelburri 6:48eeb41188dd 229
chrigelburri 6:48eeb41188dd 230 ////////////////////////////////////////////////////////
chrigelburri 6:48eeb41188dd 231
chrigelburri 6:48eeb41188dd 232
chrigelburri 6:48eeb41188dd 233
chrigelburri 6:48eeb41188dd 234 // Epä Parkour fahrä
chrigelburri 6:48eeb41188dd 235 /*
chrigelburri 6:48eeb41188dd 236 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 6:48eeb41188dd 237 wait(0.1);
chrigelburri 5:48a258f6335e 238
chrigelburri 6:48eeb41188dd 239 robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f, 3*PI/4);
chrigelburri 6:48eeb41188dd 240 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 241 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 242 };
chrigelburri 6:48eeb41188dd 243
chrigelburri 6:48eeb41188dd 244 robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4);
chrigelburri 6:48eeb41188dd 245 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 246 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 247 };
chrigelburri 6:48eeb41188dd 248
chrigelburri 6:48eeb41188dd 249 robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4);
chrigelburri 6:48eeb41188dd 250 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 251 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 252 };
chrigelburri 6:48eeb41188dd 253
chrigelburri 6:48eeb41188dd 254 robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, PI);
chrigelburri 6:48eeb41188dd 255 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 6:48eeb41188dd 256 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 257 };
chrigelburri 6:48eeb41188dd 258
chrigelburri 6:48eeb41188dd 259 robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
chrigelburri 6:48eeb41188dd 260 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 6:48eeb41188dd 261 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 262 };
chrigelburri 5:48a258f6335e 263
chrigelburri 6:48eeb41188dd 264 robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2);
chrigelburri 6:48eeb41188dd 265 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 266 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 267 };
chrigelburri 6:48eeb41188dd 268
chrigelburri 6:48eeb41188dd 269 robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2);
chrigelburri 6:48eeb41188dd 270 while(!(robotControl.getDistanceError() <= 0.06)) {
chrigelburri 6:48eeb41188dd 271 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 272 };
chrigelburri 6:48eeb41188dd 273
chrigelburri 4:3a97923ff2d4 274
chrigelburri 6:48eeb41188dd 275 */
chrigelburri 6:48eeb41188dd 276
chrigelburri 6:48eeb41188dd 277
chrigelburri 6:48eeb41188dd 278
chrigelburri 6:48eeb41188dd 279
chrigelburri 6:48eeb41188dd 280 /*
chrigelburri 6:48eeb41188dd 281 printf("here we go... \n");
chrigelburri 6:48eeb41188dd 282 AdkTerm.setupDevice();
chrigelburri 6:48eeb41188dd 283 printf("Android Development Kit: start\r\n");
chrigelburri 6:48eeb41188dd 284 USBInit();
chrigelburri 6:48eeb41188dd 285 while (!(s.millis >= 60000)) {
chrigelburri 6:48eeb41188dd 286 USBLoop();
chrigelburri 6:48eeb41188dd 287
chrigelburri 6:48eeb41188dd 288 printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() )
chrigelburri 6:48eeb41188dd 289
chrigelburri 6:48eeb41188dd 290 if( AdkTerm.getx() == 99) {
chrigelburri 6:48eeb41188dd 291 break;
chrigelburri 6:48eeb41188dd 292 }
chrigelburri 6:48eeb41188dd 293 }
chrigelburri 6:48eeb41188dd 294 */
chrigelburri 2:d8e1613dc38b 295
chrigelburri 2:d8e1613dc38b 296 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 297 state.closePlotFile();
chrigelburri 1:6cd533a712c6 298 state.stop();
chrigelburri 1:6cd533a712c6 299 robotControl.setEnable(false);
chrigelburri 1:6cd533a712c6 300 }