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@5:b384cf06de76, 2014-11-07 (annotated)

Committer:
gerardo_carmona
Date:
Fri Nov 07 05:12:50 2014 +0000
Revision:
5:b384cf06de76
Parent:
4:c60636c95b80 
Not finished!

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 "sd_card.h"
gerardo_carmona 2:94059cb643be 59 #include "xbee.h"
gerardo_carmona 3:bd16e43ad7be 60 #include "encoders.h"
gerardo_carmona 3:bd16e43ad7be 61 #include "ultrasonic.h"
gerardo_carmona 0:3a322aad8c88 62
gerardo_carmona 0:3a322aad8c88 63 // ----- Constants ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 64 #define LED_ERROR 6
gerardo_carmona 0:3a322aad8c88 65 #define LED_NORMAL 5
gerardo_carmona 0:3a322aad8c88 66 #define LED_BUSY 3
gerardo_carmona 3:bd16e43ad7be 67 // Motores
gerardo_carmona 3:bd16e43ad7be 68 #define MOVE_FWD_F 'W'
gerardo_carmona 3:bd16e43ad7be 69 #define MOVE_REV_F 'S'
gerardo_carmona 3:bd16e43ad7be 70 #define MOVE_LEF_F 'A'
gerardo_carmona 3:bd16e43ad7be 71 #define MOVE_RIG_F 'D'
gerardo_carmona 3:bd16e43ad7be 72 #define MOVE_FWD_S 'w'
gerardo_carmona 3:bd16e43ad7be 73 #define MOVE_REV_S 's'
gerardo_carmona 3:bd16e43ad7be 74 #define MOVE_LEF_S 'a'
gerardo_carmona 3:bd16e43ad7be 75 #define MOVE_RIG_S 'd'
gerardo_carmona 3:bd16e43ad7be 76 #define MOVE_STO 'q'
gerardo_carmona 3:bd16e43ad7be 77 #define MOVE_90L 'o'
gerardo_carmona 3:bd16e43ad7be 78 #define MOVE_90R 'p'
gerardo_carmona 4:c60636c95b80 79 #define GET_GPS 'g'
gerardo_carmona 3:bd16e43ad7be 80 // Otros
gerardo_carmona 3:bd16e43ad7be 81 #define reset_encoders 'r'
gerardo_carmona 3:bd16e43ad7be 82 #define request_data 'm'
gerardo_carmona 3:bd16e43ad7be 83 #define encoder_rigth 1
gerardo_carmona 3:bd16e43ad7be 84 #define encoder_left 2
gerardo_carmona 0:3a322aad8c88 85
gerardo_carmona 0:3a322aad8c88 86 // ----- I/O Pins -------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 87 Serial pc(USBTX, USBRX);
gerardo_carmona 0:3a322aad8c88 88 Serial bt(PTC15, PTC14);
gerardo_carmona 0:3a322aad8c88 89 // Leds for status
gerardo_carmona 0:3a322aad8c88 90 BusOut leds(LED_RED, LED_GREEN, LED_BLUE);
gerardo_carmona 4:c60636c95b80 91 Serial gps(PTC17, PTC16);
gerardo_carmona 0:3a322aad8c88 92
gerardo_carmona 0:3a322aad8c88 93 // Sensors
gerardo_carmona 0:3a322aad8c88 94 AnalogIn ultra_left(A2);
gerardo_carmona 0:3a322aad8c88 95 AnalogIn ultra_right(A3);
gerardo_carmona 3:bd16e43ad7be 96 //DigitalIn encoder_left(D3);
gerardo_carmona 3:bd16e43ad7be 97 //DigitalIn encoder_right(D4);
gerardo_carmona 0:3a322aad8c88 98
gerardo_carmona 0:3a322aad8c88 99 // ----- Others ---------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 100 Timer bt_timer;
gerardo_carmona 0:3a322aad8c88 101 Timer gps_timer;
gerardo_carmona 0:3a322aad8c88 102
gerardo_carmona 0:3a322aad8c88 103 // ----- Variables ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 104 char bt_data;
gerardo_carmona 4:c60636c95b80 105 float lat_val, lon_val;
gerardo_carmona 0:3a322aad8c88 106
gerardo_carmona 0:3a322aad8c88 107 // ----- Function prototypes --------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 108 void read_bt();
gerardo_carmona 3:bd16e43ad7be 109 void command_bt(char _command);
gerardo_carmona 3:bd16e43ad7be 110 void send_all_data();
gerardo_carmona 4:c60636c95b80 111 void gps_data();
gerardo_carmona 0:3a322aad8c88 112
gerardo_carmona 0:3a322aad8c88 113 // ----- Main program ---------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 114 int main(){
gerardo_carmona 3:bd16e43ad7be 115 init_encoders();
gerardo_carmona 4:c60636c95b80 116 init_magnometer();
gerardo_carmona 0:3a322aad8c88 117 bt.baud(9600);
gerardo_carmona 0:3a322aad8c88 118 pc.baud(9600);
gerardo_carmona 4:c60636c95b80 119 gps.baud(4800);
gerardo_carmona 0:3a322aad8c88 120 leds = LED_NORMAL;
gerardo_carmona 0:3a322aad8c88 121 while (true){
gerardo_carmona 4:c60636c95b80 122 read_bt();
gerardo_carmona 4:c60636c95b80 123 gps_data();
gerardo_carmona 0:3a322aad8c88 124 }
gerardo_carmona 0:3a322aad8c88 125 }
gerardo_carmona 0:3a322aad8c88 126
gerardo_carmona 0:3a322aad8c88 127 // ----- Functions ------------------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 128 void read_bt(){
gerardo_carmona 0:3a322aad8c88 129 char c = ' ';
gerardo_carmona 0:3a322aad8c88 130 if (bt.readable()){
gerardo_carmona 0:3a322aad8c88 131 c = bt.getc();
gerardo_carmona 0:3a322aad8c88 132 bt.printf("%c\n\r", c);
gerardo_carmona 3:bd16e43ad7be 133 //}
gerardo_carmona 5:b384cf06de76 134 //if (c != '\n'){
gerardo_carmona 3:bd16e43ad7be 135 command_bt(c);
gerardo_carmona 0:3a322aad8c88 136 }
gerardo_carmona 3:bd16e43ad7be 137 }
gerardo_carmona 3:bd16e43ad7be 138
gerardo_carmona 3:bd16e43ad7be 139 void command_bt(char _command){
gerardo_carmona 3:bd16e43ad7be 140 switch (_command){
gerardo_carmona 3:bd16e43ad7be 141 case MOVE_FWD_F:
gerardo_carmona 3:bd16e43ad7be 142 motor_fwd(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 143 break;
gerardo_carmona 3:bd16e43ad7be 144 case MOVE_REV_F:
gerardo_carmona 3:bd16e43ad7be 145 motor_rev(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 146 break;
gerardo_carmona 3:bd16e43ad7be 147 case MOVE_LEF_F:
gerardo_carmona 3:bd16e43ad7be 148 motor_left(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 149 break;
gerardo_carmona 3:bd16e43ad7be 150 case MOVE_RIG_F:
gerardo_carmona 3:bd16e43ad7be 151 motor_right(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 152 break;
gerardo_carmona 3:bd16e43ad7be 153 case MOVE_FWD_S:
gerardo_carmona 3:bd16e43ad7be 154 motor_fwd(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 155 break;
gerardo_carmona 3:bd16e43ad7be 156 case MOVE_REV_S:
gerardo_carmona 3:bd16e43ad7be 157 motor_rev(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 158 break;
gerardo_carmona 3:bd16e43ad7be 159 case MOVE_LEF_S:
gerardo_carmona 3:bd16e43ad7be 160 motor_left(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 161 break;
gerardo_carmona 3:bd16e43ad7be 162 case MOVE_RIG_S:
gerardo_carmona 3:bd16e43ad7be 163 motor_right(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 164 break;
gerardo_carmona 3:bd16e43ad7be 165 case MOVE_STO:
gerardo_carmona 3:bd16e43ad7be 166 motor_stop();
gerardo_carmona 3:bd16e43ad7be 167 break;
gerardo_carmona 4:c60636c95b80 168 case GET_GPS:
gerardo_carmona 4:c60636c95b80 169 gps_data();
gerardo_carmona 4:c60636c95b80 170 break;
gerardo_carmona 3:bd16e43ad7be 171 //case MOVE_90L:
gerardo_carmona 3:bd16e43ad7be 172 // move_90(1);
gerardo_carmona 3:bd16e43ad7be 173 // break;
gerardo_carmona 3:bd16e43ad7be 174 //case MOVE_90R:
gerardo_carmona 3:bd16e43ad7be 175 // move_90(2);
gerardo_carmona 3:bd16e43ad7be 176 // break;
gerardo_carmona 3:bd16e43ad7be 177 case reset_encoders:
gerardo_carmona 3:bd16e43ad7be 178 encoders_to_zero();
gerardo_carmona 3:bd16e43ad7be 179 break;
gerardo_carmona 3:bd16e43ad7be 180 case request_data:
gerardo_carmona 3:bd16e43ad7be 181 send_all_data();
gerardo_carmona 3:bd16e43ad7be 182 break;
gerardo_carmona 3:bd16e43ad7be 183 default:
gerardo_carmona 5:b384cf06de76 184 //motor_stop();
gerardo_carmona 3:bd16e43ad7be 185 // bt.printf("%f\r\n", get_mag_angle());
gerardo_carmona 3:bd16e43ad7be 186 break;
gerardo_carmona 3:bd16e43ad7be 187 }
gerardo_carmona 3:bd16e43ad7be 188 }
gerardo_carmona 3:bd16e43ad7be 189
gerardo_carmona 3:bd16e43ad7be 190 void send_all_data(){
gerardo_carmona 5:b384cf06de76 191 pc.printf("sending all data...\r\n");
gerardo_carmona 5:b384cf06de76 192 pc.printf("Right encoder: %f cms\tLeft encoder: %f cms\r\n", encoder(encoder_rigth), encoder(encoder_rigth));
gerardo_carmona 5:b384cf06de76 193 pc.printf("Rigt distance: %fcms\tLeft distance: %fcms\r\n", ultrasonicos(ULTRA_R), ultrasonicos(ULTRA_L));
gerardo_carmona 5:b384cf06de76 194 pc.printf("Angle: %f\r\n", get_mag_angle());
gerardo_carmona 4:c60636c95b80 195 }
gerardo_carmona 4:c60636c95b80 196
gerardo_carmona 4:c60636c95b80 197 void gps_data(){
gerardo_carmona 4:c60636c95b80 198 leds = LED_BUSY;
gerardo_carmona 4:c60636c95b80 199
gerardo_carmona 4:c60636c95b80 200 if (gps.readable()){
gerardo_carmona 4:c60636c95b80 201
gerardo_carmona 4:c60636c95b80 202 char lat[10];
gerardo_carmona 4:c60636c95b80 203 char lon[10];
gerardo_carmona 4:c60636c95b80 204 char c;
gerardo_carmona 4:c60636c95b80 205 char str[200];
gerardo_carmona 4:c60636c95b80 206 c = gps.getc();
gerardo_carmona 4:c60636c95b80 207 if (c == '$') {
gerardo_carmona 4:c60636c95b80 208 gps.scanf ("%199s",str); // Get all the data
gerardo_carmona 4:c60636c95b80 209 //pc.printf("%s \n",str);
gerardo_carmona 4:c60636c95b80 210 // Checking if its the nmea string that we need
gerardo_carmona 4:c60636c95b80 211 if (str[2] == 'G' && str[3] == 'G' && str[4] == 'A'){
gerardo_carmona 4:c60636c95b80 212 int i = 0;
gerardo_carmona 4:c60636c95b80 213 int j = 0;
gerardo_carmona 4:c60636c95b80 214 c = ' ';
gerardo_carmona 4:c60636c95b80 215 int array_size = 0;
gerardo_carmona 4:c60636c95b80 216 array_size = sizeof(str);
gerardo_carmona 4:c60636c95b80 217 // Searching for the second comma
gerardo_carmona 4:c60636c95b80 218 for (i = 6; i < array_size; i++){
gerardo_carmona 4:c60636c95b80 219 c = str[i];
gerardo_carmona 4:c60636c95b80 220 if (c == ',') break;
gerardo_carmona 4:c60636c95b80 221 }
gerardo_carmona 4:c60636c95b80 222 // Latitud
gerardo_carmona 4:c60636c95b80 223 c = ' ';
gerardo_carmona 4:c60636c95b80 224 i++;
gerardo_carmona 4:c60636c95b80 225 j = 0;
gerardo_carmona 4:c60636c95b80 226 while ( c != ','){
gerardo_carmona 4:c60636c95b80 227 c = str[i+j];
gerardo_carmona 4:c60636c95b80 228 if (c != ',')
gerardo_carmona 4:c60636c95b80 229 lat[j] = c;
gerardo_carmona 4:c60636c95b80 230 j++;
gerardo_carmona 4:c60636c95b80 231 }
gerardo_carmona 4:c60636c95b80 232 // N
gerardo_carmona 4:c60636c95b80 233 for (i = j; i < array_size; i++){
gerardo_carmona 4:c60636c95b80 234 c = str[i];
gerardo_carmona 4:c60636c95b80 235 if (c == 'N') break;
gerardo_carmona 4:c60636c95b80 236 }
gerardo_carmona 4:c60636c95b80 237
gerardo_carmona 4:c60636c95b80 238 // Obtenemos el valor de longitud
gerardo_carmona 4:c60636c95b80 239 c = ' ';
gerardo_carmona 4:c60636c95b80 240 i = i + 2;
gerardo_carmona 4:c60636c95b80 241 j = 0;
gerardo_carmona 4:c60636c95b80 242 while ( c != ','){
gerardo_carmona 4:c60636c95b80 243 c = str[i+j];
gerardo_carmona 4:c60636c95b80 244 if (c != ',')
gerardo_carmona 4:c60636c95b80 245 lon[j] = c;
gerardo_carmona 4:c60636c95b80 246 j++;
gerardo_carmona 4:c60636c95b80 247 }
gerardo_carmona 4:c60636c95b80 248 //pc.printf("sLatitud %s \n",lat);
gerardo_carmona 4:c60636c95b80 249 //pc.printf("sLongitud %s \n",lon);
gerardo_carmona 4:c60636c95b80 250
gerardo_carmona 4:c60636c95b80 251 lat_val = atof(lat);
gerardo_carmona 4:c60636c95b80 252 lon_val = atof(lon);
gerardo_carmona 4:c60636c95b80 253
gerardo_carmona 4:c60636c95b80 254 pc.printf("Latitud %f \n",lat_val);
gerardo_carmona 4:c60636c95b80 255 pc.printf("Longitud %f \n",lon_val);
gerardo_carmona 4:c60636c95b80 256 wait(0.001);
gerardo_carmona 4:c60636c95b80 257 }
gerardo_carmona 4:c60636c95b80 258
gerardo_carmona 4:c60636c95b80 259 }
gerardo_carmona 4:c60636c95b80 260 }else{
gerardo_carmona 4:c60636c95b80 261 //pc.printf("No gps data available \n");
gerardo_carmona 4:c60636c95b80 262 //leds = LED_ERROR; wait(0.1);
gerardo_carmona 4:c60636c95b80 263 }
gerardo_carmona 4:c60636c95b80 264 leds = LED_NORMAL;
gerardo_carmona 0:3a322aad8c88 265 }