Revision 23:8d11d953ceeb, committed 2018-10-16

Comitter:

jvfausto

Date:

Tue Oct 16 23:03:40 2018 +0000

Parent:

20:f42db4ae16f0

Child:

24:d2f234fbc20d

Commit message:

revision

Changed in this revision

--- a/PID.lib	Fri Aug 31 20:00:01 2018 +0000
+++ b/PID.lib	Tue Oct 16 23:03:40 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/jvfausto/code/PID/#60801ab3cbf9
+https://os.mbed.com/users/jvfausto/code/PID/#62f1e316f15b

--- a/wheelchair.cpp	Fri Aug 31 20:00:01 2018 +0000
+++ b/wheelchair.cpp	Tue Oct 16 23:03:40 2018 +0000
@@ -1,414 +1,202 @@
 #include "wheelchair.h"
 
-bool manual_drive = false;
-volatile float north;
-//volatile double curr_yaw;
-double curr_yaw;
-double encoder_distance;
-char myString[64];
-volatile double Setpoint, Output, Input, Input2;
-volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;
-PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
-PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0025, P_ON_E, DIRECT);
+bool manual_drive = false;                                                             // Variable Changes between joystick and auto drive
+double curr_yaw;                                                                       // Variable that contains current relative angle
+double encoder_distance;                                                               // Keeps distanse due to original position
 
+volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
+volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;              // Variables for PID
 
-//PID myPIDDistance2(&Input2, &Output2, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT);
-//QEI wheel_right(D0, D1, NC, 450);
-void Wheelchair::compass_thread() {
+PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
+PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);       // Distance PID object constructor
+
+void Wheelchair::compass_thread() {                                                    // Thread that measures which angle we are at
      curr_yaw = imu->yaw();
-     north = boxcar(imu->angle_north());
-
     }
     
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei)   // Function Constructor for Wheelchair class
 {
-    x = new PwmOut(xPin);
+    //Initializes X and Y variables to Pins
+    x = new PwmOut(xPin);                                                               
     y = new PwmOut(yPin);
+    
+    // Initializes IMU Library
     imu = new chair_BNO055(pc, time);
-    //imu = new chair_MPU9250(pc, time);
-    Wheelchair::stop();
-    imu->setup();
-    out = pc;
-    wheel = qei;
-    out->printf("wheelchair setup done \r\n");
+    Wheelchair::stop();                                                                 // Wheelchair is not moving when initializing
+    imu->setup();                                                                       // turns on the IMU
+    out = pc;                                                                           // "out" is called for serial monitor
+    wheel = qei;                                                                        // "wheel" is called for encoder
+    out->printf("wheelchair setup done \r\n");                                          // make sure it initialized
     ti = time;
-    //wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
-    myPID.SetMode(AUTOMATIC);
+    myPID.SetMode(AUTOMATIC);                                                           // set PID to automatic
 }
 
-/*
-* joystick has analog out of 200-700, scale values between 1.3 and 3.3
-*/
-void Wheelchair::move(float x_coor, float y_coor)
+void Wheelchair::move(float x_coor, float y_coor)                                       // moves the chair with joystick on manual
 {
 
-    float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
-    float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
-    
-   // lowPass(scaled_x);
-    //lowPass(scaled_y);
+    float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;                                     // Scales one joystic measurement to the
+    float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;                                     // chair's joystic measurement
     
-    x->write(scaled_x);
+    x->write(scaled_x);                                                                 // Sends the scaled joystic values to the chair
     y->write(scaled_y);
-    
-    //out->printf("yaw %f\r\r\n", imu->yaw());
-
 }
 
-void Wheelchair::forward()
+void Wheelchair::forward()                                                              // In auto to move foward
 {
     x->write(high);
     y->write(def+offset);
-  //  out->printf("distance %f\r\n", wheel_right.getDistance(37.5));
 }
 
-void Wheelchair::backward()
+void Wheelchair::backward()                                                             // In auto to move reverse
 {
     x->write(low);
     y->write(def);
 }
 
-void Wheelchair::right()
+void Wheelchair::right()                                                                // In auto to move right
 {
     x->write(def);
     y->write(low);
 }
 
-void Wheelchair::left()
+void Wheelchair::left()                                                                 // In auto to move left
 {
     x->write(def);
     y->write(high);
 }
 
