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