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