Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
wheelchair.h@21:3489cffad196, 2019-04-19 (annotated)
- Committer:
- jvfausto
- Date:
- Fri Apr 19 23:04:01 2019 +0000
- Revision:
- 21:3489cffad196
- Parent:
- 20:f42db4ae16f0
- Child:
- 26:c842070aa0b8
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:fc0c4a184482 | 1 | #ifndef wheelchair |
ryanlin97 | 0:fc0c4a184482 | 2 | #define wheelchair |
ryanlin97 | 0:fc0c4a184482 | 3 | |
jvfausto | 21:3489cffad196 | 4 | /* Importing libraries into wheelchair.h */ |
ryanlin97 | 11:d14a1f7f1297 | 5 | #include "chair_BNO055.h" |
jvfausto | 17:7f3b69300bb6 | 6 | #include "PID.h" |
ryanlin97 | 12:921488918749 | 7 | #include "QEI.h" |
jvfausto | 21:3489cffad196 | 8 | #include "VL53L1X.h" |
jvfausto | 21:3489cffad196 | 9 | |
ryanlin97 | 16:b403082eeacd | 10 | #include <ros.h> |
ryanlin97 | 16:b403082eeacd | 11 | #include <geometry_msgs/Twist.h> |
jvfausto | 21:3489cffad196 | 12 | #include <nav_msgs/Odometry.h> |
ryanlin97 | 12:921488918749 | 13 | |
ryanlin97 | 0:fc0c4a184482 | 14 | |
jvfausto | 21:3489cffad196 | 15 | /* |
jvfausto | 21:3489cffad196 | 16 | * Joystick has analog out of 200-700, scale values between 1.3 and 3.3 |
jvfausto | 21:3489cffad196 | 17 | */ |
jvfausto | 21:3489cffad196 | 18 | #define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis |
jvfausto | 21:3489cffad196 | 19 | #define high 3.3f/3.3f //High power on joystick; used on x and y axis |
jvfausto | 21:3489cffad196 | 20 | #define low (1.7f/3.3f) //Low power on joystick; used on x and y axis |
jvfausto | 21:3489cffad196 | 21 | #define offset .035f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision |
jvfausto | 21:3489cffad196 | 22 | #define process .1 //Defines default time delay in seconds |
ryanlin97 | 14:9caca9fde9b0 | 23 | |
jvfausto | 21:3489cffad196 | 24 | /* Pin plug in for Nucleo-L432KC */ |
jvfausto | 21:3489cffad196 | 25 | #define xDir PA_6 //* PWM Pins */ |
jvfausto | 21:3489cffad196 | 26 | #define yDir PA_5 |
jvfausto | 21:3489cffad196 | 27 | #define Encoder1 D7 //*Digital In Pull Up Pin */ |
jvfausto | 21:3489cffad196 | 28 | #define Encoder2 D8 |
jvfausto | 21:3489cffad196 | 29 | #define Diameter 31.75 //Diameter of encoder wheel |
jvfausto | 21:3489cffad196 | 30 | #define maxDeceleration 130 |
ryanlin97 | 14:9caca9fde9b0 | 31 | |
jvfausto | 21:3489cffad196 | 32 | #define ToFSensorNum 12 |
ryanlin97 | 14:9caca9fde9b0 | 33 | |
jvfausto | 21:3489cffad196 | 34 | |
ryanlin97 | 12:921488918749 | 35 | /** Wheelchair class |
ryanlin97 | 12:921488918749 | 36 | * Used for controlling the smart wheelchair |
ryanlin97 | 12:921488918749 | 37 | */ |
jvfausto | 17:7f3b69300bb6 | 38 | |
ryanlin97 | 0:fc0c4a184482 | 39 | class Wheelchair |
ryanlin97 | 0:fc0c4a184482 | 40 | { |
ryanlin97 | 0:fc0c4a184482 | 41 | public: |
ryanlin97 | 12:921488918749 | 42 | /** Create Wheelchair Object with x,y pin for analog dc output |
ryanlin97 | 12:921488918749 | 43 | * serial for printout, and timer |
ryanlin97 | 12:921488918749 | 44 | */ |
jvfausto | 21:3489cffad196 | 45 | Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS, |
jvfausto | 21:3489cffad196 | 46 | VL53L1X** ToF); |
ryanlin97 | 12:921488918749 | 47 | |
jvfausto | 21:3489cffad196 | 48 | /** Move using the joystick */ |
ryanlin97 | 3:a5e71bfdb492 | 49 | void move(float x_coor, float y_coor); |
ryanlin97 | 12:921488918749 | 50 | |
jvfausto | 21:3489cffad196 | 51 | /* Turn right a certain amount of degrees using PID*/ |
ryanlin97 | 12:921488918749 | 52 | void pid_right(int deg); |
ryanlin97 | 12:921488918749 | 53 | |
jvfausto | 21:3489cffad196 | 54 | /* Turn left a certain amount of degrees using PID*/ |
ryanlin97 | 12:921488918749 | 55 | void pid_left(int deg); |
ryanlin97 | 12:921488918749 | 56 | |
jvfausto | 21:3489cffad196 | 57 | /* Drive the wheelchair forward */ |
ryanlin97 | 1:c0beadca1617 | 58 | void forward(); |
ryanlin97 | 12:921488918749 | 59 | |
jvfausto | 21:3489cffad196 | 60 | /* Drive the wheelchair in reverse*/ |
ryanlin97 | 1:c0beadca1617 | 61 | void backward(); |
ryanlin97 | 12:921488918749 | 62 | |
jvfausto | 21:3489cffad196 | 63 | /* Turn the wheelchair right*/ |
ryanlin97 | 1:c0beadca1617 | 64 | void right(); |
ryanlin97 | 12:921488918749 | 65 | |
jvfausto | 21:3489cffad196 | 66 | /* Turn the wheelchair left*/ |
ryanlin97 | 1:c0beadca1617 | 67 | void left(); |
ryanlin97 | 12:921488918749 | 68 | |
jvfausto | 21:3489cffad196 | 69 | /* Stop the wheelchair*/ |
ryanlin97 | 1:c0beadca1617 | 70 | void stop(); |
ryanlin97 | 12:921488918749 | 71 | |
jvfausto | 21:3489cffad196 | 72 | /* Functions to get IMU data*/ |
ryanlin97 | 11:d14a1f7f1297 | 73 | void compass_thread(); |
jvfausto | 21:3489cffad196 | 74 | void velocity_thread(); |
jvfausto | 21:3489cffad196 | 75 | void rosCom_thread(); |
jvfausto | 21:3489cffad196 | 76 | void assistSafe_thread(); |
jvfausto | 21:3489cffad196 | 77 | |
jvfausto | 21:3489cffad196 | 78 | |
jvfausto | 21:3489cffad196 | 79 | /* Move x millimiters foward using PID*/ |
jvfausto | 19:71a6621ee5c3 | 80 | void pid_forward(double mm); |
jvfausto | 21:3489cffad196 | 81 | |
jvfausto | 21:3489cffad196 | 82 | /* Obtain angular position for Twist message */ |
jvfausto | 21:3489cffad196 | 83 | double getTwistZ(); |
jvfausto | 21:3489cffad196 | 84 | |
jvfausto | 21:3489cffad196 | 85 | /* Gets the encoder distance moved since encoder reset*/ |
ryanlin97 | 12:921488918749 | 86 | float getDistance(); |
jvfausto | 21:3489cffad196 | 87 | |
jvfausto | 21:3489cffad196 | 88 | /* Resets encoder*/ |
ryanlin97 | 12:921488918749 | 89 | void resetDistance(); |
jvfausto | 21:3489cffad196 | 90 | |
jvfausto | 21:3489cffad196 | 91 | /* Function to determine whether we are turning left or right*/ |
ryanlin97 | 12:921488918749 | 92 | void pid_turn(int deg); |
jvfausto | 21:3489cffad196 | 93 | |
jvfausto | 21:3489cffad196 | 94 | /* Function to obtain angular and linear velocity */ |
jvfausto | 21:3489cffad196 | 95 | void pid_twistA(); |
jvfausto | 21:3489cffad196 | 96 | void pid_twistV(); |
jvfausto | 21:3489cffad196 | 97 | |
jvfausto | 21:3489cffad196 | 98 | /* Function to publish and show Odometry message */ |
jvfausto | 21:3489cffad196 | 99 | void odomMsg(); |
jvfausto | 21:3489cffad196 | 100 | void showOdom(); |
jvfausto | 21:3489cffad196 | 101 | |
jvfausto | 21:3489cffad196 | 102 | /* Functions with a predetermined path (demo) */ |
jvfausto | 19:71a6621ee5c3 | 103 | void desk(); |
jvfausto | 19:71a6621ee5c3 | 104 | void kitchen(); |
jvfausto | 19:71a6621ee5c3 | 105 | void desk_to_kitchen(); |
jvfausto | 21:3489cffad196 | 106 | |
jvfausto | 21:3489cffad196 | 107 | /* Variables for postion, angle, and velocity */ |
jvfausto | 21:3489cffad196 | 108 | double x_position; |
jvfausto | 21:3489cffad196 | 109 | double y_position; |
jvfausto | 21:3489cffad196 | 110 | double z_angular; |
jvfausto | 21:3489cffad196 | 111 | double curr_vel; |
jvfausto | 21:3489cffad196 | 112 | double z_twistA; |
jvfausto | 21:3489cffad196 | 113 | double linearV; |
jvfausto | 21:3489cffad196 | 114 | double angularV; |
jvfausto | 21:3489cffad196 | 115 | double vel; |
jvfausto | 21:3489cffad196 | 116 | double test1, test2; |
jvfausto | 21:3489cffad196 | 117 | bool forwardSafety; |
jvfausto | 21:3489cffad196 | 118 | double curr_yaw, curr_velS; // Variable that contains current relative angle |
jvfausto | 21:3489cffad196 | 119 | |
jvfausto | 21:3489cffad196 | 120 | |
ryanlin97 | 1:c0beadca1617 | 121 | private: |
jvfausto | 21:3489cffad196 | 122 | /* Pointers for the joystick speed */ |
ryanlin97 | 3:a5e71bfdb492 | 123 | PwmOut* x; |
ryanlin97 | 3:a5e71bfdb492 | 124 | PwmOut* y; |
jvfausto | 21:3489cffad196 | 125 | |
jvfausto | 21:3489cffad196 | 126 | chair_BNO055* imu; // Pointer to IMU |
jvfausto | 21:3489cffad196 | 127 | Serial* out; // Pointer to Serial Monitor |
jvfausto | 21:3489cffad196 | 128 | Timer* ti; // Pointer to the timer |
jvfausto | 21:3489cffad196 | 129 | QEI* wheel; // Pointer to encoder |
jvfausto | 21:3489cffad196 | 130 | QEI* wheelS; // Pointer to encoder |
jvfausto | 21:3489cffad196 | 131 | VL53L1X** ToF; // Arrays of pointers to ToF sensors |
ryanlin97 | 1:c0beadca1617 | 132 | |
jvfausto | 21:3489cffad196 | 133 | |
ryanlin97 | 0:fc0c4a184482 | 134 | }; |
ryanlin97 | 0:fc0c4a184482 | 135 | #endif |