Diff: wheelchair.cpp

Revision:

21:69df88af7c46

Parent:

19:df4257c75ed0

diff -r b7a5e4019cae -r 69df88af7c46 wheelchair.cpp
--- a/wheelchair.cpp	Thu Aug 30 03:44:03 2018 +0000
+++ b/wheelchair.cpp	Fri Aug 31 19:41:42 2018 +0000
@@ -33,18 +33,18 @@
     return atoi(input);
     }
     
-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);
     imu = new chair_BNO055(pc, time);
-    //imu = new chair_MPU9250(pc, time);
+   
     Wheelchair::stop();
     imu->setup();
     out = pc;
     out->printf("wheelchair setup done \n");
     ti = time;
-    wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
+    wheel = qei;
     myPID.SetMode(AUTOMATIC);
 }
 
@@ -111,8 +111,8 @@
         overturn = true;
     }
     myPID.SetTunings(5.5,0, 0.00345);
-    myPID.SetOutputLimits(0, def-low);
-    myPID.SetControllerDirection(REVERSE);
+    myPID.SetOutputLimits(0, def - low - .15);
+    myPID.SetControllerDirection(DIRECT);
     
     while(pid_yaw < Setpoint - 3){
         if(overturn && curr_yaw < Setpoint-deg-1)
@@ -122,7 +122,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);
@@ -147,7 +147,7 @@
     }
     
     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){
@@ -318,47 +318,51 @@
     
 void Wheelchair::pid_forward(double mm)
 {
-    y->write(def);
-    qei.putc('r');
+    y->write(def + offset);
+    wheel->reset();
+    Input = 0;
     out->printf("pid foward\r\n");
 
-    double tempor;
-    Setpoint = mm;
+    
+    Setpoint = mm-20;
 
   //  Setpoint = wheel_right.getDistance(37.5)+mm;
     myPIDDistance.SetTunings(5,0, 0.004);
-    myPIDDistance.SetOutputLimits(0,high-def);
+    myPIDDistance.SetOutputLimits(0,high - def - 0.15);
     myPIDDistance.SetControllerDirection(DIRECT);
     
-    while(encoder_distance < Setpoint-5){//pid_yaw < Setpoint + 2) {
-        ti->reset();
-        
-        qei.putc('h');
-        double curr_dist = readEncoder();
-        out->printf("displacement = %f\r\n", curr_dist);
-        if(abs(curr_dist - encoder_distance) < 500)
-        {
-            encoder_distance = curr_dist;
-            out->printf("this is fine\r\n");
-        }       
-
-        Input = encoder_distance;
-        out->printf("input foward %f\r\n", Input);
-        
-        wait(process);
-        
+    while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
+        if(out->readable()){
+            Wheelchair::stop();
+            break;}
+        Input = wheel->getDistance(53.975);
+        //out->printf("input foward %d\r\n", wheel->getPulses());
+        wait(.05);
         myPIDDistance.Compute();
-        out->printf("distance %f\r\n", encoder_distance);
-        
-        x->write(Output+def);
+ 
+        x->write(Output + def);
+        out->printf("distance %f\r\n", Input);
         }
 
 }   
 
+void Wheelchair::desk() {
+    Wheelchair::pid_forward(5461);
+    Wheelchair::pid_right(87);
+    Wheelchair::pid_forward(3658);
+    Wheelchair::pid_left(87);
+    Wheelchair::pid_forward(3734);
+    }
+
 void Wheelchair::kitchen() {
     Wheelchair::pid_forward(5461);
-    Wheelchair::pid_right(90);
+    Wheelchair::pid_right(87);
     Wheelchair::pid_forward(3658);
-    Wheelchair::pid_left(90);
-    Wheelchair::pid_forward(3734);
+    Wheelchair::pid_left(87);
+    Wheelchair::pid_forward(305);
+    }
+    
+void Wheelchair::back() {
+    Wheelchair::pid_right(177);
+    Wheelchair::pid_forward(3700);
     }
\ No newline at end of file