Diff: wheelchair.cpp

Revision:

16:53d912c86c7f

Parent:

14:9caca9fde9b0

Child:

17:93a636b16b9e

--- a/wheelchair.cpp	Mon Aug 13 21:33:09 2018 +0000
+++ b/wheelchair.cpp	Fri Aug 17 20:46:53 2018 +0000
@@ -5,22 +5,10 @@
 //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, .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);
-     //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 )
@@ -101,13 +89,14 @@
         overturn = true;
     }
     
-    myPID.SetOutputLimits(low, def);
+    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);
+    
+    while(Setpoint <= curr_yaw){
+        myPID.Compute();
+        y->write(Output+low);
         out->printf("curr_yaw %f\n", curr_yaw);
+        wait(process);
         }
     Wheelchair::stop();
     out->printf("done \n");