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@3:bd16e43ad7be, 2014-10-17 (annotated)

Committer:
gerardo_carmona
Date:
Fri Oct 17 06:21:52 2014 +0000
Revision:
3:bd16e43ad7be
Parent:
2:94059cb643be
Child:
4:c60636c95b80
v1.3

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 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 0:3a322aad8c88 91
gerardo_carmona 0:3a322aad8c88 92 // Sensors
gerardo_carmona 0:3a322aad8c88 93 AnalogIn ultra_left(A2);
gerardo_carmona 0:3a322aad8c88 94 AnalogIn ultra_right(A3);
gerardo_carmona 3:bd16e43ad7be 95 //DigitalIn encoder_left(D3);
gerardo_carmona 3:bd16e43ad7be 96 //DigitalIn encoder_right(D4);
gerardo_carmona 0:3a322aad8c88 97
gerardo_carmona 0:3a322aad8c88 98 // ----- Others ---------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 99 Timer bt_timer;
gerardo_carmona 0:3a322aad8c88 100 Timer gps_timer;
gerardo_carmona 0:3a322aad8c88 101
gerardo_carmona 0:3a322aad8c88 102 // ----- Variables ------------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 103 char bt_data;
gerardo_carmona 0:3a322aad8c88 104
gerardo_carmona 0:3a322aad8c88 105 // ----- Function prototypes --------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 106 void read_bt();
gerardo_carmona 3:bd16e43ad7be 107 void command_bt(char _command);
gerardo_carmona 3:bd16e43ad7be 108 void send_all_data();
gerardo_carmona 0:3a322aad8c88 109
gerardo_carmona 0:3a322aad8c88 110 // ----- Main program ---------------------------------------------------------------
gerardo_carmona 0:3a322aad8c88 111 int main(){
gerardo_carmona 3:bd16e43ad7be 112 init_encoders();
gerardo_carmona 0:3a322aad8c88 113 bt.baud(9600);
gerardo_carmona 0:3a322aad8c88 114 pc.baud(9600);
gerardo_carmona 0:3a322aad8c88 115 leds = LED_NORMAL;
gerardo_carmona 0:3a322aad8c88 116 while (true){
gerardo_carmona 3:bd16e43ad7be 117 read_bt();
gerardo_carmona 0:3a322aad8c88 118 }
gerardo_carmona 0:3a322aad8c88 119 }
gerardo_carmona 0:3a322aad8c88 120
gerardo_carmona 0:3a322aad8c88 121 // ----- Functions ------------------------------------------------------------------
gerardo_carmona 3:bd16e43ad7be 122 void read_bt(){
gerardo_carmona 0:3a322aad8c88 123 char c = ' ';
gerardo_carmona 0:3a322aad8c88 124 if (bt.readable()){
gerardo_carmona 0:3a322aad8c88 125 c = bt.getc();
gerardo_carmona 0:3a322aad8c88 126 bt.printf("%c\n\r", c);
gerardo_carmona 3:bd16e43ad7be 127 //}
gerardo_carmona 3:bd16e43ad7be 128 //if (c != ' '){
gerardo_carmona 3:bd16e43ad7be 129 command_bt(c);
gerardo_carmona 0:3a322aad8c88 130 }
gerardo_carmona 3:bd16e43ad7be 131 }
gerardo_carmona 3:bd16e43ad7be 132
gerardo_carmona 3:bd16e43ad7be 133 void command_bt(char _command){
gerardo_carmona 3:bd16e43ad7be 134 switch (_command){
gerardo_carmona 3:bd16e43ad7be 135 case MOVE_FWD_F:
gerardo_carmona 3:bd16e43ad7be 136 motor_fwd(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 137 break;
gerardo_carmona 3:bd16e43ad7be 138 case MOVE_REV_F:
gerardo_carmona 3:bd16e43ad7be 139 motor_rev(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 140 break;
gerardo_carmona 3:bd16e43ad7be 141 case MOVE_LEF_F:
gerardo_carmona 3:bd16e43ad7be 142 motor_left(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 143 break;
gerardo_carmona 3:bd16e43ad7be 144 case MOVE_RIG_F:
gerardo_carmona 3:bd16e43ad7be 145 motor_right(FAST, FAST);
gerardo_carmona 3:bd16e43ad7be 146 break;
gerardo_carmona 3:bd16e43ad7be 147 case MOVE_FWD_S:
gerardo_carmona 3:bd16e43ad7be 148 motor_fwd(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 149 break;
gerardo_carmona 3:bd16e43ad7be 150 case MOVE_REV_S:
gerardo_carmona 3:bd16e43ad7be 151 motor_rev(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 152 break;
gerardo_carmona 3:bd16e43ad7be 153 case MOVE_LEF_S:
gerardo_carmona 3:bd16e43ad7be 154 motor_left(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 155 break;
gerardo_carmona 3:bd16e43ad7be 156 case MOVE_RIG_S:
gerardo_carmona 3:bd16e43ad7be 157 motor_right(SLOW, SLOW);
gerardo_carmona 3:bd16e43ad7be 158 break;
gerardo_carmona 3:bd16e43ad7be 159 case MOVE_STO:
gerardo_carmona 3:bd16e43ad7be 160 motor_stop();
gerardo_carmona 3:bd16e43ad7be 161 break;
gerardo_carmona 3:bd16e43ad7be 162 //case MOVE_90L:
gerardo_carmona 3:bd16e43ad7be 163 // move_90(1);
gerardo_carmona 3:bd16e43ad7be 164 // break;
gerardo_carmona 3:bd16e43ad7be 165 //case MOVE_90R:
gerardo_carmona 3:bd16e43ad7be 166 // move_90(2);
gerardo_carmona 3:bd16e43ad7be 167 // break;
gerardo_carmona 3:bd16e43ad7be 168 case reset_encoders:
gerardo_carmona 3:bd16e43ad7be 169 encoders_to_zero();
gerardo_carmona 3:bd16e43ad7be 170 break;
gerardo_carmona 3:bd16e43ad7be 171 case request_data:
gerardo_carmona 3:bd16e43ad7be 172 send_all_data();
gerardo_carmona 3:bd16e43ad7be 173 break;
gerardo_carmona 3:bd16e43ad7be 174 default:
gerardo_carmona 3:bd16e43ad7be 175 motor_stop();
gerardo_carmona 3:bd16e43ad7be 176 // bt.printf("%f\r\n", get_mag_angle());
gerardo_carmona 3:bd16e43ad7be 177 break;
gerardo_carmona 3:bd16e43ad7be 178 }
gerardo_carmona 3:bd16e43ad7be 179 }
gerardo_carmona 3:bd16e43ad7be 180
gerardo_carmona 3:bd16e43ad7be 181 void send_all_data(){
gerardo_carmona 3:bd16e43ad7be 182 bt.printf("sending all data...\n");
gerardo_carmona 3:bd16e43ad7be 183 bt.printf("Right encoder: %f cms\tLeft encoder: %f cms\n", encoder(encoder_rigth), encoder(encoder_rigth));
gerardo_carmona 3:bd16e43ad7be 184 bt.printf("Rigt distance: %fcms\tLeft distance: %fcms\n", ultrasonicos(ULTRA_R), ultrasonicos(ULTRA_L));
gerardo_carmona 3:bd16e43ad7be 185 bt.printf("Angle: %f\n", get_mag_angle());
gerardo_carmona 0:3a322aad8c88 186 }