-void Wheelchair::stop()
+void Wheelchair::stop()                                                                 // Stops the chair
 {
     x->write(def);
     y->write(def);
 }
 // counter clockwise is -
 // clockwise is +
-void Wheelchair::pid_right(int deg) 
+void Wheelchair::pid_right(int deg)                                                     // Takes in degree and turns right
 {
-    bool overturn = false;
+    bool overturn = false;                                                              //Boolean if we have to turn over relative 360˚
     
-    out->printf("pid right\r\r\n");
-    x->write(def);
-    Setpoint = curr_yaw + deg;
-    pid_yaw = curr_yaw;
-    if(Setpoint > 360) {
-     //   Setpoint -= 360;
+    out->printf("pid right\r\r\n");                                
+    x->write(def);                                                                      // Not moving fowards or reverse
+    Setpoint = curr_yaw + deg;                                                          // Relative angle we want to turn
+    pid_yaw = curr_yaw;                                                                 // Sets input to current angle(pid_yaw = input)
+    
+    if(Setpoint > 360) {                                                                //Turns on overturn boolean if setpoint over 360˚
         overturn = true;
     }
-    myPID.SetTunings(5.5,0, 0.0035);
-    myPID.SetOutputLimits(0, def-low-.15);
-    myPID.SetControllerDirection(DIRECT);
-    while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
-        if(overturn && curr_yaw < Setpoint-deg-1)
+    
+    myPID.SetTunings(5.5,0, 0.0035);                                                    // Sets the constants for P and D
+    myPID.SetOutputLimits(0, def-low-.15);                                              // Limits to the differnce between def and low
+    myPID.SetControllerDirection(DIRECT);                                               // PID mode Direct
+    
+    while(pid_yaw < Setpoint - 3){                                                      // Tells PID to stop when reaching
+                                                                                        // a little less than desired angle
+        if(overturn && curr_yaw < Setpoint-deg-1)                                       // Sets PID yaw to coterminal angle if necesary
         {
             pid_yaw = curr_yaw + 360;
         }   
         else
             pid_yaw = curr_yaw;
-        myPID.Compute();
-        double tempor = -Output+def;
-        y->write(tempor);
-        out->printf("curr_yaw %f\r\r\n", curr_yaw);
+            
+        myPID.Compute();                                                                // Does PID calculations
+        double tempor = -Output+def;                                                    // Temporary value with the voltage output
+        y->write(tempor);                                                               // Sends to chair y output command
+        
+        out->printf("curr_yaw %f\r\r\n", curr_yaw);                              
         out->printf("Setpoint = %f \r\n", Setpoint);
 
-        wait(.05);
+        wait(.05);                                                                      // Small delay
         }
-    Wheelchair::stop();
+    Wheelchair::stop();                                                                 // Safety Stop
     out->printf("done \r\n");
-    }
+}
 
