AVR Competition / Mbed 2 deprecated AVC_Robot_Controled_Navigation

First release.

Dependencies:   FXOS8700CQ SDFileSystem mbed

Final program

  • Controling a robot by Bluetooth, it is capable of making 90° turns and move pre-defined distances.

Code by:

  • Mayumi Hori
  • Sarahí Morán
  • Gerardo Carmona

main.cpp@4:c60636c95b80, 2014-10-17 (annotated)

Committer:
gerardo_carmona
Date:
Fri Oct 17 09:08:06 2014 +0000
Revision:
4:c60636c95b80
Parent:
3:bd16e43ad7be
Child:
5:b384cf06de76
Mayu..

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gerardo_carmona 0:3a322aad8c88 1 /* ==================================================================================
gerardo_carmona 0:3a322aad8c88 2 --- TEST 1 ---
gerardo_carmona 0:3a322aad8c88 3 OBJETIVE:
gerardo_carmona 0:3a322aad8c88 4 Move the motor via bluetooth and program 90° turns to the right and left using
gerardo_carmona 0:3a322aad8c88 5 the magnometer. It will report data from the encoders and limit the movement by
gerardo_carmona 0:3a322aad8c88 6 the ultrasonic sensors
gerardo_carmona 0:3a322aad8c88 7
gerardo_carmona 3:bd16e43ad7be 8
gerardo_carmona 0:3a322aad8c88 9 COMPONENTS:
gerardo_carmona 0:3a322aad8c88 10 * Bluetooth
gerardo_carmona 2:94059cb643be 11 * Magnometer (FXOS8700CQ & magnometer.h)
gerardo_carmona 2:94059cb643be 12 * Motors (motors.h)
gerardo_carmona 3:bd16e43ad7be 13 * Encoder (encoder.h)
gerardo_carmona 2:94059cb643be 14 * Ultrasonic sensors (motors.h)
gerardo_carmona 3:bd16e43ad7be 15 * SD Card (sd_card.h & SDFileSystem)
gerardo_carmona 3:bd16e43ad7be 16 * xbee (xbee.h)
gerardo_carmona 3:bd16e43ad7be 17
gerardo_carmona 0:3a322aad8c88 18
gerardo_carmona 0:3a322aad8c88 19 CONTROLS:
gerardo_carmona 0:3a322aad8c88 20 w: foward
gerardo_carmona 0:3a322aad8c88 21 s: reverse
gerardo_carmona 0:3a322aad8c88 22 a: left turn
gerardo_carmona 0:3a322aad8c88 23 d: right turn
gerardo_carmona 0:3a322aad8c88 24 o: 90° left turn
gerardo_carmona 0:3a322aad8c88 25 p: 90° right turn
gerardo_carmona 0:3a322aad8c88 26
gerardo_carmona 3:bd16e43ad7be 27
gerardo_carmona 0:3a322aad8c88 28 Credits:
gerardo_carmona 0:3a322aad8c88 29 * Mayumi Haro
gerardo_carmona 0:3a322aad8c88 30 * Sarahí Morán
gerardo_carmona 0:3a322aad8c88 31 * Gerardo Carmona
gerardo_carmona 0:3a322aad8c88 32
gerardo_carmona 0:3a322aad8c88 33 VERSION:
gerardo_carmona 3:bd16e43ad7be 34 1.3 - 10/17/14
gerardo_carmona 3:bd16e43ad7be 35
gerardo_carmona 2:94059cb643be 36
gerardo_carmona 0:3a322aad8c88 37 HISTORY:
gerardo_carmona 3:bd16e43ad7be 38 1.3 - 10/17/14
gerardo_carmona 3:bd16e43ad7be 39 (+) Added nuw functions
gerardo_carmona 3:bd16e43ad7be 40
gerardo_carmona 2:94059cb643be 41 1.2 - 10/16/14
gerardo_carmona 2:94059cb643be 42 (+) Added xbee, ultrasonic, sd_card
gerardo_carmona 2:94059cb643be 43
gerardo_carmona 1:ab09b233da7b 44 1.1 - 10/16/14
gerardo_carmona 1:ab09b233da7b 45 (*) Some bug fixes
gerardo_carmona 2:94059cb643be 46 (+) Added encoder.h and encpoder.cpp (empty files)
gerardo_carmona 1:ab09b233da7b 47
gerardo_carmona 0:3a322aad8c88 48 1.0 - 10/14/14
gerardo_carmona 0:3a322aad8c88 49 (+) Initial release.
gerardo_carmona 0:3a322aad8c88 50
gerardo_carmona 0:3a322aad8c88 51
gerardo_carmona 0:3a322aad8c88 52 ================================================================================== */
gerardo_carmona 0:3a322aad8c88 53
gerardo_carmona 0:3a322aad8c88 54 // ----- Libraries ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 55 #include "mbed.h"
gerardo_carmona 0:3a322aad8c88 56 #include "motors.h"
gerardo_carmona 0:3a322aad8c88 57 #include "magnometer.h"
gerardo_carmona 2:94059cb643be 58 #include "encoder.h"
gerardo_carmona 2:94059cb643be 59 #include "sd_card.h"
gerardo_carmona 2:94059cb643be 60 #include "xbee.h"
gerardo_carmona 3:bd16e43ad7be 61 #include "encoders.h"
gerardo_carmona 3:bd16e43ad7be 62 #include "ultrasonic.h"
gerardo_carmona 0:3a322aad8c88 63
gerardo_carmona 0:3a322aad8c88 64 // ----- Constants ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 65 #define LED_ERROR 6
gerardo_carmona 0:3a322aad8c88 66 #define LED_NORMAL 5
gerardo_carmona 0:3a322aad8c88 67 #define LED_BUSY 3
gerardo_carmona 3:bd16e43ad7be 68 // Motores
gerardo_carmona 3:bd16e43ad7be 69 #define MOVE_FWD_F 'W'
gerardo_carmona 3:bd16e43ad7be 70 #define MOVE_REV_F 'S'
gerardo_carmona 3:bd16e43ad7be 71 #define MOVE_LEF_F 'A'
gerardo_carmona 3:bd16e43ad7be 72 #define MOVE_RIG_F 'D'
gerardo_carmona 3:bd16e43ad7be 73 #define MOVE_FWD_S 'w'
gerardo_carmona 3:bd16e43ad7be 74 #define MOVE_REV_S 's'
gerardo_carmona 3:bd16e43ad7be 75 #define MOVE_LEF_S 'a'
gerardo_carmona 3:bd16e43ad7be 76 #define MOVE_RIG_S 'd'
gerardo_carmona 3:bd16e43ad7be 77 #define MOVE_STO 'q'
gerardo_carmona 3:bd16e43ad7be 78 #define MOVE_90L 'o'
gerardo_carmona 3:bd16e43ad7be 79 #define MOVE_90R 'p'
gerardo_carmona 4:c60636c95b80 80 #define GET_GPS 'g'
gerardo_carmona 3:bd16e43ad7be 81 // Otros
gerardo_carmona 3:bd16e43ad7be 82 #define reset_encoders 'r'
gerardo_carmona 3:bd16e43ad7be 83 #define request_data 'm'
gerardo_carmona 3:bd16e43ad7be 84 #define encoder_rigth 1
gerardo_carmona 3:bd16e43ad7be 85 #define encoder_left 2
gerardo_carmona 0:3a322aad8c88 86
gerardo_carmona 0:3a322aad8c88 87 // ----- I/O Pins -------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 88 Serial pc(USBTX, USBRX);
gerardo_carmona 0:3a322aad8c88 89 Serial bt(PTC15, PTC14);
gerardo_carmona 0:3a322aad8c88 90 // Leds for status
gerardo_carmona 0:3a322aad8c88 91 BusOut leds(LED_RED, LED_GREEN, LED_BLUE);
gerardo_carmona 4:c60636c95b80 92 Serial gps(PTC17, PTC16);
gerardo_carmona 0:3a322aad8c88 93
gerardo_carmona 0:3a322aad8c88 94 // Sensors
gerardo_carmona 0:3a322aad8c88 95 AnalogIn ultra_left(A2);
gerardo_carmona 0:3a322aad8c88 96 AnalogIn ultra_right(A3);
gerardo_carmona 3:bd16e43ad7be 97 //DigitalIn encoder_left(D3);
gerardo_carmona 3:bd16e43ad7be 98 //DigitalIn encoder_right(D4);
gerardo_carmona 0:3a322aad8c88 99
gerardo_carmona 0:3a322aad8c88 100 // ----- Others ---------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 101 Timer bt_timer;
gerardo_carmona 0:3a322aad8c88 102 Timer gps_timer;
gerardo_carmona 0:3a322aad8c88 103
gerardo_carmona 0:3a322aad8c88 104 // ----- Variables ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 105 char bt_data;
gerardo_carmona 4:c60636c95b80 106 float lat_val, lon_val;
gerardo_carmona 0:3a322aad8c88 107
gerardo_carmona 0:3a322aad8c88 108 // ----- Function prototypes --------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 109 void read_bt();
gerardo_carmona 3:bd16e43ad7be 110 void command_bt(char _command);
gerardo_carmona 3:bd16e43ad7be 111 void send_all_data();
gerardo_carmona 4:c60636c95b80 112 void gps_data();
gerardo_carmona 0:3a322aad8c88 113
gerardo_carmona 0:3a322aad8c88 114 // ----- Main program ---------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 115 int main(){
gerardo_carmona 3:bd16e43ad7be 116 init_encoders();
gerardo_carmona 4:c60636c95b80 117 init_magnometer();
gerardo_carmona 0:3a322aad8c88 118 bt.baud(9600);
gerardo_carmona 0:3a322aad8c88 119 pc.baud(9600);
gerardo_carmona 4:c60636c95b80 120 gps.baud(4800);
gerardo_carmona 0:3a322aad8c88 121 leds = LED_NORMAL;
gerardo_carmona 0:3a322aad8c88 122 while (true){
gerardo_carmona 4:c60636c95b80 123 read_bt();
gerardo_carmona 4:c60636c95b80 124 gps_data();
gerardo_carmona 0:3a322aad8c88 125 }
gerardo_carmona 0:3a322aad8c88 126 }
gerardo_carmona 0:3a322aad8c88 127
gerardo_carmona 0:3a322aad8c88 128 // ----- Functions ------------------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 129 void read_bt(){
gerardo_carmona 0:3a322aad8c88 130 char c = ' ';
gerardo_carmona 0:3a322aad8c88 131 if (bt.readable()){
gerardo_carmona 0:3a322aad8c88 132 c = bt.getc();
gerardo_carmona 0:3a322aad8c88 133 bt.printf("%c\n\r", c);
gerardo_carmona 3:bd16e43ad7be 134 //}
gerardo_carmona 3:bd16e43ad7be 135 //if (c != ' '){
gerardo_carmona 3:bd16e43ad7be 136 command_bt(c);
gerardo_carmona 0:3a322aad8c88 137 }
gerardo_carmona 3:bd16e43ad7be 138 }
gerardo_carmona 3:bd16e43ad7be 139
gerardo_carmona 3:bd16e43ad7be 140 void command_bt(char _command){
gerardo_carmona 3:bd16e43ad7be 141 switch (_command){
gerardo_carmona 3:bd16e43ad7be 142 case MOVE_FWD_F:
gerardo_carmona 3:bd16e43ad7be 143 motor_fwd(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 144 break;
gerardo_carmona 3:bd16e43ad7be 145 case MOVE_REV_F:
gerardo_carmona 3:bd16e43ad7be 146 motor_rev(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 147 break;
gerardo_carmona 3:bd16e43ad7be 148 case MOVE_LEF_F:
gerardo_carmona 3:bd16e43ad7be 149 motor_left(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 150 break;
gerardo_carmona 3:bd16e43ad7be 151 case MOVE_RIG_F:
gerardo_carmona 3:bd16e43ad7be 152 motor_right(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 153 break;
gerardo_carmona 3:bd16e43ad7be 154 case MOVE_FWD_S:
gerardo_carmona 3:bd16e43ad7be 155 motor_fwd(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 156 break;
gerardo_carmona 3:bd16e43ad7be 157 case MOVE_REV_S:
gerardo_carmona 3:bd16e43ad7be 158 motor_rev(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 159 break;
gerardo_carmona 3:bd16e43ad7be 160 case MOVE_LEF_S:
gerardo_carmona 3:bd16e43ad7be 161 motor_left(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 162 break;
gerardo_carmona 3:bd16e43ad7be 163 case MOVE_RIG_S:
gerardo_carmona 3:bd16e43ad7be 164 motor_right(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 165 break;
gerardo_carmona 3:bd16e43ad7be 166 case MOVE_STO:
gerardo_carmona 3:bd16e43ad7be 167 motor_stop();
gerardo_carmona 3:bd16e43ad7be 168 break;
gerardo_carmona 4:c60636c95b80 169 case GET_GPS:
gerardo_carmona 4:c60636c95b80 170 gps_data();
gerardo_carmona 4:c60636c95b80 171 break;
gerardo_carmona 3:bd16e43ad7be 172 //case MOVE_90L:
gerardo_carmona 3:bd16e43ad7be 173 // move_90(1);
gerardo_carmona 3:bd16e43ad7be 174 // break;
gerardo_carmona 3:bd16e43ad7be 175 //case MOVE_90R:
gerardo_carmona 3:bd16e43ad7be 176 // move_90(2);
gerardo_carmona 3:bd16e43ad7be 177 // break;
gerardo_carmona 3:bd16e43ad7be 178 case reset_encoders:
gerardo_carmona 3:bd16e43ad7be 179 encoders_to_zero();
gerardo_carmona 3:bd16e43ad7be 180 break;
gerardo_carmona 3:bd16e43ad7be 181 case request_data:
gerardo_carmona 3:bd16e43ad7be 182 send_all_data();
gerardo_carmona 3:bd16e43ad7be 183 break;
gerardo_carmona 3:bd16e43ad7be 184 default:
gerardo_carmona 3:bd16e43ad7be 185 motor_stop();
gerardo_carmona 3:bd16e43ad7be 186 // bt.printf("%f\r\n", get_mag_angle());
gerardo_carmona 3:bd16e43ad7be 187 break;
gerardo_carmona 3:bd16e43ad7be 188 }
gerardo_carmona 3:bd16e43ad7be 189 }
gerardo_carmona 3:bd16e43ad7be 190
gerardo_carmona 3:bd16e43ad7be 191 void send_all_data(){
gerardo_carmona 3:bd16e43ad7be 192 bt.printf("sending all data...\n");
gerardo_carmona 3:bd16e43ad7be 193 bt.printf("Right encoder: %f cms\tLeft encoder: %f cms\n", encoder(encoder_rigth), encoder(encoder_rigth));
gerardo_carmona 3:bd16e43ad7be 194 bt.printf("Rigt distance: %fcms\tLeft distance: %fcms\n", ultrasonicos(ULTRA_R), ultrasonicos(ULTRA_L));
gerardo_carmona 3:bd16e43ad7be 195 bt.printf("Angle: %f\n", get_mag_angle());
gerardo_carmona 4:c60636c95b80 196 }
gerardo_carmona 4:c60636c95b80 197
gerardo_carmona 4:c60636c95b80 198 void gps_data(){
gerardo_carmona 4:c60636c95b80 199 leds = LED_BUSY;
gerardo_carmona 4:c60636c95b80 200
gerardo_carmona 4:c60636c95b80 201 if (gps.readable()){
gerardo_carmona 4:c60636c95b80 202
gerardo_carmona 4:c60636c95b80 203 char lat[10];
gerardo_carmona 4:c60636c95b80 204 char lon[10];
gerardo_carmona 4:c60636c95b80 205 char c;
gerardo_carmona 4:c60636c95b80 206 char str[200];
gerardo_carmona 4:c60636c95b80 207 c = gps.getc();
gerardo_carmona 4:c60636c95b80 208 if (c == '$') {
gerardo_carmona 4:c60636c95b80 209 gps.scanf ("%199s",str); // Get all the data
gerardo_carmona 4:c60636c95b80 210 //pc.printf("%s \n",str);
gerardo_carmona 4:c60636c95b80 211 // Checking if its the nmea string that we need
gerardo_carmona 4:c60636c95b80 212 if (str[2] == 'G' && str[3] == 'G' && str[4] == 'A'){
gerardo_carmona 4:c60636c95b80 213 int i = 0;
gerardo_carmona 4:c60636c95b80 214 int j = 0;
gerardo_carmona 4:c60636c95b80 215 c = ' ';
gerardo_carmona 4:c60636c95b80 216 int array_size = 0;
gerardo_carmona 4:c60636c95b80 217 array_size = sizeof(str);
gerardo_carmona 4:c60636c95b80 218 // Searching for the second comma
gerardo_carmona 4:c60636c95b80 219 for (i = 6; i < array_size; i++){
gerardo_carmona 4:c60636c95b80 220 c = str[i];
gerardo_carmona 4:c60636c95b80 221 if (c == ',') break;
gerardo_carmona 4:c60636c95b80 222 }
gerardo_carmona 4:c60636c95b80 223 // Latitud
gerardo_carmona 4:c60636c95b80 224 c = ' ';
gerardo_carmona 4:c60636c95b80 225 i++;
gerardo_carmona 4:c60636c95b80 226 j = 0;
gerardo_carmona 4:c60636c95b80 227 while ( c != ','){
gerardo_carmona 4:c60636c95b80 228 c = str[i+j];
gerardo_carmona 4:c60636c95b80 229 if (c != ',')
gerardo_carmona 4:c60636c95b80 230 lat[j] = c;
gerardo_carmona 4:c60636c95b80 231 j++;
gerardo_carmona 4:c60636c95b80 232 }
gerardo_carmona 4:c60636c95b80 233 // N
gerardo_carmona 4:c60636c95b80 234 for (i = j; i < array_size; i++){
gerardo_carmona 4:c60636c95b80 235 c = str[i];
gerardo_carmona 4:c60636c95b80 236 if (c == 'N') break;
gerardo_carmona 4:c60636c95b80 237 }
gerardo_carmona 4:c60636c95b80 238
gerardo_carmona 4:c60636c95b80 239 // Obtenemos el valor de longitud
gerardo_carmona 4:c60636c95b80 240 c = ' ';
gerardo_carmona 4:c60636c95b80 241 i = i + 2;
gerardo_carmona 4:c60636c95b80 242 j = 0;
gerardo_carmona 4:c60636c95b80 243 while ( c != ','){
gerardo_carmona 4:c60636c95b80 244 c = str[i+j];
gerardo_carmona 4:c60636c95b80 245 if (c != ',')
gerardo_carmona 4:c60636c95b80 246 lon[j] = c;
gerardo_carmona 4:c60636c95b80 247 j++;
gerardo_carmona 4:c60636c95b80 248 }
gerardo_carmona 4:c60636c95b80 249 //pc.printf("sLatitud %s \n",lat);
gerardo_carmona 4:c60636c95b80 250 //pc.printf("sLongitud %s \n",lon);
gerardo_carmona 4:c60636c95b80 251
gerardo_carmona 4:c60636c95b80 252 lat_val = atof(lat);
gerardo_carmona 4:c60636c95b80 253 lon_val = atof(lon);
gerardo_carmona 4:c60636c95b80 254
gerardo_carmona 4:c60636c95b80 255 pc.printf("Latitud %f \n",lat_val);
gerardo_carmona 4:c60636c95b80 256 pc.printf("Longitud %f \n",lon_val);
gerardo_carmona 4:c60636c95b80 257 wait(0.001);
gerardo_carmona 4:c60636c95b80 258 }
gerardo_carmona 4:c60636c95b80 259
gerardo_carmona 4:c60636c95b80 260 }
gerardo_carmona 4:c60636c95b80 261 }else{
gerardo_carmona 4:c60636c95b80 262 //pc.printf("No gps data available \n");
gerardo_carmona 4:c60636c95b80 263 //leds = LED_ERROR; wait(0.1);
gerardo_carmona 4:c60636c95b80 264 }
gerardo_carmona 4:c60636c95b80 265 leds = LED_NORMAL;
gerardo_carmona 0:3a322aad8c88 266 }