Jesi Miranda / wheelchaircontrol_jesi

Working file for communication with ROS

Dependencies:   QEI2 PID VL53L1X_Filter

wheelchair.h@32:5ca063fc0695, 2019-08-20 (annotated)

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