Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

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?

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