Revision 12:921488918749, committed 2018-08-09

Comitter:

ryanlin97

Date:

Thu Aug 09 16:36:46 2018 +0000

Parent:

11:d14a1f7f1297

Child:

13:7b7f85403b4e

Commit message:

incorporated pid and encoder;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Aug 09 16:36:46 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/jvfausto/code/QEI/#2a173fdae3ca

--- a/chair_BNO055.lib	Wed Aug 01 22:39:22 2018 +0000
+++ b/chair_BNO055.lib	Thu Aug 09 16:36:46 2018 +0000
@@ -1,1 +1,1 @@
-chair_BNO055#3258d62af038
+chair_BNO055#7d6467fa1977

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.lib	Thu Aug 09 16:36:46 2018 +0000
@@ -0,0 +1,1 @@
+pid#539e3b3b91e1

--- 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();
+    }
 
+

--- a/wheelchair.h	Wed Aug 01 22:39:22 2018 +0000
+++ b/wheelchair.h	Thu Aug 09 16:36:46 2018 +0000
@@ -2,40 +2,80 @@
 #define wheelchair
 
 #include "chair_BNO055.h"
+#include "PID_v1.h"
+#include "QEI.h"
+
 //#include "chair_MPU9250.h"
 
 #define turn_precision 10
 #define def (2.5f/3.3f)
-#define high 3.3f
+#define high 3.3f/3.3f
 #define offset .02f
 #define low (1.7f/3.3f)
 #define process .1
 #define xDir D12 //top right two pins
 #define yDir D13 //top left two pins
-
+#define Encoder1 D0
+#define Encoder2 D1
+#define EncoderReadRate 1200
+#define Diameter 31.75
+/** Wheelchair class
+ * Used for controlling the smart wheelchair
+ */
 class Wheelchair
 {
 public:
+    /** Create Wheelchair Object with x,y pin for analog dc output
+     * serial for printout, and timer
+     */
     Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time);
+    
+    /** move using the joystick */
     void move(float x_coor, float y_coor);
+    
+    /* turn right a certain amount of degrees (overshoots)*/
     double turn_right(int deg);
+    
+    /* turn left a certain amount of degrees (overshoots)*/
     double turn_left(int deg);
+    
+    /* turn right a certain amount of degrees using PID*/
+    void pid_right(int deg);
+    
+    /* turn left a certain amount of degrees using PID*/
+    void pid_left(int deg);
+    
+    /* turning function that turns any direction */
     void turn(int deg);
+    
+    /* drive the wheelchair forward */
     void forward();
+    
+    /* drive the wheelchair backward*/
     void backward();
+    
+    /* turn the wheelchair right*/
     void right();
+    
+    /* turn the wheelchair left*/
     void left();
+    
+    /* stop the wheelchair*/
     void stop();
+    
+    /* function to get imu data*/
     void compass_thread();
-    chair_BNO055* imu;
-
+    float getDistance();
+    void resetDistance();
+    void pid_turn(int deg);
+    
 private:
     PwmOut* x;
     PwmOut* y;
-    //chair_BNO055* imu;
-    //chair_MPU9250* imu;
+    chair_BNO055* imu;
     Serial* out;
     Timer* ti;
+    QEI* wheel;
 
 };
 #endif
\ No newline at end of file