Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
wheelchair.h@29:5714715ab1f5, 2019-07-05 (annotated)
- Committer:
- JesiMiranda
- Date:
- Fri Jul 05 19:38:55 2019 +0000
- Revision:
- 29:5714715ab1f5
- Parent:
- 26:c842070aa0b8
- Child:
- 31:9283c8cd5362
no changes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:fc0c4a184482 | 1 | #ifndef wheelchair |
ryanlin97 | 0:fc0c4a184482 | 2 | #define wheelchair |
JesiMiranda | 29:5714715ab1f5 | 3 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 4 | * Importing libraries into wheelchair.h * |
JesiMiranda | 29:5714715ab1f5 | 5 | **************************************************************************/ |
ryanlin97 | 11:d14a1f7f1297 | 6 | #include "chair_BNO055.h" |
jvfausto | 17:7f3b69300bb6 | 7 | #include "PID.h" |
ryanlin97 | 12:921488918749 | 8 | #include "QEI.h" |
jvfausto | 21:3489cffad196 | 9 | #include "VL53L1X.h" |
jvfausto | 21:3489cffad196 | 10 | |
ryanlin97 | 16:b403082eeacd | 11 | #include <ros.h> |
ryanlin97 | 16:b403082eeacd | 12 | #include <geometry_msgs/Twist.h> |
jvfausto | 21:3489cffad196 | 13 | #include <nav_msgs/Odometry.h> |
JesiMiranda | 29:5714715ab1f5 | 14 | #include <std_msgs/Float64.h> |
JesiMiranda | 29:5714715ab1f5 | 15 | #include <std_msgs/Float64MultiArray.h> |
ryanlin97 | 0:fc0c4a184482 | 16 | |
JesiMiranda | 29:5714715ab1f5 | 17 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 18 | * Joystick has analog out of 200-700, scale values between 1.3 and 3.3 * |
JesiMiranda | 29:5714715ab1f5 | 19 | * Here are some global constants for joystick * |
JesiMiranda | 29:5714715ab1f5 | 20 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 21 | #define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis |
jvfausto | 21:3489cffad196 | 22 | #define high 3.3f/3.3f //High power on joystick; used on x and y axis |
jvfausto | 21:3489cffad196 | 23 | #define low (1.7f/3.3f) //Low power on joystick; used on x and y axis |
jvfausto | 26:c842070aa0b8 | 24 | #define offset .03f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision |
jvfausto | 21:3489cffad196 | 25 | #define process .1 //Defines default time delay in seconds |
JesiMiranda | 29:5714715ab1f5 | 26 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 27 | *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)* |
JesiMiranda | 29:5714715ab1f5 | 28 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 29 | #define xDir PA_6 //* PWM Pins */ |
jvfausto | 21:3489cffad196 | 30 | #define yDir PA_5 |
jvfausto | 21:3489cffad196 | 31 | #define Encoder1 D7 //*Digital In Pull Up Pin */ |
jvfausto | 21:3489cffad196 | 32 | #define Encoder2 D8 |
jvfausto | 21:3489cffad196 | 33 | #define Diameter 31.75 //Diameter of encoder wheel |
JesiMiranda | 29:5714715ab1f5 | 34 | #define maxDecelerationSlow 120 |
jvfausto | 26:c842070aa0b8 | 35 | #define maxDecelerationFast 30 |
jvfausto | 21:3489cffad196 | 36 | #define ToFSensorNum 12 |
JesiMiranda | 29:5714715ab1f5 | 37 | #define USE_USBCON |
jvfausto | 21:3489cffad196 | 38 | |
JesiMiranda | 29:5714715ab1f5 | 39 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 40 | * * |
JesiMiranda | 29:5714715ab1f5 | 41 | * Wheelchair class * |
JesiMiranda | 29:5714715ab1f5 | 42 | * Used for controlling the Smart Wheelchair * |
JesiMiranda | 29:5714715ab1f5 | 43 | * * |
JesiMiranda | 29:5714715ab1f5 | 44 | **************************************************************************/ |
ryanlin97 | 0:fc0c4a184482 | 45 | class Wheelchair |
ryanlin97 | 0:fc0c4a184482 | 46 | { |
ryanlin97 | 0:fc0c4a184482 | 47 | public: |
JesiMiranda | 29:5714715ab1f5 | 48 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 49 | * Create Wheelchair constructor with x,y pin for analog-to-dc output * |
JesiMiranda | 29:5714715ab1f5 | 50 | * serial for printout, and time. This is also used to initialize some * |
JesiMiranda | 29:5714715ab1f5 | 51 | * variables for the timer,encoders and time-of-flight sensors * |
JesiMiranda | 29:5714715ab1f5 | 52 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 53 | Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS, |
JesiMiranda | 29:5714715ab1f5 | 54 | VL53L1X** ToF); |
JesiMiranda | 29:5714715ab1f5 | 55 | |
JesiMiranda | 29:5714715ab1f5 | 56 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 57 | * This method is to move using the joystick * |
JesiMiranda | 29:5714715ab1f5 | 58 | **************************************************************************/ |
ryanlin97 | 3:a5e71bfdb492 | 59 | void move(float x_coor, float y_coor); |
JesiMiranda | 29:5714715ab1f5 | 60 | |
JesiMiranda | 29:5714715ab1f5 | 61 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 62 | * This method is to drive the wheelchair forward manually (NO PID used)* |
JesiMiranda | 29:5714715ab1f5 | 63 | ************************************************************************ */ |
ryanlin97 | 1:c0beadca1617 | 64 | void forward(); |
JesiMiranda | 29:5714715ab1f5 | 65 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 66 | * This method is to drive the wheelchair backwards (NO PID used) * |
JesiMiranda | 29:5714715ab1f5 | 67 | ************************************************************************ */ |
ryanlin97 | 1:c0beadca1617 | 68 | void backward(); |
JesiMiranda | 29:5714715ab1f5 | 69 | |
JesiMiranda | 29:5714715ab1f5 | 70 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 71 | * This method is to turn the wheelchair right manually (NO PID used) * |
JesiMiranda | 29:5714715ab1f5 | 72 | ************************************************************************ */ |
ryanlin97 | 1:c0beadca1617 | 73 | void right(); |
JesiMiranda | 29:5714715ab1f5 | 74 | |
JesiMiranda | 29:5714715ab1f5 | 75 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 76 | * This method is to turn the wheelchair left manually (NO PID used) * |
JesiMiranda | 29:5714715ab1f5 | 77 | ************************************************************************ */ |
ryanlin97 | 1:c0beadca1617 | 78 | void left(); |
JesiMiranda | 29:5714715ab1f5 | 79 | |
JesiMiranda | 29:5714715ab1f5 | 80 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 81 | * This method stops the wheelchair and reset the joystick position * |
JesiMiranda | 29:5714715ab1f5 | 82 | ************************************************************************ */ |
ryanlin97 | 1:c0beadca1617 | 83 | void stop(); |
JesiMiranda | 29:5714715ab1f5 | 84 | |
JesiMiranda | 29:5714715ab1f5 | 85 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 86 | * This method is a thread that will obtain the IMU information such * |
JesiMiranda | 29:5714715ab1f5 | 87 | * as the gyroscope x,y axis. Z-axis is not used. * |
JesiMiranda | 29:5714715ab1f5 | 88 | ************************************************************************ */ |
ryanlin97 | 11:d14a1f7f1297 | 89 | void compass_thread(); |
JesiMiranda | 29:5714715ab1f5 | 90 | |
JesiMiranda | 29:5714715ab1f5 | 91 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 92 | * This method is a thread that will calculate the velocity of the * |
JesiMiranda | 29:5714715ab1f5 | 93 | * wheechair using the encoder values this is being obatined. * |
JesiMiranda | 29:5714715ab1f5 | 94 | ************************************************************************ */ |
jvfausto | 21:3489cffad196 | 95 | void velocity_thread(); |
JesiMiranda | 29:5714715ab1f5 | 96 | |
JesiMiranda | 29:5714715ab1f5 | 97 | //not being used |
jvfausto | 21:3489cffad196 | 98 | void rosCom_thread(); |
JesiMiranda | 29:5714715ab1f5 | 99 | //void tof_encoder_roscom_thread(); |
JesiMiranda | 29:5714715ab1f5 | 100 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 101 | * This method is a thread that iterates through all the sensor's * |
JesiMiranda | 29:5714715ab1f5 | 102 | * values and determines whether or not the wheelchair is about to hit * |
JesiMiranda | 29:5714715ab1f5 | 103 | * or crash into something. If the sensors detect something close to * |
JesiMiranda | 29:5714715ab1f5 | 104 | * the chair, then the chair will safely halt and allow movement in the * |
JesiMiranda | 29:5714715ab1f5 | 105 | * direction opposed to where an object is detected. * |
JesiMiranda | 29:5714715ab1f5 | 106 | ************************************************************************ */ |
jvfausto | 21:3489cffad196 | 107 | void assistSafe_thread(); |
jvfausto | 21:3489cffad196 | 108 | |
JesiMiranda | 29:5714715ab1f5 | 109 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 110 | * This method is a thread that will constantly be checking the value * |
JesiMiranda | 29:5714715ab1f5 | 111 | * of the emergency button. If the button is pressed, then the chair * |
JesiMiranda | 29:5714715ab1f5 | 112 | * will stop and the entire system will reset. * |
JesiMiranda | 29:5714715ab1f5 | 113 | ************************************************************************ */ |
JesiMiranda | 29:5714715ab1f5 | 114 | void emergencyButton_thread(); |
JesiMiranda | 29:5714715ab1f5 | 115 | |
JesiMiranda | 29:5714715ab1f5 | 116 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 117 | * This method gets the encoder values and calculates the distance since* |
JesiMiranda | 29:5714715ab1f5 | 118 | * the last encoder reset. * |
JesiMiranda | 29:5714715ab1f5 | 119 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 120 | float getDistance(); |
JesiMiranda | 29:5714715ab1f5 | 121 | |
JesiMiranda | 29:5714715ab1f5 | 122 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 123 | * This method resets the encoder value to recalculate distance * |
JesiMiranda | 29:5714715ab1f5 | 124 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 125 | void resetDistance(); |
JesiMiranda | 29:5714715ab1f5 | 126 | |
JesiMiranda | 29:5714715ab1f5 | 127 | |
JesiMiranda | 29:5714715ab1f5 | 128 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 129 | * * |
JesiMiranda | 29:5714715ab1f5 | 130 | * PID Control Methods * |
JesiMiranda | 29:5714715ab1f5 | 131 | * * |
JesiMiranda | 29:5714715ab1f5 | 132 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 133 | |
JesiMiranda | 29:5714715ab1f5 | 134 | |
JesiMiranda | 29:5714715ab1f5 | 135 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 136 | * This method moves the wheelchair x-millimiters forward using PID * |
JesiMiranda | 29:5714715ab1f5 | 137 | **************************************************************************/ |
jvfausto | 19:71a6621ee5c3 | 138 | void pid_forward(double mm); |
JesiMiranda | 29:5714715ab1f5 | 139 | |
JesiMiranda | 29:5714715ab1f5 | 140 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 141 | * This method turns the chair right a certain amount of degrees using PID* |
JesiMiranda | 29:5714715ab1f5 | 142 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 143 | void pid_right(int deg); |
JesiMiranda | 29:5714715ab1f5 | 144 | |
JesiMiranda | 29:5714715ab1f5 | 145 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 146 | * This method turns the chair left a certain amount of degrees using PID * |
JesiMiranda | 29:5714715ab1f5 | 147 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 148 | void pid_left(int deg); |
JesiMiranda | 29:5714715ab1f5 | 149 | |
JesiMiranda | 29:5714715ab1f5 | 150 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 151 | * This method determines whether to turn the wheelchair left or right * |
JesiMiranda | 29:5714715ab1f5 | 152 | **************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 153 | void pid_turn(int deg); |
JesiMiranda | 29:5714715ab1f5 | 154 | |
JesiMiranda | 29:5714715ab1f5 | 155 | |
JesiMiranda | 29:5714715ab1f5 | 156 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 157 | * * |
JesiMiranda | 29:5714715ab1f5 | 158 | * ROS-Embed Communication Methods * |
JesiMiranda | 29:5714715ab1f5 | 159 | * * |
JesiMiranda | 29:5714715ab1f5 | 160 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 161 | |
JesiMiranda | 29:5714715ab1f5 | 162 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 163 | * This method computes the relative angle for Twist message in ROS * |
JesiMiranda | 29:5714715ab1f5 | 164 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 165 | void pid_twistA(); |
jvfausto | 21:3489cffad196 | 166 | |
JesiMiranda | 29:5714715ab1f5 | 167 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 168 | * This method computes the relative velocity for Twist message in ROS * |
JesiMiranda | 29:5714715ab1f5 | 169 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 170 | void pid_twistV(); |
JesiMiranda | 29:5714715ab1f5 | 171 | |
JesiMiranda | 29:5714715ab1f5 | 172 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 173 | * This method computes the angular position for Twist message in ROS * |
JesiMiranda | 29:5714715ab1f5 | 174 | *************************************************************************/ |
jvfausto | 21:3489cffad196 | 175 | double getTwistZ(); |
jvfausto | 21:3489cffad196 | 176 | |
JesiMiranda | 29:5714715ab1f5 | 177 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 178 | * This method calculates the relative position of the chair everytime * |
JesiMiranda | 29:5714715ab1f5 | 179 | * the encoders reset by setting its old position as the origin to * |
JesiMiranda | 29:5714715ab1f5 | 180 | * calculate the new position. Moreover, this function is important to * |
JesiMiranda | 29:5714715ab1f5 | 181 | * be able to publish (send) the information to ROS * |
JesiMiranda | 29:5714715ab1f5 | 182 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 183 | void odomMsg(); |
jvfausto | 21:3489cffad196 | 184 | |
JesiMiranda | 29:5714715ab1f5 | 185 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 186 | * This method prints the Odometry message to the serial monitor * |
JesiMiranda | 29:5714715ab1f5 | 187 | *************************************************************************/ |
jvfausto | 21:3489cffad196 | 188 | void showOdom(); |
JesiMiranda | 29:5714715ab1f5 | 189 | |
JesiMiranda | 29:5714715ab1f5 | 190 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 191 | * (not being used) Functions with a predetermined path (demo) * |
JesiMiranda | 29:5714715ab1f5 | 192 | *************************************************************************/ |
JesiMiranda | 29:5714715ab1f5 | 193 | |
JesiMiranda | 29:5714715ab1f5 | 194 | |
jvfausto | 19:71a6621ee5c3 | 195 | void desk(); |
jvfausto | 19:71a6621ee5c3 | 196 | void kitchen(); |
jvfausto | 19:71a6621ee5c3 | 197 | void desk_to_kitchen(); |
JesiMiranda | 29:5714715ab1f5 | 198 | |
JesiMiranda | 29:5714715ab1f5 | 199 | /************************************************************************* |
JesiMiranda | 29:5714715ab1f5 | 200 | * Public Variables * |
JesiMiranda | 29:5714715ab1f5 | 201 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 202 | double x_position; |
jvfausto | 21:3489cffad196 | 203 | double y_position; |
JesiMiranda | 29:5714715ab1f5 | 204 | double z_angular; |
jvfausto | 21:3489cffad196 | 205 | double curr_vel; |
jvfausto | 21:3489cffad196 | 206 | double z_twistA; |
jvfausto | 21:3489cffad196 | 207 | double linearV; |
jvfausto | 21:3489cffad196 | 208 | double angularV; |
jvfausto | 21:3489cffad196 | 209 | double vel; |
JesiMiranda | 29:5714715ab1f5 | 210 | double test1, test2; |
jvfausto | 21:3489cffad196 | 211 | bool forwardSafety; |
jvfausto | 21:3489cffad196 | 212 | double curr_yaw, curr_velS; // Variable that contains current relative angle |
JesiMiranda | 29:5714715ab1f5 | 213 | int ToFV[12]; |
JesiMiranda | 29:5714715ab1f5 | 214 | //Needed to send encoder values to ROS |
JesiMiranda | 29:5714715ab1f5 | 215 | double dist_new; |
JesiMiranda | 29:5714715ab1f5 | 216 | double dist_newS; |
jvfausto | 21:3489cffad196 | 217 | |
ryanlin97 | 1:c0beadca1617 | 218 | private: |
jvfausto | 21:3489cffad196 | 219 | /* Pointers for the joystick speed */ |
ryanlin97 | 3:a5e71bfdb492 | 220 | PwmOut* x; |
ryanlin97 | 3:a5e71bfdb492 | 221 | PwmOut* y; |
jvfausto | 21:3489cffad196 | 222 | |
jvfausto | 21:3489cffad196 | 223 | chair_BNO055* imu; // Pointer to IMU |
jvfausto | 21:3489cffad196 | 224 | Serial* out; // Pointer to Serial Monitor |
jvfausto | 21:3489cffad196 | 225 | Timer* ti; // Pointer to the timer |
jvfausto | 21:3489cffad196 | 226 | QEI* wheel; // Pointer to encoder |
jvfausto | 21:3489cffad196 | 227 | QEI* wheelS; // Pointer to encoder |
jvfausto | 21:3489cffad196 | 228 | VL53L1X** ToF; // Arrays of pointers to ToF sensors |
ryanlin97 | 1:c0beadca1617 | 229 | |
JesiMiranda | 29:5714715ab1f5 | 230 | |
ryanlin97 | 0:fc0c4a184482 | 231 | }; |
ryanlin97 | 0:fc0c4a184482 | 232 | #endif |