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