Diff: wheelchair.cpp

Revision:

17:7f3b69300bb6

Parent:

14:9caca9fde9b0

Child:

18:663b6d693252

diff -r b403082eeacd -r 7f3b69300bb6 wheelchair.cpp
--- a/wheelchair.cpp	Mon Aug 13 21:54:31 2018 +0000
+++ b/wheelchair.cpp	Tue Aug 28 23:24:08 2018 +0000
@@ -1,27 +1,46 @@
 #include "wheelchair.h"
+Serial qei(D1, D0);
 
 bool manual_drive = false;
 volatile float north;
 //volatile double curr_yaw;
 double curr_yaw;
-double Setpoint, Output;
-PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT);
-
+double encoder_distance;
+char myString[64];
+double Setpoint, Output, Input;
+double pid_yaw, Distance, Setpoint2;
+PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
+PID myPIDDistance(&encoder_distance, &Output, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT);
+//QEI wheel_right(D0, D1, NC, 450);
 void Wheelchair::compass_thread() {
-    
-     //north = lowPass(imu->angle_north());
-     //Input = (double)curr_yaw;
      curr_yaw = imu->yaw();
      north = boxcar(imu->angle_north());
-    //out->printf("curr_yaw %f\n", curr_yaw);
-     //out->printf("%f\n", curr_yaw);
-     //out->printf("%f gyroz\n",imu->gyro_z());
-     //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw);
-    
-        //out->printf("Yaw is %f\n", imu->yaw());
-        //out->printf("north is %f\n", imu->angle_north());
-    
+
     }
+void Wheelchair::distance_thread() {
+/*        int i;
+        qei.putc('h');
+        //qei.gets(myString, 10);
+        ti->reset();
+
+        for (i=0; myString[i-1] != '\n'; i++) {
+            while (true) {
+                //pc.printf("%f\r\n", ti.read());
+                if (ti->read() > .02) break;
+                if (qei.readable()) {
+                    myString[i]= qei.getc();
+                    break;
+                    }
+                }
+        }            
+        myString[i-1] = 0;
+        encoder_distance = atof(myString);
+            //out->printf("displacement = %f\r\n", encoder_distance);
+        for(i = 0; i < 64; i++)
+        {
+            myString[i] = 0;
+        }   */ 
+}
     
 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
 {
@@ -32,7 +51,7 @@
     Wheelchair::stop();
     imu->setup();
     out = pc;
-    out->printf("wheelchair setup done \n");
+    out->printf("wheelchair setup done \r\n");
     ti = time;
     wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
     myPID.SetMode(AUTOMATIC);
@@ -53,7 +72,7 @@
     x->write(scaled_x);
     y->write(scaled_y);
     
-    //out->printf("yaw %f\n", imu->yaw());
+    //out->printf("yaw %f\r\r\n", imu->yaw());
 
 }
 
@@ -61,6 +80,7 @@
 {
     x->write(high);
     y->write(def+offset);
+  //  out->printf("distance %f\r\n", wheel_right.getDistance(37.5));
 }
 
 void Wheelchair::backward()
