Diff: wheelchair.cpp

Revision:

19:df4257c75ed0

Parent:

18:3eadf01ec1b0

Child:

21:69df88af7c46

--- a/wheelchair.cpp	Mon Aug 27 04:48:16 2018 +0000
+++ b/wheelchair.cpp	Wed Aug 29 21:01:51 2018 +0000
@@ -1,16 +1,38 @@
 #include "wheelchair.h"
+Serial qei(D1, D0);
+
+DigitalOut on(D12);
+DigitalOut off(D11);
+DigitalOut up(D2);
+DigitalOut down(D3);
 
 bool manual_drive = false;
-volatile float north;
-//volatile double curr_yaw;
+double encoder_distance;
+
 double curr_yaw;
-double Setpoint, Output;
+double Setpoint, Output, Input;
 PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT);
+PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
 
 void Wheelchair::compass_thread() {
      curr_yaw = imu->yaw();
     }
     
+double Wheelchair::readEncoder() {
+    char input[64] = {0};
+    for (int i=0; input[i-1] != '\n'; i++) {
+            while (true) {
+                //pc.printf("%f\r\n", ti.read());
+                if (ti->read() > .02) break;
+                if (qei.readable()) {
+                    input[i]= qei.getc();
+                    break;
+                    }
+                }
+        }
+    return atoi(input);
+    }
+    
 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
 {
     x = new PwmOut(xPin);
@@ -282,4 +304,61 @@
     wheel->reset();
     }
 
+void Wheelchair::turn_on() {
+    on = 1;
+    wait(1);
+    on = 0;
+    }
 
+void Wheelchair::turn_off() {
+    off = 1;
+    wait(1);
+    off = 0;
+    }
+    
+void Wheelchair::pid_forward(double mm)
+{
+    y->write(def);
+    qei.putc('r');
+    out->printf("pid foward\r\n");
+
+    double tempor;
+    Setpoint = mm;
+
+  //  Setpoint = wheel_right.getDistance(37.5)+mm;
+    myPIDDistance.SetTunings(5,0, 0.004);
+    myPIDDistance.SetOutputLimits(0,high-def);
+    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);
+        
+        myPIDDistance.Compute();
+        out->printf("distance %f\r\n", encoder_distance);
+        
+        x->write(Output+def);
+        }
+
+}   
+
+void Wheelchair::kitchen() {
+    Wheelchair::pid_forward(5461);
+    Wheelchair::pid_right(90);
+    Wheelchair::pid_forward(3658);
+    Wheelchair::pid_left(90);
+    Wheelchair::pid_forward(3734);
+    }
\ No newline at end of file