Diff: wheelchair.cpp

Revision:

17:93a636b16b9e

Parent:

16:53d912c86c7f

Child:

18:3eadf01ec1b0

--- a/wheelchair.cpp	Fri Aug 17 20:46:53 2018 +0000
+++ b/wheelchair.cpp	Fri Aug 24 22:41:55 2018 +0000
@@ -5,7 +5,7 @@
 //volatile double curr_yaw;
 double curr_yaw;
 double Setpoint, Output;
-//PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT);
+PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT);
 
 void Wheelchair::compass_thread() {
      curr_yaw = imu->yaw();
@@ -78,48 +78,69 @@
 // clockwise is +
 void Wheelchair::pid_right(int deg) 
 {
+    float pid_yaw;
     bool overturn = false;
     
-    out->printf("pid right\n");
+    out->printf("pid right\r\r\n");
     x->write(def);
     Setpoint = curr_yaw + deg;
-    
+    pid_yaw = curr_yaw;
     if(Setpoint > 360) {
-        Setpoint -= 360;
+     //   Setpoint -= 360;
         overturn = true;
     }
-    
+    myPID.SetTunings(5.5,0, 0.00345);
     myPID.SetOutputLimits(0, def-low);
     myPID.SetControllerDirection(REVERSE);
-    
-    while(Setpoint <= curr_yaw){
+    while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
+        if(overturn && curr_yaw < Setpoint-deg-1)
+        {
+            pid_yaw = curr_yaw + 360;
+        }   
+        else
+            pid_yaw = curr_yaw;
         myPID.Compute();
-        y->write(Output+low);
-        out->printf("curr_yaw %f\n", curr_yaw);
-        wait(process);
+        double tempor = Output+low;
+        y->write(tempor);
+        out->printf("curr_yaw %f\r\r\n", curr_yaw);
+        out->printf("Setpoint = %f \r\n", Setpoint);
+
+        wait(.05);
         }
     Wheelchair::stop();
-    out->printf("done \n");
+    out->printf("done \r\n");
     }
-
 void Wheelchair::pid_left(int deg) 
 {
+    float pid_yaw;
     bool overturn = false;
     
+    out->printf("pid Left\r\r\n");
     x->write(def);
     Setpoint = curr_yaw - deg;
-    
+    pid_yaw = curr_yaw;
     if(Setpoint < 0) {
-        Setpoint += 360;
+   //     Setpoint += 360;
         overturn = true;
     }
-    myPID.SetOutputLimits(def,high);
-    myPID.SetControllerDirection(DIRECT);
-    while(myPID.Compute()) {
-        y->write(Output);
-        out->printf("curr_yaw %f\n", curr_yaw);
+    myPID.SetTunings(5,0, 0.004);
+    myPID.SetOutputLimits(0,high-def);
+    myPID.SetControllerDirection(REVERSE);
+    while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) {
+       myPID.Compute();
+       if(overturn && curr_yaw > Setpoint+deg+1)
+       {
+          pid_yaw = curr_yaw - 360;
+        }   
+        else
+            pid_yaw = curr_yaw;
+       double tempor = Output+def;
+
+        y->write(tempor);
+        out->printf("curr_yaw %f\r\n", curr_yaw);
+        wait(.05);
         }
-    Wheelchair::stop();
+        Wheelchair::stop();
     }
 
 void Wheelchair::pid_turn(int deg) {
@@ -220,14 +241,7 @@
     out->printf("done turning start %f final %f\n", start, final);
     Wheelchair::stop();
     
-    //delete me
-    wait(2);
-    /*
-    float correction = final - curr_yaw;
-    out->printf("final pos %f actual pos %f\n", final, curr_yaw);
-    Wheelchair::turn_right(abs(correction));
-    Wheelchair::stop();
-*/
+   
     return final;
 }
 
@@ -243,16 +257,7 @@
     
     double finalpos;
     int turnAmt = abs(deg);
-    //ti->reset();
-    /*
-    if(deg >= 0){
-        finalpos = Wheelchair::turn_right(turnAmt);
-        }
-    else {
-        finalpos = Wheelchair::turn_left(turnAmt);
-        }
-    */
-    wait(2);
+
     
     float correction = finalpos - curr_yaw;
     out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);