Jesi Miranda / wheelchaircontrol_jesi

Working file for communication with ROS

Dependencies:   QEI2 PID VL53L1X_Filter

wheelchair.h@29:5714715ab1f5, 2019-07-05 (annotated)

Committer:
JesiMiranda
Date:
Fri Jul 05 19:38:55 2019 +0000
Revision:
29:5714715ab1f5
Parent:
26:c842070aa0b8
Child:
31:9283c8cd5362
no changes

Who changed what in which revision?

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