-void Wheelchair::pid_left(int deg) 
+void Wheelchair::pid_left(int deg)                                                      // Takes in degree and turns left
 {
-    bool overturn = false;
+    bool overturn = false;                                                              //Boolean if we have to turn under relative 0˚
     
-    out->printf("pid Left\r\r\n");
-    x->write(def);
-    Setpoint = curr_yaw - deg;
-    pid_yaw = curr_yaw;
-    if(Setpoint < 0) {
-   //     Setpoint += 360;
+    out->printf("pid Left\r\r\n");                                                     
+    x->write(def);                                                                      // Not moving fowards or reverse
+    Setpoint = curr_yaw - deg;                                                          // Relative angle we want to turn
+    pid_yaw = curr_yaw;                                                                 // Sets input to current angle(pid_yaw = input)
+    if(Setpoint < 0) {                                                                  //Turns on overturn boolean if setpoint under 0˚
         overturn = true;
     }
-    myPID.SetTunings(5,0, 0.004);
-    myPID.SetOutputLimits(0,high-def-.12);
-    myPID.SetControllerDirection(REVERSE);
-    while(pid_yaw > Setpoint+3){//pid_yaw < Setpoint + 2) {
-       myPID.Compute();
-       if(overturn && curr_yaw > Setpoint+deg+1)
+    myPID.SetTunings(5,0, 0.004);                                                       // Sets the constants for P and D
+    myPID.SetOutputLimits(0,high-def-.12);                                              // Limits to the differnce between High and Def
+    myPID.SetControllerDirection(REVERSE);                                              // PID mode Reverse
+    while(pid_yaw > Setpoint+3){                                                        // Tells PID to stop when reaching
+                                                                                        // a little more than desired angle
+       if(overturn && curr_yaw > Setpoint+deg+1)                                        // Sets PID yaw to coterminal angle if necesary
        {
           pid_yaw = curr_yaw - 360;
         }   
         else
             pid_yaw = curr_yaw;
-       double tempor = Output+def;
-
-        y->write(tempor);
+            
+        myPID.Compute();                                                                // Does PID calculations
+        double tempor = Output+def;                                                     // Temporary value with the voltage output
+        y->write(tempor);                                                               // Sends to chair y output command
+        
         out->printf("curr_yaw %f\r\n", curr_yaw);
-        wait(.05);
+        wait(.05);                                                                      // Small Delay
         }
-        Wheelchair::stop();
-    }
+    Wheelchair::stop();                                                                 // Safety Stop
+}
 
-void Wheelchair::pid_turn(int deg) {
-    if(deg > 180) {
+void Wheelchair::pid_turn(int deg) {                                                    // Determine wether we are turn right or left
+
+    if(deg > 180) {                                                                     // If deg > 180 turn left: coterminal angle
         deg -= 360;
     }
 
-    else if(deg < -180) {
+    else if(deg < -180) {                                                               // If deg < -180 turn right: coterminal angle
         deg+=360;
     }  
     
-    int turnAmt = abs(deg);
-    ti->reset();
+    int turnAmt = abs(deg);                                                             // Makes sure input angle is positive
 
     if(deg >= 0){
-        Wheelchair::pid_right(turnAmt);
-        }
+        Wheelchair::pid_right(turnAmt);                                                 // Calls PID right if positive degree
+    }
     else {
-        Wheelchair::pid_left(turnAmt);
-        }
+        Wheelchair::pid_left(turnAmt);                                                  // Calls PID left if negative degree
     }
+}
 void Wheelchair::pid_forward(double mm)
 {
-    mm -= 20;
-    Input = 0;
-    wheel->reset();
+    mm -= 20;                                                                           // Makes sure distance does not overshoot
+    Input = 0;                                                                          // Initializes imput to cero: Test latter w/o
+    wheel->reset();                                                                     // Resets encoders so that they start at 0
     out->printf("pid foward\r\n");
 
-    double tempor;
-    Setpoint = mm;
+    double tempor;                                                                      // Initializes Temporary variable for x input
+    Setpoint = mm;                                                                      // Initializes the setpoint to desired value
 
-  //  Setpoint = wheel_right.getDistance(37.5)+mm;
-    myPIDDistance.SetTunings(5,0, 0.004);
-    myPIDDistance.SetOutputLimits(0,high-def-.15);
-    myPIDDistance.SetControllerDirection(DIRECT);
-    y->write(def+offset);
-    while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
-        if(out->readable())
+    myPIDDistance.SetTunings(5.5,0, 0.0015);                                            // Sets constants for P and D
+    myPIDDistance.SetOutputLimits(0,high-def-.15);                                      // Limits to the differnce between High and Def 
+    myPIDDistance.SetControllerDirection(DIRECT);                                       // PID to Direct
+    y->write(def+offset);                                                               // Sets chair to not turn
+    
+    while(Input < Setpoint){                                                            // Stop moving when reaching setpoint
+    
+        if(out->readable())                                                             // Emergency Break
             break;
-        Input = wheel->getDistance(53.975);
-        //out->printf("input foward %d\r\n", wheel->getPulses());
-        wait(.05);
-        myPIDDistance.Compute();
+            
+        Input = wheel->getDistance(53.975);                                             // Gets Distance from Encoder onto PID
+        wait(.05);                                                                      // Slight Delay: *****Test without
+        myPIDDistance.Compute();                                                        // Compute Output for chair
 
-        tempor = Output + def;
-        x->write(tempor);
+        tempor = Output + def;                                                          // Temporary output variable                    
+        x->write(tempor);                                                               // Sends to chair x output
         out->printf("distance %f\r\n", Input);
         }
-        
     
 }   
 void Wheelchair::pid_reverse(double mm)
 {
-   /* qei.putc('r');
-    out->printf("pid reverse\r\n");
 
-    double tempor;
-    Setpoint2 = mm;
-
-  //  Setpoint = wheel_right.getDistance(37.5)+mm;
-    myPIDDistance.SetTunings(5,0, 0.004);
-    myPIDDistance.SetOutputLimits(0,def);
-    myPIDDistance.SetControllerDirection(REVERSE);
-    y->write(def);
-    while(encoder_distance > Setpoint2+5){//pid_yaw < Setpoint + 2) {
-        int i;
-        qei.putc('h');
-        //qei.gets(myString, 10);
-        ti->reset();
-
-        for (i=0; myString[i-1] != '\n'; i++) {
-            while (true) {
-                //pc.printf("%f\r\n", ti.read());
-                if (ti->read() > .02) break;
-                if (qei.readable()) {
-                    myString[i]= qei.getc();
-                    break;
-                    }
-                }
-        }            
-        myString[i-1] = 0;
-        double tempor = atof(myString);
-        out->printf("displacement = %f\r\n", tempor);
-        if(abs(tempor - encoder_distance) < 500)
-        {
-            encoder_distance = tempor;
-            out->printf("this is fine\r\n");
-        }
-        for(i = 0; i < 64; i++)
-        {
-            myString[i] = 0;
-        } 
-        Input = encoder_distance;
-        out->printf("input foward %f\r\n", Input);
-        wait(.1);
-        myPIDDistance.Compute();
-        
-        // get value from encoder2
-        qei.putc('k');
-        ti->reset();
-
-        for (i=0; myString[i-1] != '\n'; i++) {
-            while (true) {
-                //pc.printf("%f\r\n", ti.read());
-                if (ti->read() > .02) break;
-                if (qei.readable()) {
-                    myString[i]= qei.getc();
-                    break;
-                    }
-                }
-        }            
-        myString[i-1] = 0;
-        double tempor2 = atof(myString);
-        out->printf("displacement = %f\r\n", tempor2);
-        
-        if(abs(tempor - encoder_distance) < 500)
-        {
-            encoder_distance = tempor;
-            out->printf("this is fine\r\n");
-        }        
-        if(abs(tempor2 - encoder_distance2) < 500)
-        {
-            encoder_distance2 = tempor2;
-            out->printf("this is fine\r\n");
-        }
-        for(i = 0; i < 64; i++)
-        {
-            myString[i] = 0;
-        } 
-        tempor = Output;
-        x->write(tempor);
-        out->printf("distance %f\r\n", encoder_distance);
-        }*/
 }   
-double Wheelchair::turn_right(int deg)
-{
-    bool overturn = false;
-    out->printf("turning right\r\n");
-
-    double start = curr_yaw;
-    double final = start + deg;
-
-    if(final > 360) {
-        final -= 360;
-        overturn = true;
-    }
-
-    out->printf("start %f, final %f\r\n", start, final);
-
-    double curr = -1;
-    while(curr <= final - 30) {
-        Wheelchair::right();
-        if( out->readable()) {
-            out->printf("stopped\r\n");
-            Wheelchair::stop();
-            return;
-        }
-        curr = curr_yaw;
-        if(overturn && curr > (360 - deg) ) {
-            curr = 0;
-        }
-    }
-    
-    out->printf("done turning start %f final %f\r\n", start, final);
-    Wheelchair::stop();
-    
-     //delete me
-    wait(5);
-    
-    float correction = final - curr_yaw;
-    out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
-    Wheelchair::turn_left(abs(correction));
-    Wheelchair::stop();
-    
-    wait(5);
-    out->printf("curr_yaw %f\r\n", curr_yaw);
-    return final;
-}
-
-double Wheelchair::turn_left(int deg)
-{
-    bool overturn = false;
-    out->printf("turning left\r\n");
-
-    double start = curr_yaw;
-    double final = start - deg;
-
-    if(final < 0) {
-        final += 360;
-        overturn = true;
-    }
-
-    out->printf("start %f, final %f\r\n", start, final);
-
-    double curr = 361;
-    while(curr >= final) {
-        Wheelchair::left();
-        if( out->readable()) {
-            out->printf("stopped\r\n");
-            Wheelchair::stop();
-            return;
-        }
-        curr = curr_yaw;
-
-        if(overturn && curr >= 0 && curr <= start ) {
-            curr = 361;
-        }
-    }
-
-    out->printf("done turning start %f final %f\r\n", start, final);
-    Wheelchair::stop();
-    
-    //delete me
-    wait(2);
-    /*
-    float correction = final - curr_yaw;
-    out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
-    Wheelchair::turn_right(abs(correction));
-    Wheelchair::stop();
-*/
-    return final;
-}
-
-void Wheelchair::turn(int deg)
-{
-    if(deg > 180) {
-        deg -= 360;
-    }
-
-    else if(deg < -180) {
-        deg+=360;
-    }  
-    
-    double finalpos;
-    int turnAmt = abs(deg);
-    //ti->reset();
-    /*
-    if(deg >= 0){
-        finalpos = Wheelchair::turn_right(turnAmt);
-        }
-    else {
-        finalpos = Wheelchair::turn_left(turnAmt);
-        }
-    */
-    wait(2);
-    
-    float correction = finalpos - curr_yaw;
-    out->printf("final pos %f actual pos %f\r\n", finalpos, curr_yaw);
-    
-    
-    //if(abs(correction) > turn_precision) {
-        out->printf("correcting %f\r\n", correction);
-        //ti->reset();
-        Wheelchair::turn_left(curr_yaw - finalpos);
-        return;
-        //} 
-    
-}
 
 float Wheelchair::getDistance() {
     return wheel->getDistance(Diameter);
@@ -417,6 +205,7 @@
 void Wheelchair::resetDistance(){
     wheel->reset();
     }
+/*Predetermined paths For Demmo*/    
 void Wheelchair::desk() {
     Wheelchair::pid_forward(5461);
     Wheelchair::pid_right(87);

--- a/wheelchair.h	Fri Aug 31 20:00:01 2018 +0000
+++ b/wheelchair.h	Tue Oct 16 23:03:40 2018 +0000
@@ -1,37 +1,30 @@
 #ifndef wheelchair
 #define wheelchair
 
+//Importing libraries into wheelchair.h
 #include "chair_BNO055.h"
 #include "PID.h"
 #include "QEI.h"
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
-//#include "BufferedSerial.h"
 
-//#include "chair_MPU9250.h"
-
-#define turn_precision 10
-#define def (2.5f/3.3f)
-#define high 3.3f/3.3f
-#define offset .02742f
-#define low (1.7f/3.3f)
-#define process .1
 
-/* for big mbed board
-#define xDir D12 //top right two pins
-#define yDir D13 //top left two pins
-#define Encoder1 D0
-#define Encoder2 D1
-*/
+/*
+* joystick has analog out of 200-700, scale values between 1.3 and 3.3
+*/             
+#define def (2.5f/3.3f)                 //Default axis on joystick to stay neutral; used on x and y axis
+#define high 3.3f/3.3f                  //High power on joystick; used on x and y axis
+#define low (1.7f/3.3f)                 //Low power on joystick; used on x and y axis
+#define offset .035f                  //Joystick ajustment to be able to go strait. Chair dependent on manufactoring presision
+#define process .1                      //Defines default time delay in seconds
 
-//for small mbed board
-#define xDir D9
+//Pin plug in for Nucleo-L432KC
+#define xDir D9                         //PWM Pins
 #define yDir D10
-#define Encoder1 D7
+#define Encoder1 D7                     //Digital In Pull Up Pin
 #define Encoder2 D8
 
-#define EncoderReadRate 1200
-#define Diameter 31.75
+#define Diameter 31.75                  //Diameter of encoder wheel
 /** Wheelchair class
  * Used for controlling the smart wheelchair
  */
@@ -47,21 +40,12 @@
     /** 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();
     
@@ -79,23 +63,35 @@
     
     /* function to get imu data*/
     void compass_thread();
-    void distance_thread();
+    
+    /* move x millimiters foward using PID*/
     void pid_forward(double mm);
+    
+    /* move x millimiters reverse using PID*/
     void pid_reverse(double mm);
+    
+    /*  gets the encoder distance moved since encoder reset*/
     float getDistance();
+    
+    /* resets encoder*/
     void resetDistance();
+    
+    /* function to to determine whether we are turning left or right*/
     void pid_turn(int deg);
+    
+    /* functions with a predetermined path demmo*/
     void desk();
     void kitchen();
     void desk_to_kitchen();
 private:
+    /* Pointers for the joystick speed*/
     PwmOut* x;
     PwmOut* y;
-    chair_BNO055* imu;
-    Serial* out;
-    Timer* tim;
-    Timer* ti;
-    QEI* wheel;
+    
+    chair_BNO055* imu;                  // Pointer to IMU
+    Serial* out;                        // Pointer to Serial Monitor
+    Timer* ti;                          // Pointer to the timer
+    QEI* wheel;                         // Pointer to encoder
 
 };
 #endif
\ No newline at end of file