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@8:696c2f9dfc62, 2013-03-30 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 30 06:55:08 2013 +0000
Revision:
8:696c2f9dfc62
Parent:
7:34be8b3a979c
Child:
9:d3cdcdef9719
vor dem tuning. nicht merh 50% duty cicle und 1.65V mitte

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 2:d8e1613dc38b 1 /**
chrigelburri 6:48eeb41188dd 2 * \mainpage Index Page
chrigelburri 8:696c2f9dfc62 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 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 6:48eeb41188dd 101 robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 8:696c2f9dfc62 102 robotControl.start();
chrigelburri 8:696c2f9dfc62 103 // robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
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 2:d8e1613dc38b 111
chrigelburri 8:696c2f9dfc62 112 robotControl.setDesiredPositionAndAngle(0.0f, 1.0f, PI);
chrigelburri 8:696c2f9dfc62 113 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 8:696c2f9dfc62 114 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 115 };
chrigelburri 2:d8e1613dc38b 116
chrigelburri 8:696c2f9dfc62 117 robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f, -PI/2);
chrigelburri 8:696c2f9dfc62 118 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 8:696c2f9dfc62 119 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 120 };
chrigelburri 2:d8e1613dc38b 121
chrigelburri 8:696c2f9dfc62 122 robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f, 0);
chrigelburri 8:696c2f9dfc62 123 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 8:696c2f9dfc62 124 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 125 };
chrigelburri 8:696c2f9dfc62 126
chrigelburri 8:696c2f9dfc62 127 robotControl.setDesiredPositionAndAngle(0.0f, -0.8f, PI/2);
chrigelburri 8:696c2f9dfc62 128 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 8:696c2f9dfc62 129 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 130 };
chrigelburri 8:696c2f9dfc62 131
chrigelburri 8:696c2f9dfc62 132 robotControl.setDesiredPositionAndAngle(0.0f, -0.2f, PI/2);
chrigelburri 8:696c2f9dfc62 133 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 8:696c2f9dfc62 134 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 135 };
chrigelburri 8:696c2f9dfc62 136
chrigelburri 8:696c2f9dfc62 137 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 8:696c2f9dfc62 138 while(!(s.millis >= 65000)) {
chrigelburri 8:696c2f9dfc62 139 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 140 };
chrigelburri 8:696c2f9dfc62 141
chrigelburri 8:696c2f9dfc62 142
chrigelburri 8:696c2f9dfc62 143
chrigelburri 8:696c2f9dfc62 144
chrigelburri 8:696c2f9dfc62 145
chrigelburri 8:696c2f9dfc62 146 ///////////////////////stay
chrigelburri 7:34be8b3a979c 147 /*
chrigelburri 8:696c2f9dfc62 148 robotControl.setDesiredPositionAndAngle(0.0, 0.0f, PI/2);
chrigelburri 8:696c2f9dfc62 149 while(!(s.millis >= 25000)) {
chrigelburri 6:48eeb41188dd 150 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 151 };
chrigelburri 8:696c2f9dfc62 152 */
chrigelburri 8:696c2f9dfc62 153 //////////////////////////stay
chrigelburri 5:48a258f6335e 154
chrigelburri 8:696c2f9dfc62 155
chrigelburri 8:696c2f9dfc62 156
chrigelburri 8:696c2f9dfc62 157
chrigelburri 8:696c2f9dfc62 158
chrigelburri 8:696c2f9dfc62 159
chrigelburri 8:696c2f9dfc62 160
chrigelburri 8:696c2f9dfc62 161 ////////////////////////////////////////////////////////////////////////
chrigelburri 8:696c2f9dfc62 162
chrigelburri 8:696c2f9dfc62 163
chrigelburri 6:48eeb41188dd 164
chrigelburri 8:696c2f9dfc62 165 //Zum Umfang einstellen 2m fahren
chrigelburri 8:696c2f9dfc62 166 /*
chrigelburri 8:696c2f9dfc62 167 robotControl.setDesiredPositionAndAngle(0.0f, 4.0f, PI/2);
chrigelburri 8:696c2f9dfc62 168 while(!(s.millis >= 30000)) {
chrigelburri 8:696c2f9dfc62 169 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 170 };
chrigelburri 8:696c2f9dfc62 171 */
chrigelburri 8:696c2f9dfc62 172
chrigelburri 8:696c2f9dfc62 173
chrigelburri 8:696c2f9dfc62 174
chrigelburri 8:696c2f9dfc62 175 ///////////////oder//////////////////e
chrigelburri 8:696c2f9dfc62 176
chrigelburri 8:696c2f9dfc62 177
chrigelburri 8:696c2f9dfc62 178 // Zum radabstand einstellen
chrigelburri 8:696c2f9dfc62 179
chrigelburri 8:696c2f9dfc62 180 /*
chrigelburri 8:696c2f9dfc62 181 robotControl.setDesiredPositionAndAngle(-0.7f, 1.0f, PI);
chrigelburri 7:34be8b3a979c 182 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 183 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 184 };
chrigelburri 2:d8e1613dc38b 185
chrigelburri 8:696c2f9dfc62 186 robotControl.setDesiredPositionAndAngle(-0.8f, 1.0f, PI);
chrigelburri 8:696c2f9dfc62 187 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 7:34be8b3a979c 188 state.savePlotFile(s);
chrigelburri 7:34be8b3a979c 189 };
chrigelburri 6:48eeb41188dd 190
chrigelburri 6:48eeb41188dd 191
chrigelburri 8:696c2f9dfc62 192 robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f, PI);
chrigelburri 8:696c2f9dfc62 193 while(!(s.millis >= 30000)) {
chrigelburri 6:48eeb41188dd 194 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 195 }
chrigelburri 8:696c2f9dfc62 196 */
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 // Epä Parkour fahrä
chrigelburri 8:696c2f9dfc62 205 /*
chrigelburri 8:696c2f9dfc62 206 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 8:696c2f9dfc62 207 wait(0.1);
chrigelburri 5:48a258f6335e 208
chrigelburri 8:696c2f9dfc62 209 robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f, 3*PI/4);
chrigelburri 8:696c2f9dfc62 210 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 8:696c2f9dfc62 211 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 212 };
chrigelburri 6:48eeb41188dd 213
chrigelburri 8:696c2f9dfc62 214 robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f, PI/4);
chrigelburri 8:696c2f9dfc62 215 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 8:696c2f9dfc62 216 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 217 };
chrigelburri 6:48eeb41188dd 218
chrigelburri 8:696c2f9dfc62 219 robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f, 3*PI/4);
chrigelburri 8:696c2f9dfc62 220 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 8:696c2f9dfc62 221 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 222 };
chrigelburri 6:48eeb41188dd 223
chrigelburri 8:696c2f9dfc62 224 robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, PI);
chrigelburri 8:696c2f9dfc62 225 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 8:696c2f9dfc62 226 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 227 };
chrigelburri 6:48eeb41188dd 228
chrigelburri 8:696c2f9dfc62 229 robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
chrigelburri 8:696c2f9dfc62 230 while(!(robotControl.getDistanceError() <= 0.1)) {
chrigelburri 8:696c2f9dfc62 231 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 232 };
chrigelburri 5:48a258f6335e 233
chrigelburri 8:696c2f9dfc62 234 robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f, -PI/2);
chrigelburri 8:696c2f9dfc62 235 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 8:696c2f9dfc62 236 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 237 };
chrigelburri 6:48eeb41188dd 238
chrigelburri 8:696c2f9dfc62 239 robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f, -PI/2);
chrigelburri 8:696c2f9dfc62 240 while(!(robotControl.getDistanceError() <= 0.06)) {
chrigelburri 8:696c2f9dfc62 241 state.savePlotFile(s);
chrigelburri 8:696c2f9dfc62 242 };
chrigelburri 8:696c2f9dfc62 243
chrigelburri 4:3a97923ff2d4 244
chrigelburri 8:696c2f9dfc62 245 */
chrigelburri 8:696c2f9dfc62 246
chrigelburri 8:696c2f9dfc62 247
chrigelburri 8:696c2f9dfc62 248
chrigelburri 8:696c2f9dfc62 249
chrigelburri 6:48eeb41188dd 250 /*
chrigelburri 6:48eeb41188dd 251 printf("here we go... \n");
chrigelburri 6:48eeb41188dd 252 AdkTerm.setupDevice();
chrigelburri 6:48eeb41188dd 253 printf("Android Development Kit: start\r\n");
chrigelburri 6:48eeb41188dd 254 USBInit();
chrigelburri 6:48eeb41188dd 255 while (!(s.millis >= 60000)) {
chrigelburri 6:48eeb41188dd 256 USBLoop();
chrigelburri 6:48eeb41188dd 257
chrigelburri 6:48eeb41188dd 258 printf("x: %f y: %f theta: %f", AdkTerm.getx(), AdkTerm.getx(), AdkTerm.getx() )
chrigelburri 6:48eeb41188dd 259
chrigelburri 6:48eeb41188dd 260 if( AdkTerm.getx() == 99) {
chrigelburri 6:48eeb41188dd 261 break;
chrigelburri 6:48eeb41188dd 262 }
chrigelburri 6:48eeb41188dd 263 }
chrigelburri 6:48eeb41188dd 264 */
chrigelburri 2:d8e1613dc38b 265
chrigelburri 2:d8e1613dc38b 266 state.savePlotFile(s);
chrigelburri 1:6cd533a712c6 267 state.closePlotFile();
chrigelburri 1:6cd533a712c6 268 state.stop();
chrigelburri 1:6cd533a712c6 269 robotControl.setEnable(false);
chrigelburri 1:6cd533a712c6 270 }