Diff: main.cpp

Revision:

2:5f9b78950a17

Parent:

1:5b9fa1823663

Child:

4:453d534b1c1d

--- a/main.cpp	Tue Oct 20 03:07:42 2015 +0000
+++ b/main.cpp	Tue Nov 10 07:29:32 2015 +0000
@@ -1,21 +1,42 @@
 #include "mbed.h"
 #include "QEI.h"
 
-PwmOut motor1_reverse(PB_4);
+PwmOut motor1_reverse(PA_6);
 PwmOut motor1_forward(PB_10);
 PwmOut motor2_forward(PA_7);
 PwmOut motor2_reverse(PB_6);
 
-//
-DigitalIn encoder1A(PC_15);
-DigitalIn encoder1B(PB_3);
-DigitalIn enconder2A(PA_1);
-DigitalIn enconder2B(PA_2); // we need to check things now.
+//DigitalIn encoder1A(PA_15);
+//DigitalIn encoder1B(PB_3);
+//DigitalIn encoder2A(PA_1);
+//DigitalIn encoder2B(PA_10); 
+
+
+
+//Gyro
+AnalogIn _gyro(PA_0);
+AnalogIn gyro_cal(PC_1); //no
 
+//Left Front IR
+DigitalOut eLF(PC_3);
+AnalogIn rLF(PC_0);
+                                                    //PC_4 is an ADC
+
+//Left Side IR
+DigitalOut eLS(PC_2);
+AnalogIn rLS(PC_1);
+
+//Right Front IR
+DigitalOut eRF(PC_12);
+AnalogIn rRF(PA_4);
+
+//Right Side IR
+DigitalOut eRS(PC_15);
+AnalogIn rRS(PB_0);
 
 DigitalOut myled(LED1);
 
-QEI LENC (PC_15, PB_3, NC, 100 );
+QEI LENC (PA_15, PB_3, NC, 100 );
 QEI RENC (PA_1, PA_10, NC, 100 );
 
 int distance = 0;
@@ -23,42 +44,63 @@
 void rightForward(float speed);
 void leftForward(float speed);
 
-Serial pc (USBTX, USBRX);
+float read_gyro( void );
+
+Serial pc (SERIAL_TX, SERIAL_RX);
 
 int main() {
-    LENC.reset();
-    RENC.reset();
-    
-  pc. printf ( "Hello World!!!!!!\r\n" );  
-    
-    myled = 0;
+   // LENC.reset();
+   // RENC.reset();
+   eRS = 0;
+   eRF = 0;
+   eLS = 0;
+   eLF = 0;
+        
+    myled = 1;
     while(1) {
-//        motor1_reverse = 0.5;
-//        motor1_forward = 0;
-//    
-//        motor2_forward = 0.5;
-//        motor2_reverse = 0;
-
+        
         motor1_reverse = 0;
         motor1_forward = 0;
     
         motor2_forward = 0;
         motor2_reverse = 0;
         
-        int in = encoder1A.read();        
+        eRS = 0;
+        eRF = 0;
+        eLS = 0;
+        eLF = 0;
+        
+        pc.printf("LEDS off\r\n");
+        wait(1);
+        pc.printf("Left Front: \t%f\r\n", rLF.read()); 
+        pc.printf("Left Side: \t%f\r\n", rLS.read()); 
+        pc.printf("Right Front: \t%f\r\n", rRS.read()); 
+        pc.printf("Right Side: \t%f\r\n\n", rRF.read()); 
+        wait(.5);    
+        
+        eRS = 1;
+        eRF = 1;
+        eLS = 1;
+        eLF = 1;
+        
+        pc.printf("LEDS on\r\n");
+        wait(1);
+        pc.printf("Left Front: \t%f\r\n", rLF.read()); 
+        pc.printf("Left Side: \t%f\r\n", rLS.read()); 
+        pc.printf("Right Front: \t%f\r\n", rRS.read()); 
+        pc.printf("Right Side: \t%f\r\n\n", rRF.read());     
+      //  pc.printf("%f\r\n", _gyro.read());  
+//         wait(0.5);
+    }
 
-        while ( in == 1 )
-        {
-            myled = 1;
-            wait(4);
-            in = encoder1A.read();
-        }
-        while ( in == 0 )
-        {
-            myled = 0;
-            in = encoder1A.read();
-        }
-    }
+    
+     //   int in = encoder1A.read();
+    //    if(in==1)
+   // myled=1;
+   // else
+   // myled=0;
+
+    //}
     //
 //    motor1_reverse = 1.0;
 //    motor1_forward = 1.0;
@@ -68,7 +110,17 @@
     //rightForward(0.5);
     //leftForward(-0.5);
 }
-
+ 
+ 
+ float read_gyro ( void ){
+    // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset 
+    float r_gyro = _gyro.read();  // Get the Analog value from the STM32
+    r_gyro *= gyro_cal;     // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
+    r_gyro = 1.5 - r_gyro;  // 1.5V off-set, invert sign
+    r_gyro *= 1492.5;       // Convert to DPS (+ve = clockwise, -ve = c.clockwise) 
+    return r_gyro;
+    }
+    
 void moveForward(float speed)
 {
     motor1_forward = speed;