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@20:01b233b0e606, 2013-05-03 (annotated)

Committer:
chrigelburri
Date:
Fri May 03 14:39:24 2013 +0000
Revision:
20:01b233b0e606
Parent:
19:b2f76b0fe4c8
Child:
21:48248c5b8992
writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b) zus?tzliche states

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 18:306d362d692b 1
chrigelburri 10:09ddb819fdcb 2 /*! \mainpage Index Page
chrigelburri 2:d8e1613dc38b 3 * @author Christian Burri
chrigelburri 11:775ebb69d5e1 4 * @author Arno Galliker
chrigelburri 2:d8e1613dc38b 5 *
chrigelburri 11:775ebb69d5e1 6 * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
chrigelburri 1:6cd533a712c6 7 * All rights reserved.
chrigelburri 2:d8e1613dc38b 8 *
chrigelburri 11:775ebb69d5e1 9 * @brief
chrigelburri 2:d8e1613dc38b 10 *
chrigelburri 15:cb1337567ad4 11 * This program is for an autonomous robot for the competition
chrigelburri 4:3a97923ff2d4 12 * at the Hochschule Luzern.
chrigelburri 3:92ba0254af87 13 * We are one of the 32 teams. In the team #1 is:
chrigelburri 11:775ebb69d5e1 14 * - Bauernfeind Julia <B>WI</B> <a href="mailto:julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 15 * - Büttler Pirmin <B>WI</B> <a href="mailto:pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 16 * - Amberg Reto <B>I</B> <a href="mailto:reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 17 * - Galliker Arno <B>I</B> <a href="mailto:arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 18 * - Amrein Marcel <B>M</B> <a href="mailto:marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 19 * - Flühler Ramon <B>M</B> <a href="mailto:ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
chrigelburri 11:775ebb69d5e1 20 * - Burri Christian <B>ET</B> <a href="mailto:christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
chrigelburri 4:3a97923ff2d4 21 *
chrigelburri 13:a7c30ee09bae 22 * The postition control is based on this Documentation: Control of Wheeled Mobile Robots:
chrigelburri 13:a7c30ee09bae 23 * An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli.
chrigelburri 15:cb1337567ad4 24 * 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>
chrigelburri 2:d8e1613dc38b 25 *
chrigelburri 15:cb1337567ad4 26 * The connection to an android smartphone is realise with the library AndroidAccessory from Rich Bayliss.
chrigelburri 15:cb1337567ad4 27 * For more information see here: <a href="https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/">https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/</a>
chrigelburri 13:a7c30ee09bae 28 *
chrigelburri 13:a7c30ee09bae 29 * The rest of the classes are only based on standard library from mbed.
chrigelburri 15:cb1337567ad4 30 * For more information see here: <a href="http://mbed.org/users/mbed_official/code/mbed/">http://mbed.org/users/mbed_official/code/mbed/</a>
chrigelburri 1:6cd533a712c6 31 */
chrigelburri 1:6cd533a712c6 32
chrigelburri 11:775ebb69d5e1 33 /**
chrigelburri 11:775ebb69d5e1 34 * @file main.cpp
chrigelburri 11:775ebb69d5e1 35 */
chrigelburri 11:775ebb69d5e1 36
chrigelburri 1:6cd533a712c6 37 #include "defines.h"
chrigelburri 1:6cd533a712c6 38 #include "State.h"
chrigelburri 1:6cd533a712c6 39 #include "RobotControl.h"
chrigelburri 18:306d362d692b 40 #include "androidADB.h"
chrigelburri 16:b5d949136a21 41 //#include "android.h"
chrigelburri 11:775ebb69d5e1 42
chrigelburri 11:775ebb69d5e1 43 /**
chrigelburri 11:775ebb69d5e1 44 * @name Hallsensor
chrigelburri 11:775ebb69d5e1 45 * @{
chrigelburri 11:775ebb69d5e1 46 */
chrigelburri 12:235e318a414f 47
chrigelburri 12:235e318a414f 48 /**
chrigelburri 12:235e318a414f 49 * @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16
chrigelburri 12:235e318a414f 50 */
chrigelburri 11:775ebb69d5e1 51 Hallsensor hallLeft(p18, p17, p16);
chrigelburri 12:235e318a414f 52
chrigelburri 11:775ebb69d5e1 53 /**
chrigelburri 11:775ebb69d5e1 54 * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29
chrigelburri 11:775ebb69d5e1 55 */
chrigelburri 11:775ebb69d5e1 56 Hallsensor hallRight(p27, p28, p29);
chrigelburri 11:775ebb69d5e1 57 /*! @} */
chrigelburri 1:6cd533a712c6 58
chrigelburri 10:09ddb819fdcb 59 /**
chrigelburri 11:775ebb69d5e1 60 * @name Motors and Robot Control
chrigelburri 12:235e318a414f 61 * @{
chrigelburri 10:09ddb819fdcb 62 */
chrigelburri 12:235e318a414f 63
chrigelburri 12:235e318a414f 64 /**
chrigelburri 12:235e318a414f 65 * @brief <code>leftMotor</code> object with pin26, pin25, pin24,
chrigelburri 12:235e318a414f 66 * pin19 and <code>hallsensorLeft</code> object
chrigelburri 12:235e318a414f 67 */
chrigelburri 1:6cd533a712c6 68 MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
chrigelburri 12:235e318a414f 69
chrigelburri 11:775ebb69d5e1 70 /**
chrigelburri 12:235e318a414f 71 * @brief <code>rightMotor</code> object with pin23, pin22, pin21,
chrigelburri 12:235e318a414f 72 * pin20 and <code>hallsensorRight</code> object
chrigelburri 11:775ebb69d5e1 73 */
chrigelburri 1:6cd533a712c6 74 MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
chrigelburri 1:6cd533a712c6 75
chrigelburri 11:775ebb69d5e1 76 /**
chrigelburri 12:235e318a414f 77 * @brief <code>robotControl</code> object with <code>leftMotor</code>,
chrigelburri 12:235e318a414f 78 * <code>rightMotor</code> and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 79 */
chrigelburri 11:775ebb69d5e1 80 RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL);
chrigelburri 11:775ebb69d5e1 81 /*! @} */
chrigelburri 1:6cd533a712c6 82
chrigelburri 11:775ebb69d5e1 83 /**
chrigelburri 11:775ebb69d5e1 84 * @name Logging & State
chrigelburri 11:775ebb69d5e1 85 * @{
chrigelburri 11:775ebb69d5e1 86 */
chrigelburri 12:235e318a414f 87
chrigelburri 12:235e318a414f 88 /**
chrigelburri 12:235e318a414f 89 * @brief Define the struct for the State and the Logging
chrigelburri 12:235e318a414f 90 */
chrigelburri 11:775ebb69d5e1 91 state_t s;
chrigelburri 12:235e318a414f 92
chrigelburri 11:775ebb69d5e1 93 /**
chrigelburri 12:235e318a414f 94 * @brief <code>state</code> object with <code>robotControl</code>,
chrigelburri 12:235e318a414f 95 * <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code>
chrigelburri 12:235e318a414f 96 * and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 97 */
chrigelburri 11:775ebb69d5e1 98 State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE);
chrigelburri 11:775ebb69d5e1 99 /*! @} */
chrigelburri 1:6cd533a712c6 100
chrigelburri 14:6a45a9f940a8 101 /**
chrigelburri 14:6a45a9f940a8 102 * @name Communication
chrigelburri 14:6a45a9f940a8 103 * @{
chrigelburri 14:6a45a9f940a8 104 */
chrigelburri 14:6a45a9f940a8 105
chrigelburri 14:6a45a9f940a8 106 /**
chrigelburri 14:6a45a9f940a8 107 * @brief <code>adkTerm</code> object is for the communication with a smartphone.
chrigelburri 14:6a45a9f940a8 108 * The operation system must be a android.
chrigelburri 14:6a45a9f940a8 109 */
chrigelburri 16:b5d949136a21 110 //AdkTerm adkTerm(&robotControl, &s, PERIOD_ANDROID);
chrigelburri 14:6a45a9f940a8 111 /*! @} */
chrigelburri 14:6a45a9f940a8 112
chrigelburri 14:6a45a9f940a8 113
chrigelburri 12:235e318a414f 114 // @todo PC USB communications DAs wird danach gelöscht
chrigelburri 18:306d362d692b 115 Serial pc(USBTX, USBRX);
chrigelburri 1:6cd533a712c6 116
chrigelburri 12:235e318a414f 117 /**
chrigelburri 12:235e318a414f 118 * @brief Main function. Start the Programm here.
chrigelburri 12:235e318a414f 119 */
chrigelburri 1:6cd533a712c6 120 int main()
chrigelburri 1:6cd533a712c6 121 {
chrigelburri 12:235e318a414f 122
chrigelburri 12:235e318a414f 123 /**
chrigelburri 12:235e318a414f 124 * Initialze the filt PLOTS.txt,
chrigelburri 12:235e318a414f 125 * start the timer for the Logging to the file
chrigelburri 12:235e318a414f 126 * and start the Task for logging
chrigelburri 12:235e318a414f 127 **/
chrigelburri 16:b5d949136a21 128 state.initPlotFile();
chrigelburri 12:235e318a414f 129 state.startTimerFromZero();
chrigelburri 12:235e318a414f 130 state.start();
chrigelburri 12:235e318a414f 131
chrigelburri 12:235e318a414f 132 /**
chrigelburri 12:235e318a414f 133 * Clear all Errors of the ESCON Module, with a disabled to enable event
chrigelburri 12:235e318a414f 134 */
chrigelburri 1:6cd533a712c6 135 robotControl.setEnable(false);
chrigelburri 16:b5d949136a21 136 wait(0.01);
chrigelburri 1:6cd533a712c6 137 robotControl.setEnable(true);
chrigelburri 6:48eeb41188dd 138 wait(0.1);
chrigelburri 12:235e318a414f 139
chrigelburri 12:235e318a414f 140 /**
chrigelburri 12:235e318a414f 141 * Set the startposition and start the Task for controlling the roboter.
chrigelburri 12:235e318a414f 142 */
chrigelburri 19:b2f76b0fe4c8 143 robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 19:b2f76b0fe4c8 144 //robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 8:696c2f9dfc62 145 robotControl.start();
chrigelburri 16:b5d949136a21 146 /*
chrigelburri 14:6a45a9f940a8 147 pc.baud(460800);
chrigelburri 14:6a45a9f940a8 148 pc.printf("Welcome to adkTerm\n\n\n\n\n\n\r");
chrigelburri 14:6a45a9f940a8 149 pc.printf("here we go... \n");
chrigelburri 14:6a45a9f940a8 150 adkTerm.setupDevice();
chrigelburri 14:6a45a9f940a8 151 pc.printf("Android Development Kit: start\r\n");
chrigelburri 14:6a45a9f940a8 152 USBInit();
chrigelburri 14:6a45a9f940a8 153
chrigelburri 15:cb1337567ad4 154 //wait(3);
chrigelburri 15:cb1337567ad4 155
chrigelburri 14:6a45a9f940a8 156 adkTerm.start();
chrigelburri 8:696c2f9dfc62 157
chrigelburri 16:b5d949136a21 158 */
chrigelburri 6:48eeb41188dd 159
chrigelburri 6:48eeb41188dd 160 // Epä Parkour fahrä
chrigelburri 18:306d362d692b 161 /*
chrigelburri 11:775ebb69d5e1 162 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 11:775ebb69d5e1 163 wait(0.1);
chrigelburri 6:48eeb41188dd 164
chrigelburri 11:775ebb69d5e1 165 robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f, 3*PI/4);
chrigelburri 11:775ebb69d5e1 166 while(!(robotControl.getDistanceError() <= 0.9)) {
chrigelburri 11:775ebb69d5e1 167 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 168 };
chrigelburri 6:48eeb41188dd 169
chrigelburri 11:775ebb69d5e1 170 robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f, PI/4);
chrigelburri 11:775ebb69d5e1 171 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 11:775ebb69d5e1 172 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 173 };
chrigelburri 5:48a258f6335e 174
chrigelburri 16:b5d949136a21 175 robotControl.setDesiredPositionAndAngle(-0.5f, 3.0f, PI/2); //0.75
chrigelburri 11:775ebb69d5e1 176 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 11:775ebb69d5e1 177 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 178 };
chrigelburri 6:48eeb41188dd 179
chrigelburri 11:775ebb69d5e1 180 robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f, 3*PI/4);
chrigelburri 11:775ebb69d5e1 181 while(!(robotControl.getDistanceError() <= 0.55)) {
chrigelburri 11:775ebb69d5e1 182 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 183 };
chrigelburri 8:696c2f9dfc62 184
chrigelburri 4:3a97923ff2d4 185
chrigelburri 11:775ebb69d5e1 186 robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f, PI);
chrigelburri 11:775ebb69d5e1 187 while(!(robotControl.getDistanceError() <= 0.05)) {
chrigelburri 11:775ebb69d5e1 188 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 189 };
chrigelburri 11:775ebb69d5e1 190
chrigelburri 11:775ebb69d5e1 191 robotControl.setDesiredPositionAndAngle(-2.6f, 3.0f, -PI/2);
chrigelburri 11:775ebb69d5e1 192 while(!(robotControl.getDistanceError() <= 1.0)) {
chrigelburri 11:775ebb69d5e1 193 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 194 };
chrigelburri 15:cb1337567ad4 195
chrigelburri 15:cb1337567ad4 196 robotControl.setDesiredPositionAndAngle(-2.0f, 2.0f, -PI/2);
chrigelburri 15:cb1337567ad4 197 while(!(robotControl.getDistanceError() <= 1.0)) {
chrigelburri 15:cb1337567ad4 198 state.savePlotFile(s);
chrigelburri 15:cb1337567ad4 199 };
chrigelburri 11:775ebb69d5e1 200
chrigelburri 15:cb1337567ad4 201 robotControl.setDesiredPositionAndAngle(-2.65f, 1.30f, -PI/2);
chrigelburri 16:b5d949136a21 202 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 11:775ebb69d5e1 203 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 204 };
chrigelburri 15:cb1337567ad4 205 robotControl.setDesiredPositionAndAngle(-2.65f, 0.80f, -PI/2);
chrigelburri 11:775ebb69d5e1 206 while(!(s.millis >= 32000)) {
chrigelburri 11:775ebb69d5e1 207 state.savePlotFile(s);
chrigelburri 11:775ebb69d5e1 208 }
chrigelburri 18:306d362d692b 209 */
chrigelburri 16:b5d949136a21 210
chrigelburri 16:b5d949136a21 211 /*
chrigelburri 12:235e318a414f 212 while (1) {
chrigelburri 12:235e318a414f 213 USBLoop();
chrigelburri 15:cb1337567ad4 214
chrigelburri 15:cb1337567ad4 215 robotControl.setDesiredPositionAndAngle(adkTerm.getDesiredX(), adkTerm.getDesiredY(), adkTerm.getDesiredTheta());
chrigelburri 15:cb1337567ad4 216
chrigelburri 15:cb1337567ad4 217 //pc.printf(".");
chrigelburri 12:235e318a414f 218
chrigelburri 15:cb1337567ad4 219 /*pc.printf("From Android x: %f y: %f t: %f",adkTerm.getDesiredX(), adkTerm.getDesiredY(),adkTerm.getDesiredTheta());
chrigelburri 15:cb1337567ad4 220
chrigelburri 14:6a45a9f940a8 221
chrigelburri 14:6a45a9f940a8 222 pc.printf( "To Android: x: %f y: %f t: %f; \n \n", robotControl.getxActualPosition(),
chrigelburri 14:6a45a9f940a8 223 robotControl.getyActualPosition(),
chrigelburri 16:b5d949136a21 224 robotControl.getActualTheta());
chrigelburri 16:b5d949136a21 225 }*/
chrigelburri 8:696c2f9dfc62 226
chrigelburri 8:696c2f9dfc62 227
chrigelburri 8:696c2f9dfc62 228
chrigelburri 12:235e318a414f 229 /**
chrigelburri 12:235e318a414f 230 * Close the File PLOTS.txt to read the file after with the computer and draw a diagramm
chrigelburri 12:235e318a414f 231 */
chrigelburri 19:b2f76b0fe4c8 232
chrigelburri 19:b2f76b0fe4c8 233
chrigelburri 19:b2f76b0fe4c8 234 pc.baud(460800);
chrigelburri 18:306d362d692b 235
chrigelburri 19:b2f76b0fe4c8 236 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 19:b2f76b0fe4c8 237
chrigelburri 19:b2f76b0fe4c8 238 init();
chrigelburri 19:b2f76b0fe4c8 239
chrigelburri 19:b2f76b0fe4c8 240 // Initialise the ADB subsystem.
chrigelburri 19:b2f76b0fe4c8 241
chrigelburri 19:b2f76b0fe4c8 242 pc.printf("connection isOpen\n");
chrigelburri 19:b2f76b0fe4c8 243
chrigelburri 19:b2f76b0fe4c8 244 while(1) {
chrigelburri 19:b2f76b0fe4c8 245
chrigelburri 19:b2f76b0fe4c8 246 ADB::poll();
chrigelburri 20:01b233b0e606 247
chrigelburri 20:01b233b0e606 248 robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
chrigelburri 19:b2f76b0fe4c8 249
chrigelburri 20:01b233b0e606 250 //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
chrigelburri 20:01b233b0e606 251 writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery);
chrigelburri 19:b2f76b0fe4c8 252
chrigelburri 19:b2f76b0fe4c8 253 //connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 254 wait(1);
chrigelburri 19:b2f76b0fe4c8 255
chrigelburri 19:b2f76b0fe4c8 256 }
chrigelburri 19:b2f76b0fe4c8 257
chrigelburri 10:09ddb819fdcb 258 state.savePlotFile(s);
chrigelburri 10:09ddb819fdcb 259 state.closePlotFile();
chrigelburri 10:09ddb819fdcb 260 state.stop();
chrigelburri 10:09ddb819fdcb 261 robotControl.setEnable(false);
chrigelburri 15:cb1337567ad4 262
chrigelburri 10:09ddb819fdcb 263 }