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@35:0e9ba5f20512, 2013-05-25 (annotated)

Committer:
chrigelburri
Date:
Sat May 25 08:39:44 2013 +0000
Revision:
35:0e9ba5f20512
Parent:
26:a201dcd4e618 
alle garagen werden angefahren qr code wird immer gelesen. cool;

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 21:48248c5b8992 26 * The connection to an android smartphone is realise with the library MicroBridge(Android ADB) from Junichi Katsu.
chrigelburri 21:48248c5b8992 27 * For more information see here: <a href="http://mbed.org/users/jksoft/code/MicroBridge/">http://mbed.org/users/jksoft/code/MicroBridge/</a>
chrigelburri 21:48248c5b8992 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 11:775ebb69d5e1 41
chrigelburri 11:775ebb69d5e1 42 /**
chrigelburri 24:08241be546ba 43 * @name LED
chrigelburri 24:08241be546ba 44 * @{
chrigelburri 24:08241be546ba 45 */
chrigelburri 24:08241be546ba 46
chrigelburri 24:08241be546ba 47 /**
chrigelburri 24:08241be546ba 48 * @brief The Array of the four LED on the mbed board.
chrigelburri 24:08241be546ba 49 */
chrigelburri 24:08241be546ba 50 PwmOut led[4] = { LED1, LED2, LED3, LED4 };
chrigelburri 24:08241be546ba 51 /*! @} */
chrigelburri 24:08241be546ba 52
chrigelburri 24:08241be546ba 53 /**
chrigelburri 11:775ebb69d5e1 54 * @name Hallsensor
chrigelburri 11:775ebb69d5e1 55 * @{
chrigelburri 11:775ebb69d5e1 56 */
chrigelburri 12:235e318a414f 57
chrigelburri 12:235e318a414f 58 /**
chrigelburri 12:235e318a414f 59 * @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16
chrigelburri 12:235e318a414f 60 */
chrigelburri 11:775ebb69d5e1 61 Hallsensor hallLeft(p18, p17, p16);
chrigelburri 12:235e318a414f 62
chrigelburri 11:775ebb69d5e1 63 /**
chrigelburri 11:775ebb69d5e1 64 * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29
chrigelburri 11:775ebb69d5e1 65 */
chrigelburri 11:775ebb69d5e1 66 Hallsensor hallRight(p27, p28, p29);
chrigelburri 11:775ebb69d5e1 67 /*! @} */
chrigelburri 1:6cd533a712c6 68
chrigelburri 10:09ddb819fdcb 69 /**
chrigelburri 11:775ebb69d5e1 70 * @name Motors and Robot Control
chrigelburri 12:235e318a414f 71 * @{
chrigelburri 10:09ddb819fdcb 72 */
chrigelburri 12:235e318a414f 73
chrigelburri 12:235e318a414f 74 /**
chrigelburri 12:235e318a414f 75 * @brief <code>leftMotor</code> object with pin26, pin25, pin24,
chrigelburri 12:235e318a414f 76 * pin19 and <code>hallsensorLeft</code> object
chrigelburri 12:235e318a414f 77 */
chrigelburri 1:6cd533a712c6 78 MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
chrigelburri 12:235e318a414f 79
chrigelburri 11:775ebb69d5e1 80 /**
chrigelburri 12:235e318a414f 81 * @brief <code>rightMotor</code> object with pin23, pin22, pin21,
chrigelburri 12:235e318a414f 82 * pin20 and <code>hallsensorRight</code> object
chrigelburri 11:775ebb69d5e1 83 */
chrigelburri 1:6cd533a712c6 84 MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
chrigelburri 1:6cd533a712c6 85
chrigelburri 11:775ebb69d5e1 86 /**
chrigelburri 12:235e318a414f 87 * @brief <code>robotControl</code> object with <code>leftMotor</code>,
chrigelburri 12:235e318a414f 88 * <code>rightMotor</code> and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 89 */
chrigelburri 11:775ebb69d5e1 90 RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL);
chrigelburri 11:775ebb69d5e1 91 /*! @} */
chrigelburri 1:6cd533a712c6 92
chrigelburri 11:775ebb69d5e1 93 /**
chrigelburri 11:775ebb69d5e1 94 * @name Logging & State
chrigelburri 11:775ebb69d5e1 95 * @{
chrigelburri 11:775ebb69d5e1 96 */
chrigelburri 12:235e318a414f 97
chrigelburri 12:235e318a414f 98 /**
chrigelburri 12:235e318a414f 99 * @brief Define the struct for the State and the Logging
chrigelburri 12:235e318a414f 100 */
chrigelburri 11:775ebb69d5e1 101 state_t s;
chrigelburri 12:235e318a414f 102
chrigelburri 11:775ebb69d5e1 103 /**
chrigelburri 12:235e318a414f 104 * @brief <code>state</code> object with <code>robotControl</code>,
chrigelburri 12:235e318a414f 105 * <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code>
chrigelburri 12:235e318a414f 106 * and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 107 */
chrigelburri 11:775ebb69d5e1 108 State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE);
chrigelburri 11:775ebb69d5e1 109 /*! @} */
chrigelburri 1:6cd533a712c6 110
chrigelburri 14:6a45a9f940a8 111 /**
chrigelburri 14:6a45a9f940a8 112 * @name Communication
chrigelburri 14:6a45a9f940a8 113 * @{
chrigelburri 14:6a45a9f940a8 114 */
chrigelburri 14:6a45a9f940a8 115
chrigelburri 14:6a45a9f940a8 116 /*! @} */
chrigelburri 14:6a45a9f940a8 117
chrigelburri 14:6a45a9f940a8 118
chrigelburri 12:235e318a414f 119 // @todo PC USB communications DAs wird danach gelöscht
chrigelburri 18:306d362d692b 120 Serial pc(USBTX, USBRX);
chrigelburri 1:6cd533a712c6 121
chrigelburri 12:235e318a414f 122 /**
chrigelburri 12:235e318a414f 123 * @brief Main function. Start the Programm here.
chrigelburri 12:235e318a414f 124 */
chrigelburri 1:6cd533a712c6 125 int main()
chrigelburri 1:6cd533a712c6 126 {
chrigelburri 12:235e318a414f 127
chrigelburri 35:0e9ba5f20512 128 int garagenumber = 5;
chrigelburri 22:bfec16575c91 129 float x = -2.954f + 0.308 * garagenumber;
chrigelburri 22:bfec16575c91 130 float ypre = 1.30f;
chrigelburri 35:0e9ba5f20512 131 float xpre = 0;
chrigelburri 26:a201dcd4e618 132 float ygoal = 0.60f;
chrigelburri 22:bfec16575c91 133
chrigelburri 12:235e318a414f 134 /**
chrigelburri 22:bfec16575c91 135 * Check at first the Battery voltage. Starts when the voltages greater as the min is.
chrigelburri 12:235e318a414f 136 * start the timer for the Logging to the file
chrigelburri 12:235e318a414f 137 * and start the Task for logging
chrigelburri 12:235e318a414f 138 **/
chrigelburri 22:bfec16575c91 139 state.start();
chrigelburri 22:bfec16575c91 140 state.initPlotFile();
chrigelburri 22:bfec16575c91 141 state.closePlotFile();
chrigelburri 24:08241be546ba 142 while(s.voltageBattery < BAT_MIN) {
chrigelburri 24:08241be546ba 143 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 144 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 145 led[i] = state.dim( i, f );
chrigelburri 24:08241be546ba 146 }
chrigelburri 24:08241be546ba 147 wait_ms(20);
chrigelburri 24:08241be546ba 148 }
chrigelburri 24:08241be546ba 149 wait(0.05);
chrigelburri 24:08241be546ba 150 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 151 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 152 led[i] = state.dim( 3-i, f );
chrigelburri 24:08241be546ba 153 }
chrigelburri 24:08241be546ba 154 wait_ms(20);
chrigelburri 24:08241be546ba 155 }
chrigelburri 24:08241be546ba 156 wait(0.05);
chrigelburri 24:08241be546ba 157 }
chrigelburri 24:08241be546ba 158 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 159 led[i] = state.dim( 0, 0.0f );
chrigelburri 24:08241be546ba 160 }
chrigelburri 22:bfec16575c91 161 state.stop();
chrigelburri 22:bfec16575c91 162 wait(0.5);
chrigelburri 22:bfec16575c91 163 state.start();
chrigelburri 16:b5d949136a21 164 state.initPlotFile();
chrigelburri 12:235e318a414f 165 state.startTimerFromZero();
chrigelburri 12:235e318a414f 166
chrigelburri 12:235e318a414f 167 /**
chrigelburri 12:235e318a414f 168 * Clear all Errors of the ESCON Module, with a disabled to enable event
chrigelburri 12:235e318a414f 169 */
chrigelburri 1:6cd533a712c6 170 robotControl.setEnable(false);
chrigelburri 16:b5d949136a21 171 wait(0.01);
chrigelburri 1:6cd533a712c6 172 robotControl.setEnable(true);
chrigelburri 6:48eeb41188dd 173 wait(0.1);
chrigelburri 12:235e318a414f 174
chrigelburri 12:235e318a414f 175 /**
chrigelburri 12:235e318a414f 176 * Set the startposition and start the Task for controlling the roboter.
chrigelburri 12:235e318a414f 177 */
chrigelburri 22:bfec16575c91 178 // robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 22:bfec16575c91 179 robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 8:696c2f9dfc62 180 robotControl.start();
chrigelburri 22:bfec16575c91 181 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 35:0e9ba5f20512 182
chrigelburri 26:a201dcd4e618 183 wait(0.02);
chrigelburri 22:bfec16575c91 184 state.savePlotFile(s);
chrigelburri 6:48eeb41188dd 185
chrigelburri 22:bfec16575c91 186 /**
chrigelburri 22:bfec16575c91 187 * Sets the desired Points.
chrigelburri 22:bfec16575c91 188 */
chrigelburri 22:bfec16575c91 189 robotControl.setDesiredPositionAndAngle(-1.35f, 1.85f, 3*PI/8);
chrigelburri 22:bfec16575c91 190 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 22:bfec16575c91 191 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 192 };
chrigelburri 6:48eeb41188dd 193
chrigelburri 35:0e9ba5f20512 194 robotControl.setDesiredPositionAndAngle(-0.60f, 2.85f, 3*PI/8);
chrigelburri 22:bfec16575c91 195 while(!(robotControl.getDistanceError() <= 0.7)) {
chrigelburri 22:bfec16575c91 196 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 197 };
chrigelburri 6:48eeb41188dd 198
chrigelburri 35:0e9ba5f20512 199 robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f, 3*PI/4);
chrigelburri 35:0e9ba5f20512 200 while(!(robotControl.getDistanceError() <= 0.50)) {
chrigelburri 22:bfec16575c91 201 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 202 };
chrigelburri 26:a201dcd4e618 203 //QR
chrigelburri 35:0e9ba5f20512 204 robotControl.setDesiredPositionAndAngle(-1.5f, 3.5f, PI);
chrigelburri 35:0e9ba5f20512 205 while(!(robotControl.getDistanceError() <= 0.045)) {
chrigelburri 22:bfec16575c91 206 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 207 };
chrigelburri 12:235e318a414f 208
chrigelburri 22:bfec16575c91 209 robotControl.setDesiredPositionAndAngle(-2.0f, 1.8f, -PI/2);
chrigelburri 22:bfec16575c91 210 while(!(robotControl.getDistanceError() <= 1.0)) {
chrigelburri 22:bfec16575c91 211 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 212 };
chrigelburri 22:bfec16575c91 213
chrigelburri 35:0e9ba5f20512 214 if(garagenumber == 1) {
chrigelburri 35:0e9ba5f20512 215 xpre=-2.575f;
chrigelburri 35:0e9ba5f20512 216 xpre -= 0.03f; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 217 x -= 0.03f; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 218 } else if(garagenumber == 2) {
chrigelburri 35:0e9ba5f20512 219 xpre=-2.338f;
chrigelburri 35:0e9ba5f20512 220 xpre -= 0.02f; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 221 x -= 0.02f; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 222 } else if(garagenumber == 3) {
chrigelburri 35:0e9ba5f20512 223 xpre=-2.095f;
chrigelburri 35:0e9ba5f20512 224 xpre -= 0.01; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 225 x -= 0.01; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 226 } else if(garagenumber == 4) {
chrigelburri 35:0e9ba5f20512 227 xpre=-1.845f;
chrigelburri 35:0e9ba5f20512 228 xpre -= 0.01; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 229 x -= 0.01; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 230 } else if(garagenumber == 5) {
chrigelburri 35:0e9ba5f20512 231 xpre=-1.510f;
chrigelburri 35:0e9ba5f20512 232 xpre -= 0.005; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 233 x -= 0.005; /// künstliche korrektur
chrigelburri 35:0e9ba5f20512 234 }
chrigelburri 35:0e9ba5f20512 235
chrigelburri 35:0e9ba5f20512 236 robotControl.setDesiredPositionAndAngle(xpre, ypre, -PI/2);
chrigelburri 22:bfec16575c91 237 while(!(robotControl.getDistanceError() <= 0.4)) {
chrigelburri 22:bfec16575c91 238 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 239 };
chrigelburri 22:bfec16575c91 240 robotControl.setDesiredPositionAndAngle(x, ygoal, -PI/2);
chrigelburri 26:a201dcd4e618 241 while(!(robotControl.getDistanceError() <= 0.2)) {
chrigelburri 22:bfec16575c91 242 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 243 }
chrigelburri 8:696c2f9dfc62 244
chrigelburri 8:696c2f9dfc62 245
chrigelburri 26:a201dcd4e618 246 /// STOP Setze actueler Wert
chrigelburri 26:a201dcd4e618 247 robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getActualTheta());
chrigelburri 26:a201dcd4e618 248 robotControl.stop();
chrigelburri 26:a201dcd4e618 249 state.savePlotFile(s);
chrigelburri 26:a201dcd4e618 250 leftMotor.setVelocity(0.0f);
chrigelburri 26:a201dcd4e618 251 rightMotor.setVelocity(0.0f);
chrigelburri 26:a201dcd4e618 252 while(!(s.millis >= 15000)) {
chrigelburri 26:a201dcd4e618 253 state.savePlotFile(s);
chrigelburri 26:a201dcd4e618 254 };
chrigelburri 8:696c2f9dfc62 255
chrigelburri 21:48248c5b8992 256
chrigelburri 18:306d362d692b 257
chrigelburri 22:bfec16575c91 258 /*
chrigelburri 22:bfec16575c91 259 pc.baud(460800);
chrigelburri 22:bfec16575c91 260 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 21:48248c5b8992 261
chrigelburri 22:bfec16575c91 262 init();
chrigelburri 19:b2f76b0fe4c8 263
chrigelburri 22:bfec16575c91 264 // Initialise the ADB subsystem.
chrigelburri 19:b2f76b0fe4c8 265
chrigelburri 22:bfec16575c91 266 pc.printf("connection isOpen\n");
chrigelburri 21:48248c5b8992 267
chrigelburri 22:bfec16575c91 268 while(getDesiredTheta() < (4 * PI)) {
chrigelburri 22:bfec16575c91 269
chrigelburri 22:bfec16575c91 270 ADB::poll();
chrigelburri 19:b2f76b0fe4c8 271
chrigelburri 22:bfec16575c91 272 if (getDesiredTheta() > (2 * PI)) {
chrigelburri 22:bfec16575c91 273 //robotControl.setAllToZero(0, 0, PI/2 );
chrigelburri 22:bfec16575c91 274 robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 22:bfec16575c91 275 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 22:bfec16575c91 276 }
chrigelburri 22:bfec16575c91 277 else
chrigelburri 22:bfec16575c91 278 {
chrigelburri 22:bfec16575c91 279 robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
chrigelburri 21:48248c5b8992 280
chrigelburri 21:48248c5b8992 281
chrigelburri 22:bfec16575c91 282 state.savePlotFile(s);
chrigelburri 22:bfec16575c91 283 //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
chrigelburri 22:bfec16575c91 284 }
chrigelburri 22:bfec16575c91 285
chrigelburri 22:bfec16575c91 286 writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery);
chrigelburri 22:bfec16575c91 287 //connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 22:bfec16575c91 288
chrigelburri 22:bfec16575c91 289 wait(0.25);
chrigelburri 22:bfec16575c91 290
chrigelburri 22:bfec16575c91 291 }
chrigelburri 22:bfec16575c91 292
chrigelburri 22:bfec16575c91 293 */
chrigelburri 21:48248c5b8992 294 /**
chrigelburri 21:48248c5b8992 295 * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm
chrigelburri 21:48248c5b8992 296 */
chrigelburri 10:09ddb819fdcb 297 state.savePlotFile(s);
chrigelburri 10:09ddb819fdcb 298 state.closePlotFile();
chrigelburri 10:09ddb819fdcb 299 state.stop();
chrigelburri 10:09ddb819fdcb 300 robotControl.setEnable(false);
chrigelburri 24:08241be546ba 301 robotControl.stop();
chrigelburri 24:08241be546ba 302
chrigelburri 24:08241be546ba 303 /**
chrigelburri 24:08241be546ba 304 * Fast PWM sample for the end
chrigelburri 24:08241be546ba 305 */
chrigelburri 24:08241be546ba 306 while(1) {
chrigelburri 24:08241be546ba 307 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 308 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 309 led[i] = state.dim( i, f );
chrigelburri 24:08241be546ba 310 }
chrigelburri 24:08241be546ba 311 wait_ms(5);
chrigelburri 24:08241be546ba 312 }
chrigelburri 24:08241be546ba 313 wait(0.1);
chrigelburri 24:08241be546ba 314 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 315 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 316 led[i] = state.dim( 3-i, f );
chrigelburri 24:08241be546ba 317 }
chrigelburri 24:08241be546ba 318 wait_ms(5);
chrigelburri 24:08241be546ba 319 }
chrigelburri 24:08241be546ba 320 wait(0.05);
chrigelburri 24:08241be546ba 321 }
chrigelburri 21:48248c5b8992 322
chrigelburri 10:09ddb819fdcb 323 }