wheelchair.h

00001 #ifndef wheelchair
00002 #define wheelchair
00003 /*************************************************************************
00004 *            Importing libraries into wheelchair.h                       *
00005 **************************************************************************/
00006 #include "chair_BNO055.h"
00007 #include "PID.h"
00008 #include "QEI.h"
00009 #include "VL53L1X.h"
00010 #include "statistics.h"
00011 #include "Watchdog.h"
00012  
00013 #include <ros.h>
00014 #include <geometry_msgs/Twist.h>
00015 #include <nav_msgs/Odometry.h>
00016  
00017 /*************************************************************************
00018 * Joystick has analog out of 200-700, scale values between 1.3 and 3.3   *
00019 * Here are some global constants for joystick                            *
00020 **************************************************************************/
00021 #define def (2.5f/3.3f)                 // Default axis on joystick to stay neutral; used on x and y axis
00022 #define high 3.3f/3.3f                  // High power on joystick; used on x and y axis
00023 #define low (1.7f/3.3f)                 // Low power on joystick; used on x and y axis
00024 #define offset .03f                     // Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
00025 #define process .1                      // Defines default time delay in seconds
00026 /*************************************************************************
00027 *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)*
00028 **************************************************************************/
00029 #define xDir PA_6                       // PWM Pins 
00030 #define yDir PA_5
00031 #define Encoder1 D7                     // Digital In Pull Up Pin 
00032 #define Encoder2 D8
00033 #define Diameter 31.75                  // Diameter of encoder wheel
00034 #define maxDecelerationSlow 120
00035 #define maxDecelerationFast 30
00036 #define ToFSensorNum 12
00037  
00038 /*************************************************************************
00039 *IMU definitions for turning wheelchair
00040 **************************************************************************/
00041 #define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm)
00042 #define  MAX_ANGULAR_DECELERATION 60 //found through testing, max 
00043                                      //acceleration at which chair can 
00044                                      //stop while turning. In degree per sec
00045 #define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm)
00046 /*************************************************************************
00047 *                                                                        *
00048 *                         Wheelchair class                               *
00049 *           Used for controlling the Smart Wheelchair                    *
00050 *                                                                        *
00051 **************************************************************************/
00052 class Wheelchair
00053 {
00054 public:
00055     /*************************************************************************
00056     *   Create Wheelchair constructor with x,y pin for analog-to-dc output   *
00057     *   serial for printout, and time. This is also used to initialize some  *
00058     *   variables for the timer,encoders and time-of-flight sensors          *
00059     **************************************************************************/
00060     Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
00061                VL53L1X** ToF);
00062  
00063     /*************************************************************************
00064     *           This method is to move using the joystick                    *
00065     **************************************************************************/
00066     void move(float x_coor, float y_coor);
00067  
00068     /*************************************************************************
00069     *   This method is to drive the wheelchair forward manually (NO PID used)*
00070     ************************************************************************ */
00071     void forward();
00072     /*************************************************************************
00073     *   This method is to drive the wheelchair backwards (NO PID used)       *
00074     ************************************************************************ */
00075     void backward();
00076  
00077     /*************************************************************************
00078     *   This method is to turn the wheelchair right manually (NO PID used)   *
00079     ************************************************************************ */
00080     void right();
00081  
00082     /*************************************************************************
00083     *   This method is to turn the wheelchair left manually (NO PID used)    *
00084     ************************************************************************ */
00085     void left();
00086  
00087     /*************************************************************************
00088     *   This method stops the wheelchair and reset the joystick position     *
00089     ************************************************************************ */
00090     void stop();
00091  
00092     /*************************************************************************
00093     *   This method is a thread that will obtain the IMU information such    *
00094     *   as the gyroscope x,y axis. Z-axis is not used.                       *
00095     ************************************************************************ */
00096     void compass_thread();
00097  
00098     /*************************************************************************
00099     *   This method is a thread that will calculate the velocity of the      *
00100     *   wheechair using the encoder values this is being obatined.           *
00101     ************************************************************************ */
00102     void velocity_thread();
00103  
00104     //not being used
00105     void rosCom_thread();
00106  
00107     /*************************************************************************
00108     *   This method is a thread that iterates through all the sensor's       *
00109     *   values and determines whether or not the wheelchair is about to hit  *
00110     *   or crash into something. If the sensors detect something close to    *
00111     *   the chair, then the chair will safely halt and allow movement in the *
00112     *   direction opposed to where an object is detected.                    *
00113     ************************************************************************ */
00114     void ToFSafe_thread();
00115  
00116     /*************************************************************************
00117     *   This method is a thread that will constantly be checking the value   *
00118     *   of the emergency button. If the button is pressed, then the chair    *
00119     *   will stop and the entire system will reset.                          *
00120     ************************************************************************ */
00121     void emergencyButton_thread();
00122  
00123     /*************************************************************************
00124     *   This method gets the encoder values and calculates the distance since*
00125     *   the last encoder reset.                                              *
00126     **************************************************************************/
00127     float getDistance();
00128  
00129     /*************************************************************************
00130     *   This method resets the encoder value to recalculate distance         *
00131     **************************************************************************/
00132     void resetDistance();
00133  
00134  
00135     /*************************************************************************
00136      *                                                                       *
00137      *                         PID Control Methods                           *
00138      *                                                                       *
00139      *************************************************************************/
00140  
00141  
00142     /*************************************************************************
00143     * This method moves the wheelchair x-millimiters forward using PID       *
00144     **************************************************************************/
00145     void pid_forward(double mm);
00146  
00147     /*************************************************************************
00148     * This method turns the chair right a certain amount of degrees using PID*
00149     **************************************************************************/
00150     void pid_right(int deg);
00151  
00152     /*************************************************************************
00153     * This method turns the chair left a certain amount of degrees using PID *
00154     **************************************************************************/
00155     void pid_left(int deg);
00156  
00157     /*************************************************************************
00158     * This method determines whether to turn the wheelchair left or right    *
00159     **************************************************************************/
00160     void pid_turn(int deg);
00161  
00162  
00163     /*************************************************************************
00164      *                                                                       *
00165      *                     ROS-Embed Communication Methods                   *
00166      *                                                                       *
00167      *************************************************************************/
00168  
00169     /*************************************************************************
00170      * This method computes the relative angle for Twist message in ROS      *
00171      *************************************************************************/
00172     void pid_twistA();
00173     
00174     /*************************************************************************
00175      * This method computes the relative velocity for Twist message in ROS   *
00176      *************************************************************************/
00177     void pid_twistV();
00178     
00179     /*************************************************************************
00180      * This method computes the angular position for Twist message in ROS    *
00181      *************************************************************************/
00182     double getTwistZ();
00183     
00184     /*************************************************************************
00185      * This method calculates the relative position of the chair everytime   * 
00186      * the encoders reset by setting its old position as the origin to       *
00187      * calculate the new position. Moreover, this function is important to   *
00188      * be able to publish (send) the information to ROS                      *
00189      *************************************************************************/
00190     void odomMsg();
00191     
00192     /*************************************************************************
00193      * This method prints the Odometry message to the serial monitor         *
00194      *************************************************************************/
00195     void showOdom();
00196  
00197     /*************************************************************************
00198      * (not being used) Functions with a predetermined path (demo)           *
00199      *************************************************************************/
00200     void desk();
00201     void kitchen();
00202     void desk_to_kitchen();
00203  
00204     /*************************************************************************
00205     *                         Public Variables                               *       
00206     **************************************************************************/ 
00207     double x_position;
00208     double y_position;
00209     double z_angular;
00210     double curr_vel;
00211     double z_twistA;
00212     double linearV;
00213     double angularV;
00214     double vel;
00215     double test1, test2;
00216     bool forwardSafety;
00217     bool sideSafety; //to check if can turn
00218     double curr_yaw, curr_velS;                                                            // Variable that contains current relative angle
00219  
00220 private:
00221     int runningAverage[4];           
00222     // Expected data used to compare whether of not there is a ledge. This serves as a ground base
00223     // Array is used for calibrating the time of flight sensors  
00224     // Used to calculate stdev and mean on 
00225  
00226  
00227  
00228     /* Pointers for the joystick speed */
00229     PwmOut* x;
00230     PwmOut* y;
00231  
00232     //Pointers for PCB
00233     PwmOut* on;
00234     PwmOut* off;
00235  
00236     DigitalIn* e_button;                //Pointer to e_button
00237  
00238     chair_BNO055* imu;                  // Pointer to IMU
00239     Serial* out;                        // Pointer to Serial Monitor
00240     Timer* ti;                          // Pointer to the timer
00241     QEI* wheel;                         // Pointer to encoder
00242     QEI* wheelS;                        // Pointer to encoder
00243     VL53L1X** ToF;                      // Arrays of pointers to ToF sensors
00244     
00245     
00246  
00247  
00248 };
00249 #endif