Diff: main.cpp

Revision:

9:aa2ce38dec6b

Parent:

8:7c5e6b1e7aa5

Child:

10:1bd0224093e4

--- a/main.cpp	Mon Nov 14 13:42:22 2016 +0000
+++ b/main.cpp	Wed Nov 16 16:05:13 2016 +0000
@@ -12,6 +12,7 @@
 #include "mbed.h"
 #include "TFC.h"
 #include "XBEE.h"
+#include "angular_speed.h"
 
 #define BAUD_RATE 57600
 #define CAM_THRESHOLD 109
@@ -46,9 +47,20 @@
 float integral;
 float measured_value, desired_value, derivative;
 float output;
-
 Timer t;
 
+//Speed Sensors variables
+InterruptIn leftHallSensor(D0);
+InterruptIn rightHallSensor(D2);
+Timer t1;
+Timer t2;
+volatile float Time_L,Time_R;
+float wL, wR;
+void GetTime_L();
+void GetTime_R();
+inline void initSpeedSensors();
+
+// Current comms command
 char curr_cmd = 0;
 
 float speed = 0.3;
@@ -79,6 +91,7 @@
     
     //Initialise/reset PID variables
     initVariables();
+    initSpeedSensors();
     
     while(1) {
         
@@ -153,6 +166,10 @@
         for(i = 0; i < 128; i++) {
             pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
         }
+        
+        wL=Get_Speed(Time_L);
+        wR=Get_Speed(Time_R);
+        sendString("wL = %f, wR = %f",wL,wR);
     }
     frame_counter++;
 }
@@ -293,4 +310,25 @@
     t.stop();
     t.reset();
     t.start();
+}
+
+
+
+inline void initSpeedSensors() {
+    t1.start();
+    t2.start();
+    
+    //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
+    leftHallSensor.rise(&GetTime_L);
+    rightHallSensor.rise(&GetTime_R);  
+}
+
+void GetTime_L(){
+    Time_L=t1.read_us();
+    t1.reset();
+}
+
+void GetTime_R(){
+    Time_R=t2.read_us();
+    t2.reset();
 }
\ No newline at end of file