Isabella Gomez Torres
/
TEST_SideToF
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 Watchdog VL53L1X_Filter ros_lib_kinetic
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 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 WheelchairRadius 95 //distance from center of rotation to edge of wheelchair 00042 #define maxAngularDeceleration 1.04 //found through testing, max 00043 //acceleration at which chair can 00044 //stop while turning. In rads per sec 00045 #define minWallLength 100 // minimum distance from wall to ToF (mm) 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, Timer* imuT, 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 backwardSafety;//Check if can move backward 00218 bool leftSafety; //to check if can turn left 00219 bool rightSafety; //to check if can turn right 00220 double curr_yaw, curr_velS; // Variable that contains current relative angle 00221 00222 private: 00223 int runningAverage[4]; 00224 // Expected data used to compare whether of not there is a ledge. This serves as a ground base 00225 // Array is used for calibrating the time of flight sensors 00226 // Used to calculate stdev and mean on 00227 00228 00229 00230 /* Pointers for the joystick speed */ 00231 PwmOut* x; 00232 PwmOut* y; 00233 00234 //Pointers for PCB 00235 PwmOut* on; 00236 PwmOut* off; 00237 00238 DigitalIn* e_button; //Pointer to e_button 00239 00240 IMUWheelchair* imu; // Pointer to IMU 00241 Serial* out; // Pointer to Serial Monitor 00242 Timer* ti; // Pointer to the timer 00243 QEI* wheel; // Pointer to encoder 00244 QEI* wheelS; // Pointer to encoder 00245 VL53L1X** ToF; // Arrays of pointers to ToF sensors 00246 00247 00248 00249 00250 }; 00251 #endif
Generated on Sat Jul 16 2022 14:27:07 by
1.7.2