Diff: PID_control.cpp

Revision:

11:e647b14ff4ef

Parent:

10:707e542688dc

Child:

12:e44dd0d4f1ee

--- a/PID_control.cpp	Fri Dec 01 02:00:02 2017 +0000
+++ b/PID_control.cpp	Fri Dec 01 03:25:53 2017 +0000
@@ -1,14 +1,16 @@
-//rev1 
-//added routines to main for easier testing of movement functions
+ //added routines to main for easier testing of movement functions
 //wrote testStop();
+//Kevin Lee Nov 30, 2017
+//edited testStop();
+//started a testTurns();
+//testTurns not working as thought
+//Myles Byrne Nov 30, 2017
+
 //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"
 
@@ -32,7 +34,7 @@
 Timer timer;
 int counter = 0;
 float threshold = 0.00085;
-float turnThreshold = 0.00085;
+float turnThreshold = 0.0004;
     
 
 
@@ -133,6 +135,48 @@
     }
 }
 
+void forwardSlow()
+{
+    float iS_speed = 0.05;
+    double f1_speed = iS_speed + C_speed;
+    double f2_speed = iS_speed - C_speed;
+
+    /*pc.printf("C: %f", C_speed);
+    if (C_speed < 0)
+        pc.printf("-");
+    if (C_speed > 0)
+        pc.printf("+");
+    */
+
+
+    if(f1_speed >= 0.15) {   //upper bound, can not go faster
+        f1_speed = 0.15;
+    }
+    if (f1_speed <= 0.05)
+        f1_speed = 0.05;
+
+    if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
+        f2_speed = 0.05;
+    }
+
+    pc.printf(" f1: %f", f1_speed);
+    pc.printf(" f2: %f", f2_speed);
+
+    //problems when left wheel is held for the + case
+    if (Error > 0) { //right wheel is turning more
+        m_Left_F.write(f1_speed);
+        m_Right_F.write(f2_speed); //f2_speed
+    }
+    if (Error < 0) { //left wheel is turning more
+        m_Right_F.write(f1_speed);
+        m_Left_F.write(f2_speed);  //f2_speed
+    }
+    if (Error == 0) {
+        m_Right_F.write(iS_speed);
+        m_Left_F.write(iS_speed);
+    }
+}
+
 void backUp()
 {
     m_Left_F.write(0);
@@ -252,6 +296,8 @@
     stop();
 } 
 */
+
+
 void testStop() //rev1
 {
     //printf("\nAnalogIn example\n");
@@ -259,9 +305,15 @@
     RF_IRE.write(1);
     forward();  
     while(1) {
-        if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
+        if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
         {
             stop();
+            forwardSlow();
+            if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
+            {
+                stop();
+                break;
+            }
         }
         float value1 = LF_IRR.read();
         float value2 = RF_IRR.read(); 
@@ -272,11 +324,57 @@
             
 }
 
+
 void testTurnAround() //rev1
 {
     
 }
 
+void testTurns()
+{
+    LF_IRE.write(1);
+    RF_IRE.write(1);
+    forward();  
+    while(1)
+    {
+        if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
+        {
+            stop();
+            forwardSlow();
+            if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
+            {
+                stop();
+                if (LS_IRR.read() > turnThreshold) {
+                    if (RS_IRR.read() < turnThreshold)
+                        turnRight();
+                    else
+                        turnAround(); 
+                }
+        
+                else if (RS_IRR.read() > turnThreshold) {
+                    if (LS_IRR.read() < turnThreshold)
+                        turnLeft();
+                    else
+                        turnAround();
+                }
+                else
+                    turnAround();
+        
+                LS_IRE.write(0);
+                RS_IRE.write(0); 
+            }
+            //stop();
+            break;
+        }
+    }
+        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);
+}
+
+
 
 int main()
 {
@@ -287,6 +385,7 @@
 
 
 
+
     /*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)