1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

Committer:
jvfausto
Date:
Wed Jun 03 20:25:18 2020 +0000
Revision:
30:b24d73663499
Parent:
29:4519f4cdcb5d
For sharing

Who changed what in which revision?

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