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@6:48eeb41188dd, 2013-03-23 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 23 13:52:48 2013 +0000
Revision:
6:48eeb41188dd
Parent:
5:48a258f6335e
Child:
7:34be8b3a979c
mit link und rechten Radradius

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 5:48a258f6335e 111 wait(0.1);
chrigelburri 6:48eeb41188dd 112
chrigelburri 6:48eeb41188dd 113 //////////////////////////////////////////
chrigelburri 6:48eeb41188dd 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 2:d8e1613dc38b 129
chrigelburri 6:48eeb41188dd 130 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 6:48eeb41188dd 131 while(!(s.millis >= 55000)) {
chrigelburri 6:48eeb41188dd 132 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 133 };
chrigelburri 6:48eeb41188dd 134
chrigelburri 6:48eeb41188dd 135
chrigelburri 6:48eeb41188dd 136
chrigelburri 6:48eeb41188dd 137
chrigelburri 6:48eeb41188dd 138
chrigelburri 6:48eeb41188dd 139
chrigelburri 6:48eeb41188dd 140
chrigelburri 6:48eeb41188dd 141
chrigelburri 6:48eeb41188dd 142
chrigelburri 6:48eeb41188dd 143
chrigelburri 6:48eeb41188dd 144
chrigelburri 6:48eeb41188dd 145
chrigelburri 6:48eeb41188dd 146 ////////////////////////////////////////////////////////////////////////
chrigelburri 6:48eeb41188dd 147
chrigelburri 6:48eeb41188dd 148
chrigelburri 2:d8e1613dc38b 149
chrigelburri 6:48eeb41188dd 150 //Zum Umfang einstellen
chrigelburri 6:48eeb41188dd 151 /*
chrigelburri 6:48eeb41188dd 152 robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI/2);
chrigelburri 6:48eeb41188dd 153 while(!(s.millis >= 20000)) {
chrigelburri 2:d8e1613dc38b 154 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 155 };
chrigelburri 6:48eeb41188dd 156
chrigelburri 2:d8e1613dc38b 157
chrigelburri 6:48eeb41188dd 158 */
chrigelburri 6:48eeb41188dd 159
chrigelburri 6:48eeb41188dd 160 ///////////////oder//////////////////
chrigelburri 6:48eeb41188dd 161
chrigelburri 6:48eeb41188dd 162
chrigelburri 6:48eeb41188dd 163 // Zum radabstand einstellen
chrigelburri 6:48eeb41188dd 164
chrigelburri 6:48eeb41188dd 165 /*
chrigelburri 6:48eeb41188dd 166 int sek = 1000;
chrigelburri 6:48eeb41188dd 167 int step = 1000;
chrigelburri 6:48eeb41188dd 168 int i = 0;
chrigelburri 6:48eeb41188dd 169 int totalTurns = 5;
chrigelburri 6:48eeb41188dd 170
chrigelburri 6:48eeb41188dd 171 while(i >= totalTurns) {
chrigelburri 6:48eeb41188dd 172 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI);
chrigelburri 6:48eeb41188dd 173 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 174 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 175 };
chrigelburri 6:48eeb41188dd 176 sek += step;
chrigelburri 6:48eeb41188dd 177
chrigelburri 6:48eeb41188dd 178 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, -PI/2);
chrigelburri 6:48eeb41188dd 179 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 180 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 181 };
chrigelburri 6:48eeb41188dd 182 sek += step;
chrigelburri 5:48a258f6335e 183
chrigelburri 6:48eeb41188dd 184 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, 0);
chrigelburri 6:48eeb41188dd 185 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 186 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 187 };
chrigelburri 6:48eeb41188dd 188 sek += step;
chrigelburri 6:48eeb41188dd 189
chrigelburri 6:48eeb41188dd 190 robotControl.setDesiredPositionAndAngle(0.0f, 0.0f, PI/2);
chrigelburri 6:48eeb41188dd 191 while(!(s.millis >= sek)) {
chrigelburri 6:48eeb41188dd 192 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 193 };
chrigelburri 6:48eeb41188dd 194 sek += step;
chrigelburri 6:48eeb41188dd 195
chrigelburri 6:48eeb41188dd 196 i++;
chrigelburri 6:48eeb41188dd 197 }
chrigelburri 6:48eeb41188dd 198 */
chrigelburri 6:48eeb41188dd 199
chrigelburri 6:48eeb41188dd 200
chrigelburri 6:48eeb41188dd 201 ////////////////////////////////////////////////////////
chrigelburri 6:48eeb41188dd 202
chrigelburri 6:48eeb41188dd 203
chrigelburri 6:48eeb41188dd 204
chrigelburri 6:48eeb41188dd 205 // Epä Parkour fahrä
chrigelburri 6:48eeb41188dd 206 /*
chrigelburri 6:48eeb41188dd 207 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 6:48eeb41188dd 208 wait(0.1);
chrigelburri 5:48a258f6335e 209
chrigelburri 6:48eeb41188dd 210 robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f, 3*PI/4);
chrigelburri 6:48eeb41188dd 211 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 212 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 213 };
chrigelburri 6:48eeb41188dd 214
chrigelburri 6:48eeb41188dd 215 robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4);
chrigelburri 6:48eeb41188dd 216 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 217 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 218 };
chrigelburri 6:48eeb41188dd 219
chrigelburri 6:48eeb41188dd 220 robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4);
chrigelburri 6:48eeb41188dd 221 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 222 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 223 };
chrigelburri 6:48eeb41188dd 224
chrigelburri 6:48eeb41188dd 225 robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, PI);
chrigelburri 6:48eeb41188dd 226 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 6:48eeb41188dd 227 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 228 };
chrigelburri 6:48eeb41188dd 229
chrigelburri 6:48eeb41188dd 230 robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
chrigelburri 6:48eeb41188dd 231 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 6:48eeb41188dd 232 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 233 };
chrigelburri 5:48a258f6335e 234
chrigelburri 6:48eeb41188dd 235 robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2);
chrigelburri 6:48eeb41188dd 236 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 6:48eeb41188dd 237 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 238 };
chrigelburri 6:48eeb41188dd 239
chrigelburri 6:48eeb41188dd 240 robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2);
chrigelburri 6:48eeb41188dd 241 while(!(robotControl.getDistanceError() <= 0.06)) {
chrigelburri 6:48eeb41188dd 242 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 243 };
chrigelburri 6:48eeb41188dd 244
chrigelburri 4:3a97923ff2d4 245
chrigelburri 6:48eeb41188dd 246 */
chrigelburri 6:48eeb41188dd 247
chrigelburri 6:48eeb41188dd 248
chrigelburri 6:48eeb41188dd 249
chrigelburri 6:48eeb41188dd 250
chrigelburri 6:48eeb41188dd 251 /*
chrigelburri 6:48eeb41188dd 252 printf("here we go... \n");
chrigelburri 6:48eeb41188dd 253 AdkTerm.setupDevice();
chrigelburri 6:48eeb41188dd 254 printf("Android Development Kit: start\r\n");
chrigelburri 6:48eeb41188dd 255 USBInit();
chrigelburri 6:48eeb41188dd 256 while (!(s.millis >= 60000)) {
chrigelburri 6:48eeb41188dd 257 USBLoop();
chrigelburri 6:48eeb41188dd 258
chrigelburri 6:48eeb41188dd 259 printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() )
chrigelburri 6:48eeb41188dd 260
chrigelburri 6:48eeb41188dd 261 if( AdkTerm.getx() == 99) {
chrigelburri 6:48eeb41188dd 262 break;
chrigelburri 6:48eeb41188dd 263 }
chrigelburri 6:48eeb41188dd 264 }
chrigelburri 6:48eeb41188dd 265 */
chrigelburri 2:d8e1613dc38b 266
chrigelburri 2:d8e1613dc38b 267 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 268 state.closePlotFile();
chrigelburri 1:6cd533a712c6 269 state.stop();
chrigelburri 1:6cd533a712c6 270 robotControl.setEnable(false);
chrigelburri 1:6cd533a712c6 271 }