Diff: main.cpp

Revision:

0:ab931824163f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 08 23:55:33 2014 +0000
@@ -0,0 +1,254 @@
+/* ==================================================================================
+    --- TEST 1 ---
+    OBJETIVE:
+    Move the motor via bluetooth and program 90° turns to the right and left using 
+    the magnometer.
+    
+    COMPONENTS:
+    * Bluetooth
+    * Magnometer
+    * Motors
+    
+    CONTROLS:
+    w: foward
+    s: reverse
+    a: left turn
+    d: right turn
+    o: 90° left turn
+    p: 90° right turn
+
+================================================================================== */
+
+// ----- Libraries ------------------------------------------------------------------
+#include "mbed.h"
+#include "FXOS8700CQ.h"
+
+#define MOVE_FWD 'w'
+#define MOVE_REV 's'
+#define MOVE_LEF 'a'
+#define MOVE_RIG 'd'
+#define MOVE_STO 'q'
+#define MOVE_90L 'o'
+#define MOVE_90R 'p'
+
+// ----- Objects --------------------------------------------------------------------
+Serial pc(USBTX, USBRX); // Primary output to demonstrate library
+Serial bt(PTC15, PTC14);
+// Pin names for FRDM-K64F
+FXOS8700CQ fxos(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // SDA, SCL, (addr << 1)
+InterruptIn fxos_int2(PTC13); // should just be the Data-Ready interrupt
+DigitalOut led_on(LED2);
+DigitalOut tled(LED3);
+// MOTORS
+DigitalOut in1_A(D2);
+DigitalOut in2_A(D4);
+PwmOut en_A(D3);
+DigitalOut in1_B(D6);
+DigitalOut in2_B(D7);
+PwmOut en_B(D5);
+
+// ----- Others ---------------------------------------------------------------------
+SRAWDATA accel_data;
+SRAWDATA magn_data;
+
+// ----- Variables ------------------------------------------------------------------
+double mag_x, mag_y;
+//double mag_angle, mag_old_angle;
+
+// ----- Function prototypes --------------------------------------------------------
+void prints();
+void trigger_fxos_int2();
+double get_mag_x();
+double get_mag_y();
+double get_mag_angle();
+void move_motors(char _move_command, int _power);
+void motor_fwd(int _power);
+void motor_rev(int _power);
+void move_90(int direction);
+void motor_left(int _power);
+void motor_right(int _power);
+void motor_stop();
+
+
+// ----- Main program ---------------------------------------------------------------
+int main(){
+    bt.baud(9600); 
+    fxos_int2.fall(&trigger_fxos_int2);
+    fxos.enable();
+    prints();
+    
+    while (true){
+        
+        //mag_angle = get_mag_angle();
+        
+        if (bt.readable()){
+            char c = bt.getc();
+            bt.printf("%c\n\r", c);
+            move_motors(c, 100);
+        }
+        
+        
+        //pc.printf("Angle: %f\r\n", mag_angle);
+        //wait(1);
+    }  
+} 
+
+// ----- Functions ------------------------------------------------------------------
+void prints(){
+    bt.printf("\r\n\nFXOS8700Q Who Am I= %X\r\n", fxos.get_whoami());
+    bt.printf("\r\nAccelerometer scale %X\r\n", fxos.get_accel_scale());
+    bt.printf("\r\nInit mag angle %3.2f\r\n", get_mag_angle());
+    bt.printf("----------------------------------\r\n\r\n");
+    wait(1);
+}
+
+void trigger_fxos_int2(){
+    //fxos_int2_triggered = true;
+    //us_ellapsed = t.read_us();
+}
+
+double get_mag_x(){
+    fxos.get_data(&accel_data, &magn_data);
+    return magn_data.x;
+}
+
+double get_mag_y(){
+    fxos.get_data(&accel_data, &magn_data);
+    return magn_data.y;
+}
+
+double get_mag_angle(){
+    double _mag_angle;
+    
+    mag_x = get_mag_x();
+    mag_y = get_mag_y();
+    
+    _mag_angle = atan2(mag_x, mag_y)*180/3.14159;
+    
+    if (_mag_angle < 0){
+        _mag_angle = 360 + _mag_angle;
+    }
+    
+    return _mag_angle;
+}
+
+void move_motors(char _move_command, int _power){
+    switch (_move_command){
+        case MOVE_FWD:
+            motor_fwd(_power);
+            break;
+        case MOVE_REV:
+            motor_rev(_power);
+            break;
+        case MOVE_LEF:
+            motor_left(_power);
+            break;
+        case MOVE_RIG:
+            motor_right(_power);
+            break;
+        case MOVE_STO:
+            motor_stop();
+            break;
+        case MOVE_90L:
+            move_90(1);
+            break;
+        case MOVE_90R:
+            move_90(2);
+            break;
+        default:
+            bt.printf("%f\r\n", get_mag_angle());
+    }
+}
+
+void move_90(int direction){
+    double old_angle = get_mag_angle();
+    double tarjet = 0;
+    
+    bt.printf("giro de 90");
+    
+    if (direction == 1){
+        tarjet = old_angle - 90;
+        if (tarjet < 90){
+            tarjet = 360 + tarjet;
+        }
+    }else if (direction == 2){
+        tarjet = old_angle + 90;
+        if (tarjet >= 360){
+            tarjet = tarjet - 360;
+        }
+    }
+    
+    bt.printf("Tarjet: %f\r\n", tarjet);
+    
+    int motor_speed = 75; // Velocidad inicial
+    double actual_angle;
+    while (true){
+        actual_angle = get_mag_angle();
+        bt.printf("actual angle: %f \r\n", actual_angle);
+        if (direction == 1){
+            // Realizar un PI o P para calcular la velocidad
+            motor_left(motor_speed);
+            if (actual_angle > tarjet - 0.05 &&  actual_angle < tarjet + 0.05){
+                // En mente tomar 50 muestras lo mas rapido posible
+                // y cuando salgan que en promedio las lecturas tienen
+                // un error menor o igual a +/- 0.5% entonces salirse
+                break;
+            }
+        }else{
+            motor_right(motor_speed);
+            if (actual_angle > tarjet - 0.05 &&  actual_angle < tarjet + 0.05){
+                break;
+            }
+        }
+    }       
+}
+
+void motor_fwd(int _power){
+    in1_A = 1;
+    in2_A = 0;
+    en_A = (float)_power/100;
+    in1_B = 1;
+    in2_B = 0;
+    en_B = (float)_power/100;
+    pc.printf("FWD\n");
+}
+
+void motor_rev(int _power){
+    in1_A = 0;
+    in2_A = 1;
+    en_A = (float)100/_power;
+    in1_B = 0;
+    in2_B = 1;
+    en_B = (float)_power/100;
+    pc.printf("REV\n");
+}
+
+void motor_left(int _power){
+    in1_A = 1;
+    in2_A = 0;
+    en_A = (float)_power/100;
+    in1_B = 0;
+    in2_B = 1;
+    en_B = (float)_power/100;
+    pc.printf("LEFT\n");
+}
+
+void motor_right(int _power){
+    in1_A = 0;
+    in2_A = 1;
+    en_A = (float)_power/100;
+    in1_B = 1;
+    in2_B = 0;
+    en_B = (float)_power/100;
+    pc.printf("RIGHT\n");
+}
+
+void motor_stop(){
+    in1_A = 0;
+    in2_A = 0;
+    en_A = 0;
+    in1_B = 0;
+    in2_B = 0;
+    en_B = 0;
+    pc.printf("STOP\n");
+}
\ No newline at end of file