@@ -92,45 +112,66 @@
 {
     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.SetOutputLimits(low, def);
+    myPID.SetTunings(5.5,0, 0.00345);
+    myPID.SetOutputLimits(0, def-low);
     myPID.SetControllerDirection(REVERSE);
-    out->printf("setpoint %f curr_yaw %f input %f output %i  %i same address\n", Setpoint, curr_yaw, *(myPID.myInput), *(myPID.myOutput), (myPID.myInput==&curr_yaw), (myPID.myOutput == &Output) );
-    out->printf("addresses %i %i\n", myPID.myInput, &curr_yaw);
-    while(myPID.Compute()) {
-        y->write(Output);
-        out->printf("curr_yaw %f\n", 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();
+        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) 
 {
     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) {
@@ -152,11 +193,81 @@
         Wheelchair::pid_left(turnAmt);
         }
     }
+void Wheelchair::pid_foward(double mm)
+{
+    qei.putc('r');
+    out->printf("pid foward\r\n");
+
+    double tempor;
+    Setpoint2 = mm;
+
+  //  Setpoint = wheel_right.getDistance(37.5)+mm;
+    myPIDDistance.SetTunings(5,0, 0.004);
+    myPIDDistance.SetOutputLimits(0,high-def);
+    myPIDDistance.SetControllerDirection(DIRECT);
+    y->write(def);
+    while(encoder_distance < Setpoint2-5){//pid_yaw < Setpoint + 2) {
+        int i;
+        qei.putc('h');
+        //qei.gets(myString, 10);
+        ti->reset();
+
+        for (i=0; myString[i-1] != '\n'; i++) {
+            while (true) {
+                //pc.printf("%f\r\n", ti.read());
+                if (ti->read() > .02) break;
+                if (qei.readable()) {
+                    myString[i]= qei.getc();
+                    break;
+                    }
+                }
+        }            
+        myString[i-1] = 0;
+        double tempor = atof(myString);
+        out->printf("displacement = %f\r\n", tempor);
+        if(abs(tempor - encoder_distance) < 500)
+        {
+            encoder_distance = tempor;
+            out->printf("this is fine\r\n");
+        }
+        for(i = 0; i < 64; i++)
+        {
+            myString[i] = 0;
+        } 
+        Input = encoder_distance;
+        out->printf("input foward %f\r\n", Input);
+        wait(.1);
+        myPIDDistance.Compute();
+
+        tempor = Output + def;
+        x->write(tempor);
+        out->printf("distance %f\r\n", encoder_distance);
+        }
+        
     
+}   
+void Wheelchair::pid_reverse(double mm)
+{
+    qei.putc('r');
+
+  /*  double tempor;
+  //  Setpoint = wheel_right.getDistance(37.5)+mm;
+    myPIDDistance.SetTunings(5,0, 0.004);
+    myPIDDistance.SetOutputLimits(0,high-def);
+    myPIDDistance.SetControllerDirection(DIRECT);
+    y->write(def);
+    while(Distance < Setpoint-5){//pid_yaw < Setpoint + 2) {
+        tempor = Output + def;
+        x->write(tempor);
+  //      out->printf("distance %f\r\n", wheel_right.getDistance(37.5));
+  //      Distance = wheel_right.getDistance(37.5);
+        wait(.05);
+    }    */
+}   
 double Wheelchair::turn_right(int deg)
 {
     bool overturn = false;
-    out->printf("turning right\n");
+    out->printf("turning right\r\n");
 
     double start = curr_yaw;
     double final = start + deg;
@@ -166,13 +277,13 @@
         overturn = true;
     }
 
-    out->printf("start %f, final %f\n", start, final);
+    out->printf("start %f, final %f\r\n", start, final);
 
     double curr = -1;
     while(curr <= final - 30) {
         Wheelchair::right();
         if( out->readable()) {
-            out->printf("stopped\n");
+            out->printf("stopped\r\n");
             Wheelchair::stop();
             return;
         }
@@ -182,26 +293,26 @@
         }
     }
     
-    out->printf("done turning start %f final %f\n", start, final);
+    out->printf("done turning start %f final %f\r\n", start, final);
     Wheelchair::stop();
     
      //delete me
     wait(5);
     
     float correction = final - curr_yaw;
-    out->printf("final pos %f actual pos %f\n", final, curr_yaw);
+    out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
     Wheelchair::turn_left(abs(correction));
     Wheelchair::stop();
     
     wait(5);
-    out->printf("curr_yaw %f\n", curr_yaw);
+    out->printf("curr_yaw %f\r\n", curr_yaw);
     return final;
 }
 
 double Wheelchair::turn_left(int deg)
 {
     bool overturn = false;
-    out->printf("turning left\n");
+    out->printf("turning left\r\n");
 
     double start = curr_yaw;
     double final = start - deg;
@@ -211,13 +322,13 @@
         overturn = true;
     }
 
-    out->printf("start %f, final %f\n", start, final);
+    out->printf("start %f, final %f\r\n", start, final);
 
     double curr = 361;
     while(curr >= final) {
         Wheelchair::left();
         if( out->readable()) {
-            out->printf("stopped\n");
+            out->printf("stopped\r\n");
             Wheelchair::stop();
             return;
         }
@@ -228,14 +339,14 @@
         }
     }
 
-    out->printf("done turning start %f final %f\n", start, final);
+    out->printf("done turning start %f final %f\r\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);
+    out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
     Wheelchair::turn_right(abs(correction));
     Wheelchair::stop();
 */
@@ -266,11 +377,11 @@
     wait(2);
     
     float correction = finalpos - curr_yaw;
-    out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
+    out->printf("final pos %f actual pos %f\r\n", finalpos, curr_yaw);
     
     
     //if(abs(correction) > turn_precision) {
-        out->printf("correcting %f\n", correction);
+        out->printf("correcting %f\r\n", correction);
         //ti->reset();
         Wheelchair::turn_left(curr_yaw - finalpos);
         return;