Diff: wheelchair.cpp

Revision:

12:921488918749

Parent:

11:d14a1f7f1297

Child:

14:9caca9fde9b0

--- a/wheelchair.cpp	Wed Aug 01 22:39:22 2018 +0000
+++ b/wheelchair.cpp	Thu Aug 09 16:36:46 2018 +0000
@@ -2,21 +2,25 @@
 
 bool manual_drive = false;
 volatile float north;
-volatile float curr_yaw;
-volatile float comp_yn;
+//volatile double curr_yaw;
+double curr_yaw;
+double Setpoint, Output;
+PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT);
 
 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);
-     comp_yn = complement(north,curr_yaw,.5);
-     out->printf("%f curr_yaw\n", curr_yaw);
-     out->printf("%f gyroz\n",imu->gyro_z());
+    //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());
+    
     }
     
 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
@@ -30,7 +34,8 @@
     out = pc;
     out->printf("wheelchair setup done \n");
     ti = time;
-
+    wheel = new QEI(Encoder1, Encoder1, NC, EncoderReadRate);
+    myPID.SetMode(AUTOMATIC);
 }
 
 /*
@@ -68,8 +73,6 @@
 {
     x->write(def);
     y->write(low);
-    //y->write(1.9);
-
 }
 
 void Wheelchair::left()
@@ -85,7 +88,71 @@
 }
 // counter clockwise is -
 // clockwise is +
+void Wheelchair::pid_right(int deg) 
+{
+    bool overturn = false;
+    
+    out->printf("pid right\n");
+    x->write(def);
+    Setpoint = curr_yaw + deg;
+    
+    if(Setpoint > 360) {
+        Setpoint -= 360;
+        overturn = true;
+    }
+    
+    myPID.SetOutputLimits(low, def);
+    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);
+        }
+    Wheelchair::stop();
+    out->printf("done \n");
+    }
 
+void Wheelchair::pid_left(int deg) 
+{
+    bool overturn = false;
+    
+    x->write(def);
+    Setpoint = curr_yaw - deg;
+    
+    if(Setpoint < 0) {
+        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);
+        }
+    Wheelchair::stop();
+    }
+
+void Wheelchair::pid_turn(int deg) {
+    if(deg > 180) {
+        deg -= 360;
+    }
+
+    else if(deg < -180) {
+        deg+=360;
+    }  
+    
+    int turnAmt = abs(deg);
+    ti->reset();
+
+    if(deg >= 0){
+        Wheelchair::pid_right(turnAmt);
+        }
+    else {
+        Wheelchair::pid_left(turnAmt);
+        }
+    }
+    
 double Wheelchair::turn_right(int deg)
 {
     bool overturn = false;
@@ -102,7 +169,7 @@
     out->printf("start %f, final %f\n", start, final);
 
     double curr = -1;
-    while(curr <= final) {
+    while(curr <= final - 30) {
         Wheelchair::right();
         if( out->readable()) {
             out->printf("stopped\n");
@@ -118,6 +185,16 @@
     out->printf("done turning start %f final %f\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);
+    Wheelchair::turn_left(abs(correction));
+    Wheelchair::stop();
+    
+    wait(5);
+    out->printf("curr_yaw %f\n", curr_yaw);
     return final;
 }
 
@@ -126,7 +203,7 @@
     bool overturn = false;
     out->printf("turning left\n");
 
-    double start = comp_yn;
+    double start = curr_yaw;
     double final = start - deg;
 
     if(final < 0) {
@@ -153,7 +230,15 @@
 
     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;
 }
 
@@ -191,7 +276,14 @@
         return;
         //} 
     
-    
 }
 
+float Wheelchair::getDistance() {
+    return wheel->getDistance(Diameter);
+    }
+    
+void Wheelchair::resetDistance(){
+    wheel->reset();
+    }
 
+