Diff: wheelchair.cpp

Revision:

18:663b6d693252

Parent:

17:7f3b69300bb6

Child:

19:71a6621ee5c3

--- a/wheelchair.cpp	Tue Aug 28 23:24:08 2018 +0000
+++ b/wheelchair.cpp	Wed Aug 29 16:51:33 2018 +0000
@@ -7,10 +7,11 @@
 double curr_yaw;
 double encoder_distance;
 char myString[64];
-double Setpoint, Output, Input;
-double pid_yaw, Distance, Setpoint2;
+double Setpoint, Output, Input, Input2;
+double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;
 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);
+PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
+PID myPIDDistance2(&Input2, &Output2, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT);
 //QEI wheel_right(D0, D1, NC, 450);
 void Wheelchair::compass_thread() {
      curr_yaw = imu->yaw();
@@ -223,13 +224,13 @@
                 }
         }            
         myString[i-1] = 0;
-        double tempor = atof(myString);
+        double tempor2 = 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;
@@ -249,20 +250,85 @@
 void Wheelchair::pid_reverse(double mm)
 {
     qei.putc('r');
+    out->printf("pid reverse\r\n");
 
-  /*  double tempor;
+    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);
+    myPIDDistance.SetOutputLimits(0,def);
+    myPIDDistance.SetControllerDirection(REVERSE);
     y->write(def);
-    while(Distance < Setpoint-5){//pid_yaw < Setpoint + 2) {
-        tempor = Output + 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();
+        
+        qei.putc('k');
+        //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 tempor2 = atof(myString);
+        out->printf("displacement = %f\r\n", tempor2);
+        
+        if(abs(tempor - encoder_distance) < 500)
+        {
+            encoder_distance = tempor;
+            out->printf("this is fine\r\n");
+        }        
+        if(abs(tempor - encoder_distance2) < 500)
+        {
+            encoder_distance = tempor;
+            out->printf("this is fine\r\n");
+        }
+        for(i = 0; i < 64; i++)
+        {
+            myString[i] = 0;
+        } 
+        tempor = Output;
         x->write(tempor);
-  //      out->printf("distance %f\r\n", wheel_right.getDistance(37.5));
-  //      Distance = wheel_right.getDistance(37.5);
-        wait(.05);
-    }    */
+        out->printf("distance %f\r\n", encoder_distance);
+        }
 }   
 double Wheelchair::turn_right(int deg)
 {