Diff: PID_control.cpp

Revision:

10:707e542688dc

Parent:

9:20b8b64ed259

Child:

11:e647b14ff4ef

--- a/PID_control.cpp	Fri Dec 01 01:19:35 2017 +0000
+++ b/PID_control.cpp	Fri Dec 01 02:00:02 2017 +0000
@@ -31,6 +31,22 @@
 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
 Timer timer;
 int counter = 0;
+float threshold = 0.00085;
+float turnThreshold = 0.00085;
+    
+
+
+//IRR = IR Reciver
+AnalogIn RS_IRR(PA_0); //Right Side 
+AnalogIn RF_IRR(PA_4); //Right Front
+AnalogIn LF_IRR(PC_1); //Left Front
+AnalogIn LS_IRR(PC_0); //Left Side
+
+//IRE = IR Emitter
+DigitalOut RS_IRE(PC_10); //Right Side
+DigitalOut RF_IRE(PC_11); //Right Front
+DigitalOut LF_IRE(PB_0); //Left Front
+DigitalOut LS_IRE(PB_7); //Left Side 
 
 double P_controller(int error)
 {
@@ -71,8 +87,8 @@
     C_speed = P_controller(Error) + I_controller(Error) + ex;
     if (C_speed < 0)
         C_speed = C_speed*-1;
-    Front_IRError = RF_IRR.read() + LF_IRR.read();  //rev1
-    Side_IRError = RS_IRR.read() - LS_IRR.read();   //rev1,define side_error as positive if closer to right
+    float Front_IRError = RF_IRR.read() + LF_IRR.read();  //rev1
+    float Side_IRError = RS_IRR.read() - LS_IRR.read();   //rev1,define side_error as positive if closer to right
 }
 
 
@@ -195,17 +211,8 @@
     }
 }
 
-AnalogIn RS_IRR(PA_0);
-AnalogIn RF_IRR(PA_4); //Right Front
-AnalogIn LF_IRR(PC_1); //Left Front
-AnalogIn LS_IRR(PC_0); //Left Side
- 
-DigitalOut RS_IRE(PC_10);
-DigitalOut RF_IRE(PC_11); //Right Front
-DigitalOut LF_IRE(PB_0); //Left Front
-DigitalOut LS_IRE(PB_7); //Left Side 
 
-void main1() {
+/*void main1() {
     printf("\nAnalogIn example\n");
     LF_IRE.write(1);
     RF_IRE.write(1);  
@@ -244,7 +251,7 @@
     }
     stop();
 } 
-
+*/
 void testStop() //rev1
 {
     //printf("\nAnalogIn example\n");
@@ -252,9 +259,17 @@
     RF_IRE.write(1);
     forward();  
     while(1) {
-        if (RF_IRR.read() < threshold && LF_IRR.read() < threshold)
+        if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
+        {
             stop();
         }
+        float value1 = LF_IRR.read();
+        float value2 = RF_IRR.read(); 
+        printf("LF Led: %f\n", value1);
+        wait(0.25);
+        printf("RF Led: %f\n\r", value2);
+    }
+            
 }
 
 void testTurnAround() //rev1
@@ -266,8 +281,6 @@
 int main()
 {
     systicker.attach_us(&systick, 1000);    //rev1
-    float threshold = 0.001;
-    float turnThreshold = 0.001;
     
     testStop();
 }