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@29:937c74ff040e, 2013-05-20 (annotated)

Committer:
chrigelburri
Date:
Mon May 20 12:44:49 2013 +0000
Revision:
29:937c74ff040e
Parent:
28:b3e195e80439
Child:
30:c32fc6174efe
drive with android

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 28:b3e195e80439 42 PwmOut led[4] = {LED1, LED2, LED3, LED4};
chrigelburri 24:08241be546ba 43
chrigelburri 24:08241be546ba 44 /**
chrigelburri 11:775ebb69d5e1 45 * @name Hallsensor
chrigelburri 11:775ebb69d5e1 46 * @{
chrigelburri 11:775ebb69d5e1 47 */
chrigelburri 12:235e318a414f 48
chrigelburri 12:235e318a414f 49 /**
chrigelburri 12:235e318a414f 50 * @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16
chrigelburri 12:235e318a414f 51 */
chrigelburri 11:775ebb69d5e1 52 Hallsensor hallLeft(p18, p17, p16);
chrigelburri 12:235e318a414f 53
chrigelburri 11:775ebb69d5e1 54 /**
chrigelburri 11:775ebb69d5e1 55 * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29
chrigelburri 11:775ebb69d5e1 56 */
chrigelburri 11:775ebb69d5e1 57 Hallsensor hallRight(p27, p28, p29);
chrigelburri 11:775ebb69d5e1 58 /*! @} */
chrigelburri 1:6cd533a712c6 59
chrigelburri 10:09ddb819fdcb 60 /**
chrigelburri 11:775ebb69d5e1 61 * @name Motors and Robot Control
chrigelburri 12:235e318a414f 62 * @{
chrigelburri 10:09ddb819fdcb 63 */
chrigelburri 12:235e318a414f 64
chrigelburri 12:235e318a414f 65 /**
chrigelburri 12:235e318a414f 66 * @brief <code>leftMotor</code> object with pin26, pin25, pin24,
chrigelburri 12:235e318a414f 67 * pin19 and <code>hallsensorLeft</code> object
chrigelburri 12:235e318a414f 68 */
chrigelburri 1:6cd533a712c6 69 MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
chrigelburri 12:235e318a414f 70
chrigelburri 11:775ebb69d5e1 71 /**
chrigelburri 12:235e318a414f 72 * @brief <code>rightMotor</code> object with pin23, pin22, pin21,
chrigelburri 12:235e318a414f 73 * pin20 and <code>hallsensorRight</code> object
chrigelburri 11:775ebb69d5e1 74 */
chrigelburri 1:6cd533a712c6 75 MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
chrigelburri 1:6cd533a712c6 76
chrigelburri 11:775ebb69d5e1 77 /**
chrigelburri 12:235e318a414f 78 * @brief <code>robotControl</code> object with <code>leftMotor</code>,
chrigelburri 12:235e318a414f 79 * <code>rightMotor</code> and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 80 */
chrigelburri 11:775ebb69d5e1 81 RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL);
chrigelburri 11:775ebb69d5e1 82 /*! @} */
chrigelburri 1:6cd533a712c6 83
chrigelburri 11:775ebb69d5e1 84 /**
chrigelburri 11:775ebb69d5e1 85 * @name Logging & State
chrigelburri 11:775ebb69d5e1 86 * @{
chrigelburri 11:775ebb69d5e1 87 */
chrigelburri 12:235e318a414f 88
chrigelburri 12:235e318a414f 89 /**
chrigelburri 12:235e318a414f 90 * @brief Define the struct for the State and the Logging
chrigelburri 12:235e318a414f 91 */
chrigelburri 11:775ebb69d5e1 92 state_t s;
chrigelburri 12:235e318a414f 93
chrigelburri 11:775ebb69d5e1 94 /**
chrigelburri 12:235e318a414f 95 * @brief <code>state</code> object with <code>robotControl</code>,
chrigelburri 12:235e318a414f 96 * <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code>
chrigelburri 12:235e318a414f 97 * and the sampling rate for the run method
chrigelburri 11:775ebb69d5e1 98 */
chrigelburri 11:775ebb69d5e1 99 State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE);
chrigelburri 11:775ebb69d5e1 100 /*! @} */
chrigelburri 1:6cd533a712c6 101
chrigelburri 14:6a45a9f940a8 102 /**
chrigelburri 14:6a45a9f940a8 103 * @name Communication
chrigelburri 14:6a45a9f940a8 104 * @{
chrigelburri 14:6a45a9f940a8 105 */
chrigelburri 14:6a45a9f940a8 106
chrigelburri 14:6a45a9f940a8 107 /*! @} */
chrigelburri 14:6a45a9f940a8 108
chrigelburri 12:235e318a414f 109 // @todo PC USB communications DAs wird danach gelöscht
chrigelburri 18:306d362d692b 110 Serial pc(USBTX, USBRX);
chrigelburri 1:6cd533a712c6 111
chrigelburri 12:235e318a414f 112 /**
chrigelburri 12:235e318a414f 113 * @brief Main function. Start the Programm here.
chrigelburri 12:235e318a414f 114 */
chrigelburri 1:6cd533a712c6 115 int main()
chrigelburri 1:6cd533a712c6 116 {
chrigelburri 12:235e318a414f 117
chrigelburri 12:235e318a414f 118 /**
chrigelburri 22:bfec16575c91 119 * Check at first the Battery voltage. Starts when the voltages greater as the min is.
chrigelburri 12:235e318a414f 120 * start the timer for the Logging to the file
chrigelburri 12:235e318a414f 121 * and start the Task for logging
chrigelburri 12:235e318a414f 122 **/
chrigelburri 22:bfec16575c91 123 state.start();
chrigelburri 24:08241be546ba 124 while(s.voltageBattery < BAT_MIN) {
chrigelburri 24:08241be546ba 125 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 126 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 127 led[i] = state.dim( i, f );
chrigelburri 24:08241be546ba 128 }
chrigelburri 24:08241be546ba 129 wait_ms(20);
chrigelburri 24:08241be546ba 130 }
chrigelburri 24:08241be546ba 131 wait(0.05);
chrigelburri 24:08241be546ba 132 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 133 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 134 led[i] = state.dim( 3-i, f );
chrigelburri 24:08241be546ba 135 }
chrigelburri 24:08241be546ba 136 wait_ms(20);
chrigelburri 24:08241be546ba 137 }
chrigelburri 24:08241be546ba 138 wait(0.05);
chrigelburri 24:08241be546ba 139 }
chrigelburri 24:08241be546ba 140 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 141 led[i] = state.dim( 0, 0.0f );
chrigelburri 24:08241be546ba 142 }
chrigelburri 28:b3e195e80439 143
chrigelburri 22:bfec16575c91 144 wait(0.5);
chrigelburri 16:b5d949136a21 145 state.initPlotFile();
chrigelburri 12:235e318a414f 146 state.startTimerFromZero();
chrigelburri 28:b3e195e80439 147 robotControl.start();
chrigelburri 12:235e318a414f 148
chrigelburri 12:235e318a414f 149 /**
chrigelburri 12:235e318a414f 150 * Clear all Errors of the ESCON Module, with a disabled to enable event
chrigelburri 12:235e318a414f 151 */
chrigelburri 1:6cd533a712c6 152 robotControl.setEnable(false);
chrigelburri 16:b5d949136a21 153 wait(0.01);
chrigelburri 1:6cd533a712c6 154 robotControl.setEnable(true);
chrigelburri 6:48eeb41188dd 155 wait(0.1);
chrigelburri 28:b3e195e80439 156 robotControl.setEnable(false);
chrigelburri 12:235e318a414f 157
chrigelburri 28:b3e195e80439 158 // Verbindung zum putty kann spöter gelöscht werden......
chrigelburri 28:b3e195e80439 159 pc.baud(460800);
chrigelburri 28:b3e195e80439 160 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 28:b3e195e80439 161
chrigelburri 29:937c74ff040e 162 /**
chrigelburri 29:937c74ff040e 163 * Initialise the ADB subsystem.
chrigelburri 29:937c74ff040e 164 */
chrigelburri 28:b3e195e80439 165 init();
chrigelburri 28:b3e195e80439 166 setDesiredTheta(400.0f);
chrigelburri 28:b3e195e80439 167 pc.printf("connection isOpen\n");
chrigelburri 28:b3e195e80439 168
chrigelburri 28:b3e195e80439 169 while(getDesiredTheta() < (4 * PI)) {
chrigelburri 28:b3e195e80439 170 ADB::poll();
chrigelburri 28:b3e195e80439 171 if (getDesiredTheta() > (2 * PI)) {
chrigelburri 29:937c74ff040e 172 /**
chrigelburri 29:937c74ff040e 173 * Runs at first till the Startbutton has pressed
chrigelburri 29:937c74ff040e 174 */
chrigelburri 28:b3e195e80439 175 state.startTimerFromZero();
chrigelburri 28:b3e195e80439 176 robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
chrigelburri 28:b3e195e80439 177 robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
chrigelburri 28:b3e195e80439 178 robotControl.setEnable(false);
chrigelburri 29:937c74ff040e 179 wait(0.1);
chrigelburri 28:b3e195e80439 180 } else {
chrigelburri 28:b3e195e80439 181 robotControl.setEnable(true);
chrigelburri 28:b3e195e80439 182 robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
chrigelburri 29:937c74ff040e 183 wait(0.1);
chrigelburri 28:b3e195e80439 184 state.savePlotFile(s);
chrigelburri 28:b3e195e80439 185 //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
chrigelburri 28:b3e195e80439 186 }
chrigelburri 28:b3e195e80439 187
chrigelburri 28:b3e195e80439 188 writeActualPosition(robotControl.getxActualPosition(),
chrigelburri 28:b3e195e80439 189 robotControl.getyActualPosition(),
chrigelburri 28:b3e195e80439 190 robotControl.getActualTheta(),
chrigelburri 28:b3e195e80439 191 s.state&STATE_UNDER,
chrigelburri 28:b3e195e80439 192 s.state&STATE_LEFT,
chrigelburri 28:b3e195e80439 193 s.state&STATE_RIGHT,
chrigelburri 28:b3e195e80439 194 s.voltageBattery);
chrigelburri 28:b3e195e80439 195 //connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 28:b3e195e80439 196
chrigelburri 29:937c74ff040e 197 wait(0.1);
chrigelburri 28:b3e195e80439 198
chrigelburri 28:b3e195e80439 199 }
chrigelburri 6:48eeb41188dd 200
chrigelburri 22:bfec16575c91 201 /**
chrigelburri 29:937c74ff040e 202 * Sets the acutal value for a fast stop.
chrigelburri 28:b3e195e80439 203 * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm
chrigelburri 22:bfec16575c91 204 */
chrigelburri 28:b3e195e80439 205 robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(),
chrigelburri 28:b3e195e80439 206 robotControl.getyActualPosition(),
chrigelburri 28:b3e195e80439 207 robotControl.getActualTheta());
chrigelburri 26:a201dcd4e618 208 robotControl.stop();
chrigelburri 26:a201dcd4e618 209 state.savePlotFile(s);
chrigelburri 26:a201dcd4e618 210 leftMotor.setVelocity(0.0f);
chrigelburri 26:a201dcd4e618 211 rightMotor.setVelocity(0.0f);
chrigelburri 29:937c74ff040e 212 // while(!(s.millis >= 15000)) {
chrigelburri 29:937c74ff040e 213 // state.savePlotFile(s);
chrigelburri 29:937c74ff040e 214 // };
chrigelburri 8:696c2f9dfc62 215
chrigelburri 21:48248c5b8992 216 /**
chrigelburri 21:48248c5b8992 217 * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm
chrigelburri 21:48248c5b8992 218 */
chrigelburri 10:09ddb819fdcb 219 state.savePlotFile(s);
chrigelburri 10:09ddb819fdcb 220 state.closePlotFile();
chrigelburri 10:09ddb819fdcb 221 state.stop();
chrigelburri 10:09ddb819fdcb 222 robotControl.setEnable(false);
chrigelburri 24:08241be546ba 223 robotControl.stop();
chrigelburri 24:08241be546ba 224
chrigelburri 24:08241be546ba 225 /**
chrigelburri 24:08241be546ba 226 * Fast PWM sample for the end
chrigelburri 24:08241be546ba 227 */
chrigelburri 24:08241be546ba 228 while(1) {
chrigelburri 24:08241be546ba 229 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 230 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 231 led[i] = state.dim( i, f );
chrigelburri 24:08241be546ba 232 }
chrigelburri 24:08241be546ba 233 wait_ms(5);
chrigelburri 24:08241be546ba 234 }
chrigelburri 24:08241be546ba 235 wait(0.1);
chrigelburri 24:08241be546ba 236 for (float f = 0.1f; f < 6.3f; f += 0.1f) {
chrigelburri 24:08241be546ba 237 for(int i = 0; i <= 3; i ++) {
chrigelburri 24:08241be546ba 238 led[i] = state.dim( 3-i, f );
chrigelburri 24:08241be546ba 239 }
chrigelburri 24:08241be546ba 240 wait_ms(5);
chrigelburri 24:08241be546ba 241 }
chrigelburri 24:08241be546ba 242 wait(0.05);
chrigelburri 24:08241be546ba 243 }
chrigelburri 21:48248c5b8992 244
chrigelburri 10:09ddb819fdcb 245 }