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@14:6a45a9f940a8, 2013-04-11 (annotated)

Committer:
chrigelburri
Date:
Thu Apr 11 09:22:35 2013 +0000
Revision:
14:6a45a9f940a8
Parent:
13:a7c30ee09bae
Child:
15:cb1337567ad4
android 2.0 implemented (untested!)

Who changed what in which revision?

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