Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

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:b24d73663499
a

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 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