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