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