Diff: wheelchair.cpp

Revision:

19:71a6621ee5c3

Parent:

18:663b6d693252

Child:

20:f42db4ae16f0

--- a/wheelchair.cpp	Wed Aug 29 16:51:33 2018 +0000
+++ b/wheelchair.cpp	Fri Aug 31 17:09:56 2018 +0000
@@ -1,5 +1,4 @@
 #include "wheelchair.h"
-Serial qei(D1, D0);
 
 bool manual_drive = false;
 volatile float north;
@@ -7,43 +6,21 @@
 double curr_yaw;
 double encoder_distance;
 char myString[64];
-double Setpoint, Output, Input, Input2;
-double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;
+volatile double Setpoint, Output, Input, Input2;
+volatile 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(&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);
+PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0025, 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();
      north = boxcar(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 )
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei)
 {
     x = new PwmOut(xPin);
     y = new PwmOut(yPin);
@@ -52,9 +29,10 @@
     Wheelchair::stop();
     imu->setup();
     out = pc;
+    wheel = qei;
     out->printf("wheelchair setup done \r\n");
     ti = time;
-    wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
+    //wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
     myPID.SetMode(AUTOMATIC);
 }
 
@@ -121,9 +99,9 @@
      //   Setpoint -= 360;
         overturn = true;
     }
-    myPID.SetTunings(5.5,0, 0.00345);
-    myPID.SetOutputLimits(0, def-low);
-    myPID.SetControllerDirection(REVERSE);
+    myPID.SetTunings(5.5,0, 0.0035);
+    myPID.SetOutputLimits(0, def-low-.15);
+    myPID.SetControllerDirection(DIRECT);
     while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
         if(overturn && curr_yaw < Setpoint-deg-1)
         {
@@ -132,7 +110,7 @@
         else
             pid_yaw = curr_yaw;
         myPID.Compute();
-        double tempor = Output+low;
+        double tempor = -Output+def;
         y->write(tempor);
         out->printf("curr_yaw %f\r\r\n", curr_yaw);
         out->printf("Setpoint = %f \r\n", Setpoint);
@@ -156,9 +134,9 @@
         overturn = true;
     }
     myPID.SetTunings(5,0, 0.004);
-    myPID.SetOutputLimits(0,high-def);
+    myPID.SetOutputLimits(0,high-def-.12);
     myPID.SetControllerDirection(REVERSE);
-    while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) {
+    while(pid_yaw > Setpoint+3){//pid_yaw < Setpoint + 2) {
        myPID.Compute();
        if(overturn && curr_yaw > Setpoint+deg+1)
        {
@@ -194,62 +172,39 @@
         Wheelchair::pid_left(turnAmt);
         }
     }
-void Wheelchair::pid_foward(double mm)
+void Wheelchair::pid_forward(double mm)
 {
-    qei.putc('r');
+    mm -= 20;
+    Input = 0;
+    wheel->reset();
     out->printf("pid foward\r\n");
 
     double tempor;
-    Setpoint2 = mm;
+    Setpoint = mm;
 
   //  Setpoint = wheel_right.getDistance(37.5)+mm;
     myPIDDistance.SetTunings(5,0, 0.004);
-    myPIDDistance.SetOutputLimits(0,high-def);
+    myPIDDistance.SetOutputLimits(0,high-def-.15);
     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 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;
-        } 
-        Input = encoder_distance;
-        out->printf("input foward %f\r\n", Input);
-        wait(.1);
+    y->write(def+offset);
+    while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
+        if(out->readable())
+            break;
+        Input = wheel->getDistance(53.975);
+        //out->printf("input foward %d\r\n", wheel->getPulses());
+        wait(.05);
         myPIDDistance.Compute();
 
         tempor = Output + def;
         x->write(tempor);
-        out->printf("distance %f\r\n", encoder_distance);
+        out->printf("distance %f\r\n", Input);
         }
         
     
 }   
 void Wheelchair::pid_reverse(double mm)
 {
-    qei.putc('r');
+   /* qei.putc('r');
     out->printf("pid reverse\r\n");
 
     double tempor;
@@ -293,8 +248,8 @@
         wait(.1);
         myPIDDistance.Compute();
         
+        // get value from encoder2
         qei.putc('k');
-        //qei.gets(myString, 10);
         ti->reset();
 
         for (i=0; myString[i-1] != '\n'; i++) {
@@ -316,9 +271,9 @@
             encoder_distance = tempor;
             out->printf("this is fine\r\n");
         }        
-        if(abs(tempor - encoder_distance2) < 500)
+        if(abs(tempor2 - encoder_distance2) < 500)
         {
-            encoder_distance = tempor;
+            encoder_distance2 = tempor2;
             out->printf("this is fine\r\n");
         }
         for(i = 0; i < 64; i++)
@@ -328,7 +283,7 @@
         tempor = Output;
         x->write(tempor);
         out->printf("distance %f\r\n", encoder_distance);
-        }
+        }*/
 }   
 double Wheelchair::turn_right(int deg)
 {
@@ -462,5 +417,24 @@
 void Wheelchair::resetDistance(){
     wheel->reset();
     }
+void Wheelchair::desk() {
+    Wheelchair::pid_forward(5461);
+    Wheelchair::pid_right(87);
+    Wheelchair::pid_forward(3658);
+    Wheelchair::pid_right(87);
+    Wheelchair::pid_forward(3658);
+    }
 
+void Wheelchair::kitchen() {
+    Wheelchair::pid_forward(5461);
+    Wheelchair::pid_right(85);
+    Wheelchair::pid_forward(3658);
+    Wheelchair::pid_left(90);
+    Wheelchair::pid_forward(305);
+    }
 
+void Wheelchair::desk_to_kitchen(){
+    Wheelchair::pid_right(180);
+    Wheelchair::pid_forward(3700);
+    }
+