Diff: PID_control.cpp

Revision:

9:20b8b64ed259

Parent:

8:6b2f7886768d

Child:

10:707e542688dc

--- a/PID_control.cpp	Wed Nov 29 02:51:31 2017 +0000
+++ b/PID_control.cpp	Fri Dec 01 01:19:35 2017 +0000
@@ -1,3 +1,13 @@
+//rev1 
+//added routines to main for easier testing of movement functions
+//wrote testStop();
+//TODO: test quarterTurnLeft(); , quarterTurnRight(); for delay constants 
+//TODO: finetune PID , Multiplexing for IR LEDS , 
+//SUGGESTIONS: implement turn functions using encoders. i.e. each pulse represents 1 degree, need difference of 90 pulses for right turn
+
+
+//Kevin Lee Nov 30, 2017
+
 
 #include "mbed.h"
 #include "QEI.h"
@@ -16,6 +26,7 @@
 int integrator = 0;
 int decayFactor = 1;
 double Error = 0;
+double IRError = 0; //added
 double prevError = 0;
 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
 Timer timer;
@@ -57,11 +68,11 @@
     double L_en_count = encoder_Left.getPulses()/100;
     Error = R_en_count - L_en_count;
     double ex = D_controller(Error);
-    // if (ex < 0)
-    //    ex = -ex;
     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
 }
 
 
@@ -124,13 +135,30 @@
     wait(.2);
 }
 
+void quarterTurnRight() //rev1, for ladders
+{
+    m_Left_B.write(0);
+    m_Right_F.write(0);
+    m_Left_F.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.1); //delay needs testing
+}
+
 void turnLeft()
 {
     m_Left_F.write(0);
     m_Right_B.write(0);
     m_Right_F.write(i_speed);
     m_Left_B.write(i_speed);
-    wait(.2);
+    wait(.2); //this is dependent on i_speed, can we write a function that varies with i_speed?
+}
+
+void quarterTurnLeft() { //rev1, for ladders
+    m_Left_F.write(0);
+    m_Right_B.write(0);
+    m_Right_F.write(i_speed);
+    m_Left_B.write(i_speed);
+    wait(.1); //time needs testing
 }
 
 void turnAround()
@@ -177,46 +205,75 @@
 DigitalOut LF_IRE(PB_0); //Left Front
 DigitalOut LS_IRE(PB_7); //Left Side 
 
- 
-
-int main()
-{
-    float threshold = 0.001;
-    float turnThreshold = 0.001;
+void main1() {
     printf("\nAnalogIn example\n");
     LF_IRE.write(1);
     RF_IRE.write(1);  
     while (1){ 
-    while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
-        forward();
-        float value1 = LF_IRR.read();
-        float value2 = RF_IRR.read(); 
-        printf("LF Led: %f\n", value1);
-        wait(0.5);
-        printf("RF Led: %f\n", value2);
-    }
-    
+        while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
+            forward();
+            float value1 = LF_IRR.read();
+            float value2 = RF_IRR.read(); 
+            printf("LF Led: %f\n", value1);
+            wait(0.5);
+            printf("RF Led: %f\n", value2);
+            }
+            
     backUp();
-    
     LS_IRE.write(1);
     RS_IRE.write(1);  
-    if (LS_IRR.read() > turnThreshold)
+    
+    if (LS_IRR.read() > turnThreshold) {
         if (RS_IRR.read() < turnThreshold)
             turnRight();
         else
-            turnAround();
-    else if (RS_IRR.read() > turnThreshold)
+            turnAround(); 
+        }
+        
+    else if (RS_IRR.read() > turnThreshold) {
         if (LS_IRR.read() < turnThreshold)
             turnRight();
         else
             turnAround();
+            }
     else
         turnAround();
+        
     LS_IRE.write(0);
     RS_IRE.write(0); 
-}
+    }
     stop();
+} 
+
+void testStop() //rev1
+{
+    //printf("\nAnalogIn example\n");
+    LF_IRE.write(1);
+    RF_IRE.write(1);
+    forward();  
+    while(1) {
+        if (RF_IRR.read() < threshold && LF_IRR.read() < threshold)
+            stop();
+        }
 }
+
+void testTurnAround() //rev1
+{
+    
+}
+
+
+int main()
+{
+    systicker.attach_us(&systick, 1000);    //rev1
+    float threshold = 0.001;
+    float turnThreshold = 0.001;
+    
+    testStop();
+}
+
+
+
     /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
         
         /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)