Jesi Miranda / wheelchaircontrol1

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@37:e0e6d3fe06a2, 2019-07-09 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 09 23:36:58 2019 +0000
Revision:
37:e0e6d3fe06a2
Parent:
35:5a2fed4c2e9f 
ff

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