Diff: PID_control.cpp

Revision:

7:e10dc3cb9212

Parent:

6:71829ae2ee07

Child:

8:6b2f7886768d

--- a/PID_control.cpp	Sat Nov 11 03:05:20 2017 +0000
+++ b/PID_control.cpp	Wed Nov 29 02:48:57 2017 +0000
@@ -1,16 +1,17 @@
+
 #include "mbed.h"
 #include "QEI.h"
 
 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
-double Kp = 0.3;//.005;
+double Kp = 0.27;//.005;
 double Ki = 0.001;//0.0000001;
 double Kd = 0.001;
 PwmOut m_Right_F(PB_10);
 PwmOut m_Right_B(PC_7);
 PwmOut m_Left_F(PA_7);
 PwmOut m_Left_B(PB_6);
-double i_speed = 0.3;
+double i_speed = 0.15;
 double C_speed(0);
 int integrator = 0;
 int decayFactor = 1;
@@ -63,6 +64,7 @@
         C_speed = C_speed*-1;
 }
 
+
 void forward()
 {
     double f1_speed = i_speed + C_speed;
@@ -73,7 +75,8 @@
         pc.printf("-");
     if (C_speed > 0)
         pc.printf("+");
-        */
+    */
+
 
     if(f1_speed >= 0.7) {   //upper bound, can not go faster
         f1_speed = 0.7;
@@ -103,19 +106,48 @@
     }
 }
 
+void backUp()
+{
+    m_Left_F.write(0);
+    m_Right_F.write(0);
+    m_Left_B.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.15);
+}
+
 void turnRight()
 {
-
+    m_Left_B.write(0);
+    m_Right_F.write(0);
+    m_Left_F.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.2);
 }
 
 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);
 }
 
 void turnAround()
 {
+    m_Left_B.write(0);
+    m_Right_F.write(0);
+    m_Left_F.write(i_speed);
+    m_Right_B.write(i_speed);
+    wait(.4);
+}
 
+void stop()
+{
+    m_Right_F.write(0);
+    m_Right_B.write(0);
+    m_Left_F.write(0);
+    m_Left_B.write(0);    
 }
 
 void debugEncoder()
@@ -135,7 +167,76 @@
     }
 }
 
-int main()      //only runs once
+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 
+
+ 
+
+int main()
+{
+    float threshold = 0.001;
+    float turnThreshold = 0.001;
+    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);
+    }
+    
+    backUp();
+    
+    if (LS_IRR.read() > turnThreshold)
+        if (RS_IRR.read() < turnThreshold)
+            turnRight();
+        else
+            turnAround();
+    else if (RS_IRR.read() > turnThreshold)
+        if (LS_IRR.read() < turnThreshold)
+            turnRight();
+        else
+            turnAround();
+    else
+        turnAround();
+}
+    stop();
+}
+    /*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)
+        meas = meas * 100000; // Change the value to be in the 0 to 3300 range
+        printf("measure = %.0f mV\n", meas);
+        */
+        /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
+          LS_IRE = 1;
+        }
+        else {
+          LS_IRE = 0;
+        }
+        */
+        /*
+        forward();
+        //wait(0.2); // 200 ms
+    }
+    stop();
+    */
+    
+
+
+
+/*int main()      //only runs once
 {
     systicker.attach_us(&systick, 1000);    //enable interrupt
     while (1) {
@@ -144,3 +245,4 @@
     //
     //debugEncoder();
 }
+*/