Gerardo CR / Mbed 2 deprecated AVC_test1

Primer version del robot autonomo.

Dependencies:   FXOS8700CQ mbed

main.cpp@0:ab931824163f, 2014-10-08 (annotated)

Committer:
gerardo_carmona
Date:
Wed Oct 08 23:55:33 2014 +0000
Revision:
0:ab931824163f
Primer version del robot autonomo.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gerardo_carmona 0:ab931824163f 1 /* ==================================================================================
gerardo_carmona 0:ab931824163f 2 --- TEST 1 ---
gerardo_carmona 0:ab931824163f 3 OBJETIVE:
gerardo_carmona 0:ab931824163f 4 Move the motor via bluetooth and program 90° turns to the right and left using
gerardo_carmona 0:ab931824163f 5 the magnometer.
gerardo_carmona 0:ab931824163f 6
gerardo_carmona 0:ab931824163f 7 COMPONENTS:
gerardo_carmona 0:ab931824163f 8 * Bluetooth
gerardo_carmona 0:ab931824163f 9 * Magnometer
gerardo_carmona 0:ab931824163f 10 * Motors
gerardo_carmona 0:ab931824163f 11
gerardo_carmona 0:ab931824163f 12 CONTROLS:
gerardo_carmona 0:ab931824163f 13 w: foward
gerardo_carmona 0:ab931824163f 14 s: reverse
gerardo_carmona 0:ab931824163f 15 a: left turn
gerardo_carmona 0:ab931824163f 16 d: right turn
gerardo_carmona 0:ab931824163f 17 o: 90° left turn
gerardo_carmona 0:ab931824163f 18 p: 90° right turn
gerardo_carmona 0:ab931824163f 19
gerardo_carmona 0:ab931824163f 20 ================================================================================== */
gerardo_carmona 0:ab931824163f 21
gerardo_carmona 0:ab931824163f 22 // ----- Libraries ------------------------------------------------------------------
gerardo_carmona 0:ab931824163f 23 #include "mbed.h"
gerardo_carmona 0:ab931824163f 24 #include "FXOS8700CQ.h"
gerardo_carmona 0:ab931824163f 25
gerardo_carmona 0:ab931824163f 26 #define MOVE_FWD 'w'
gerardo_carmona 0:ab931824163f 27 #define MOVE_REV 's'
gerardo_carmona 0:ab931824163f 28 #define MOVE_LEF 'a'
gerardo_carmona 0:ab931824163f 29 #define MOVE_RIG 'd'
gerardo_carmona 0:ab931824163f 30 #define MOVE_STO 'q'
gerardo_carmona 0:ab931824163f 31 #define MOVE_90L 'o'
gerardo_carmona 0:ab931824163f 32 #define MOVE_90R 'p'
gerardo_carmona 0:ab931824163f 33
gerardo_carmona 0:ab931824163f 34 // ----- Objects --------------------------------------------------------------------
gerardo_carmona 0:ab931824163f 35 Serial pc(USBTX, USBRX); // Primary output to demonstrate library
gerardo_carmona 0:ab931824163f 36 Serial bt(PTC15, PTC14);
gerardo_carmona 0:ab931824163f 37 // Pin names for FRDM-K64F
gerardo_carmona 0:ab931824163f 38 FXOS8700CQ fxos(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // SDA, SCL, (addr << 1)
gerardo_carmona 0:ab931824163f 39 InterruptIn fxos_int2(PTC13); // should just be the Data-Ready interrupt
gerardo_carmona 0:ab931824163f 40 DigitalOut led_on(LED2);
gerardo_carmona 0:ab931824163f 41 DigitalOut tled(LED3);
gerardo_carmona 0:ab931824163f 42 // MOTORS
gerardo_carmona 0:ab931824163f 43 DigitalOut in1_A(D2);
gerardo_carmona 0:ab931824163f 44 DigitalOut in2_A(D4);
gerardo_carmona 0:ab931824163f 45 PwmOut en_A(D3);
gerardo_carmona 0:ab931824163f 46 DigitalOut in1_B(D6);
gerardo_carmona 0:ab931824163f 47 DigitalOut in2_B(D7);
gerardo_carmona 0:ab931824163f 48 PwmOut en_B(D5);
gerardo_carmona 0:ab931824163f 49
gerardo_carmona 0:ab931824163f 50 // ----- Others ---------------------------------------------------------------------
gerardo_carmona 0:ab931824163f 51 SRAWDATA accel_data;
gerardo_carmona 0:ab931824163f 52 SRAWDATA magn_data;
gerardo_carmona 0:ab931824163f 53
gerardo_carmona 0:ab931824163f 54 // ----- Variables ------------------------------------------------------------------
gerardo_carmona 0:ab931824163f 55 double mag_x, mag_y;
gerardo_carmona 0:ab931824163f 56 //double mag_angle, mag_old_angle;
gerardo_carmona 0:ab931824163f 57
gerardo_carmona 0:ab931824163f 58 // ----- Function prototypes --------------------------------------------------------
gerardo_carmona 0:ab931824163f 59 void prints();
gerardo_carmona 0:ab931824163f 60 void trigger_fxos_int2();
gerardo_carmona 0:ab931824163f 61 double get_mag_x();
gerardo_carmona 0:ab931824163f 62 double get_mag_y();
gerardo_carmona 0:ab931824163f 63 double get_mag_angle();
gerardo_carmona 0:ab931824163f 64 void move_motors(char _move_command, int _power);
gerardo_carmona 0:ab931824163f 65 void motor_fwd(int _power);
gerardo_carmona 0:ab931824163f 66 void motor_rev(int _power);
gerardo_carmona 0:ab931824163f 67 void move_90(int direction);
gerardo_carmona 0:ab931824163f 68 void motor_left(int _power);
gerardo_carmona 0:ab931824163f 69 void motor_right(int _power);
gerardo_carmona 0:ab931824163f 70 void motor_stop();
gerardo_carmona 0:ab931824163f 71
gerardo_carmona 0:ab931824163f 72
gerardo_carmona 0:ab931824163f 73 // ----- Main program ---------------------------------------------------------------
gerardo_carmona 0:ab931824163f 74 int main(){
gerardo_carmona 0:ab931824163f 75 bt.baud(9600);
gerardo_carmona 0:ab931824163f 76 fxos_int2.fall(&trigger_fxos_int2);
gerardo_carmona 0:ab931824163f 77 fxos.enable();
gerardo_carmona 0:ab931824163f 78 prints();
gerardo_carmona 0:ab931824163f 79
gerardo_carmona 0:ab931824163f 80 while (true){
gerardo_carmona 0:ab931824163f 81
gerardo_carmona 0:ab931824163f 82 //mag_angle = get_mag_angle();
gerardo_carmona 0:ab931824163f 83
gerardo_carmona 0:ab931824163f 84 if (bt.readable()){
gerardo_carmona 0:ab931824163f 85 char c = bt.getc();
gerardo_carmona 0:ab931824163f 86 bt.printf("%c\n\r", c);
gerardo_carmona 0:ab931824163f 87 move_motors(c, 100);
gerardo_carmona 0:ab931824163f 88 }
gerardo_carmona 0:ab931824163f 89
gerardo_carmona 0:ab931824163f 90
gerardo_carmona 0:ab931824163f 91 //pc.printf("Angle: %f\r\n", mag_angle);
gerardo_carmona 0:ab931824163f 92 //wait(1);
gerardo_carmona 0:ab931824163f 93 }
gerardo_carmona 0:ab931824163f 94 }
gerardo_carmona 0:ab931824163f 95
gerardo_carmona 0:ab931824163f 96 // ----- Functions ------------------------------------------------------------------
gerardo_carmona 0:ab931824163f 97 void prints(){
gerardo_carmona 0:ab931824163f 98 bt.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", fxos.get_whoami());
gerardo_carmona 0:ab931824163f 99 bt.printf("\r\nAccelerometer scale %X\r\n", fxos.get_accel_scale());
gerardo_carmona 0:ab931824163f 100 bt.printf("\r\nInit mag angle %3.2f\r\n", get_mag_angle());
gerardo_carmona 0:ab931824163f 101 bt.printf("----------------------------------\r\n\r\n");
gerardo_carmona 0:ab931824163f 102 wait(1);
gerardo_carmona 0:ab931824163f 103 }
gerardo_carmona 0:ab931824163f 104
gerardo_carmona 0:ab931824163f 105 void trigger_fxos_int2(){
gerardo_carmona 0:ab931824163f 106 //fxos_int2_triggered = true;
gerardo_carmona 0:ab931824163f 107 //us_ellapsed = t.read_us();
gerardo_carmona 0:ab931824163f 108 }
gerardo_carmona 0:ab931824163f 109
gerardo_carmona 0:ab931824163f 110 double get_mag_x(){
gerardo_carmona 0:ab931824163f 111 fxos.get_data(&accel_data, &magn_data);
gerardo_carmona 0:ab931824163f 112 return magn_data.x;
gerardo_carmona 0:ab931824163f 113 }
gerardo_carmona 0:ab931824163f 114
gerardo_carmona 0:ab931824163f 115 double get_mag_y(){
gerardo_carmona 0:ab931824163f 116 fxos.get_data(&accel_data, &magn_data);
gerardo_carmona 0:ab931824163f 117 return magn_data.y;
gerardo_carmona 0:ab931824163f 118 }
gerardo_carmona 0:ab931824163f 119
gerardo_carmona 0:ab931824163f 120 double get_mag_angle(){
gerardo_carmona 0:ab931824163f 121 double _mag_angle;
gerardo_carmona 0:ab931824163f 122
gerardo_carmona 0:ab931824163f 123 mag_x = get_mag_x();
gerardo_carmona 0:ab931824163f 124 mag_y = get_mag_y();
gerardo_carmona 0:ab931824163f 125
gerardo_carmona 0:ab931824163f 126 _mag_angle = atan2(mag_x, mag_y)*180/3.14159;
gerardo_carmona 0:ab931824163f 127
gerardo_carmona 0:ab931824163f 128 if (_mag_angle < 0){
gerardo_carmona 0:ab931824163f 129 _mag_angle = 360 + _mag_angle;
gerardo_carmona 0:ab931824163f 130 }
gerardo_carmona 0:ab931824163f 131
gerardo_carmona 0:ab931824163f 132 return _mag_angle;
gerardo_carmona 0:ab931824163f 133 }
gerardo_carmona 0:ab931824163f 134
gerardo_carmona 0:ab931824163f 135 void move_motors(char _move_command, int _power){
gerardo_carmona 0:ab931824163f 136 switch (_move_command){
gerardo_carmona 0:ab931824163f 137 case MOVE_FWD:
gerardo_carmona 0:ab931824163f 138 motor_fwd(_power);
gerardo_carmona 0:ab931824163f 139 break;
gerardo_carmona 0:ab931824163f 140 case MOVE_REV:
gerardo_carmona 0:ab931824163f 141 motor_rev(_power);
gerardo_carmona 0:ab931824163f 142 break;
gerardo_carmona 0:ab931824163f 143 case MOVE_LEF:
gerardo_carmona 0:ab931824163f 144 motor_left(_power);
gerardo_carmona 0:ab931824163f 145 break;
gerardo_carmona 0:ab931824163f 146 case MOVE_RIG:
gerardo_carmona 0:ab931824163f 147 motor_right(_power);
gerardo_carmona 0:ab931824163f 148 break;
gerardo_carmona 0:ab931824163f 149 case MOVE_STO:
gerardo_carmona 0:ab931824163f 150 motor_stop();
gerardo_carmona 0:ab931824163f 151 break;
gerardo_carmona 0:ab931824163f 152 case MOVE_90L:
gerardo_carmona 0:ab931824163f 153 move_90(1);
gerardo_carmona 0:ab931824163f 154 break;
gerardo_carmona 0:ab931824163f 155 case MOVE_90R:
gerardo_carmona 0:ab931824163f 156 move_90(2);
gerardo_carmona 0:ab931824163f 157 break;
gerardo_carmona 0:ab931824163f 158 default:
gerardo_carmona 0:ab931824163f 159 bt.printf("%f\r\n", get_mag_angle());
gerardo_carmona 0:ab931824163f 160 }
gerardo_carmona 0:ab931824163f 161 }
gerardo_carmona 0:ab931824163f 162
gerardo_carmona 0:ab931824163f 163 void move_90(int direction){
gerardo_carmona 0:ab931824163f 164 double old_angle = get_mag_angle();
gerardo_carmona 0:ab931824163f 165 double tarjet = 0;
gerardo_carmona 0:ab931824163f 166
gerardo_carmona 0:ab931824163f 167 bt.printf("giro de 90");
gerardo_carmona 0:ab931824163f 168
gerardo_carmona 0:ab931824163f 169 if (direction == 1){
gerardo_carmona 0:ab931824163f 170 tarjet = old_angle - 90;
gerardo_carmona 0:ab931824163f 171 if (tarjet < 90){
gerardo_carmona 0:ab931824163f 172 tarjet = 360 + tarjet;
gerardo_carmona 0:ab931824163f 173 }
gerardo_carmona 0:ab931824163f 174 }else if (direction == 2){
gerardo_carmona 0:ab931824163f 175 tarjet = old_angle + 90;
gerardo_carmona 0:ab931824163f 176 if (tarjet >= 360){
gerardo_carmona 0:ab931824163f 177 tarjet = tarjet - 360;
gerardo_carmona 0:ab931824163f 178 }
gerardo_carmona 0:ab931824163f 179 }
gerardo_carmona 0:ab931824163f 180
gerardo_carmona 0:ab931824163f 181 bt.printf("Tarjet: %f\r\n", tarjet);
gerardo_carmona 0:ab931824163f 182
gerardo_carmona 0:ab931824163f 183 int motor_speed = 75; // Velocidad inicial
gerardo_carmona 0:ab931824163f 184 double actual_angle;
gerardo_carmona 0:ab931824163f 185 while (true){
gerardo_carmona 0:ab931824163f 186 actual_angle = get_mag_angle();
gerardo_carmona 0:ab931824163f 187 bt.printf("actual angle: %f \r\n", actual_angle);
gerardo_carmona 0:ab931824163f 188 if (direction == 1){
gerardo_carmona 0:ab931824163f 189 // Realizar un PI o P para calcular la velocidad
gerardo_carmona 0:ab931824163f 190 motor_left(motor_speed);
gerardo_carmona 0:ab931824163f 191 if (actual_angle > tarjet - 0.05 && actual_angle < tarjet + 0.05){
gerardo_carmona 0:ab931824163f 192 // En mente tomar 50 muestras lo mas rapido posible
gerardo_carmona 0:ab931824163f 193 // y cuando salgan que en promedio las lecturas tienen
gerardo_carmona 0:ab931824163f 194 // un error menor o igual a +/- 0.5% entonces salirse
gerardo_carmona 0:ab931824163f 195 break;
gerardo_carmona 0:ab931824163f 196 }
gerardo_carmona 0:ab931824163f 197 }else{
gerardo_carmona 0:ab931824163f 198 motor_right(motor_speed);
gerardo_carmona 0:ab931824163f 199 if (actual_angle > tarjet - 0.05 && actual_angle < tarjet + 0.05){
gerardo_carmona 0:ab931824163f 200 break;
gerardo_carmona 0:ab931824163f 201 }
gerardo_carmona 0:ab931824163f 202 }
gerardo_carmona 0:ab931824163f 203 }
gerardo_carmona 0:ab931824163f 204 }
gerardo_carmona 0:ab931824163f 205
gerardo_carmona 0:ab931824163f 206 void motor_fwd(int _power){
gerardo_carmona 0:ab931824163f 207 in1_A = 1;
gerardo_carmona 0:ab931824163f 208 in2_A = 0;
gerardo_carmona 0:ab931824163f 209 en_A = (float)_power/100;
gerardo_carmona 0:ab931824163f 210 in1_B = 1;
gerardo_carmona 0:ab931824163f 211 in2_B = 0;
gerardo_carmona 0:ab931824163f 212 en_B = (float)_power/100;
gerardo_carmona 0:ab931824163f 213 pc.printf("FWD\n");
gerardo_carmona 0:ab931824163f 214 }
gerardo_carmona 0:ab931824163f 215
gerardo_carmona 0:ab931824163f 216 void motor_rev(int _power){
gerardo_carmona 0:ab931824163f 217 in1_A = 0;
gerardo_carmona 0:ab931824163f 218 in2_A = 1;
gerardo_carmona 0:ab931824163f 219 en_A = (float)100/_power;
gerardo_carmona 0:ab931824163f 220 in1_B = 0;
gerardo_carmona 0:ab931824163f 221 in2_B = 1;
gerardo_carmona 0:ab931824163f 222 en_B = (float)_power/100;
gerardo_carmona 0:ab931824163f 223 pc.printf("REV\n");
gerardo_carmona 0:ab931824163f 224 }
gerardo_carmona 0:ab931824163f 225
gerardo_carmona 0:ab931824163f 226 void motor_left(int _power){
gerardo_carmona 0:ab931824163f 227 in1_A = 1;
gerardo_carmona 0:ab931824163f 228 in2_A = 0;
gerardo_carmona 0:ab931824163f 229 en_A = (float)_power/100;
gerardo_carmona 0:ab931824163f 230 in1_B = 0;
gerardo_carmona 0:ab931824163f 231 in2_B = 1;
gerardo_carmona 0:ab931824163f 232 en_B = (float)_power/100;
gerardo_carmona 0:ab931824163f 233 pc.printf("LEFT\n");
gerardo_carmona 0:ab931824163f 234 }
gerardo_carmona 0:ab931824163f 235
gerardo_carmona 0:ab931824163f 236 void motor_right(int _power){
gerardo_carmona 0:ab931824163f 237 in1_A = 0;
gerardo_carmona 0:ab931824163f 238 in2_A = 1;
gerardo_carmona 0:ab931824163f 239 en_A = (float)_power/100;
gerardo_carmona 0:ab931824163f 240 in1_B = 1;
gerardo_carmona 0:ab931824163f 241 in2_B = 0;
gerardo_carmona 0:ab931824163f 242 en_B = (float)_power/100;
gerardo_carmona 0:ab931824163f 243 pc.printf("RIGHT\n");
gerardo_carmona 0:ab931824163f 244 }
gerardo_carmona 0:ab931824163f 245
gerardo_carmona 0:ab931824163f 246 void motor_stop(){
gerardo_carmona 0:ab931824163f 247 in1_A = 0;
gerardo_carmona 0:ab931824163f 248 in2_A = 0;
gerardo_carmona 0:ab931824163f 249 en_A = 0;
gerardo_carmona 0:ab931824163f 250 in1_B = 0;
gerardo_carmona 0:ab931824163f 251 in2_B = 0;
gerardo_carmona 0:ab931824163f 252 en_B = 0;
gerardo_carmona 0:ab931824163f 253 pc.printf("STOP\n");
gerardo_carmona 0:ab931824163f 254 }