ryan lin / wheelchaircontrolRos

for ros

Dependencies:   QEI chair_BNO055 pid ros_lib_kinetic

Dependents:   wheelchaircontrolrealtimeROS

Fork of wheelchaircontrol by ryan lin

Diff: wheelchair.cpp

Revision:
18:3eadf01ec1b0
Parent:
17:93a636b16b9e
Child:
19:df4257c75ed0
--- a/wheelchair.cpp	Fri Aug 24 22:41:55 2018 +0000
+++ b/wheelchair.cpp	Mon Aug 27 04:48:16 2018 +0000
@@ -86,13 +86,13 @@
     Setpoint = curr_yaw + deg;
     pid_yaw = curr_yaw;
     if(Setpoint > 360) {
-     //   Setpoint -= 360;
         overturn = true;
     }
     myPID.SetTunings(5.5,0, 0.00345);
     myPID.SetOutputLimits(0, def-low);
     myPID.SetControllerDirection(REVERSE);
-    while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
+    
+    while(pid_yaw < Setpoint - 3){
         if(overturn && curr_yaw < Setpoint-deg-1)
         {
             pid_yaw = curr_yaw + 360;
@@ -104,12 +104,13 @@
         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 \r\n");
     }
+    
 void Wheelchair::pid_left(int deg) 
 {
     float pid_yaw;
@@ -120,13 +121,14 @@
     Setpoint = curr_yaw - deg;
     pid_yaw = curr_yaw;
     if(Setpoint < 0) {
-   //     Setpoint += 360;
         overturn = true;
     }
+    
     myPID.SetTunings(5,0, 0.004);
     myPID.SetOutputLimits(0,high-def);
     myPID.SetControllerDirection(REVERSE);
-    while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) {
+    
+    while(pid_yaw > Setpoint + 3){
        myPID.Compute();
        if(overturn && curr_yaw > Setpoint+deg+1)
        {