Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
wheelchair.h@25:58ec657a44f2, 2018-10-27 (annotated)
- Committer:
- jvfausto
- Date:
- Sat Oct 27 16:47:50 2018 +0000
- Revision:
- 25:58ec657a44f2
- Parent:
- 24:d2f234fbc20d
With odometry
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 | 23:8d11d953ceeb | 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" |
ryanlin97 | 16:b403082eeacd | 8 | #include <ros.h> |
ryanlin97 | 16:b403082eeacd | 9 | #include <geometry_msgs/Twist.h> |
ryanlin97 | 12:921488918749 | 10 | |
ryanlin97 | 14:9caca9fde9b0 | 11 | |
jvfausto | 23:8d11d953ceeb | 12 | /* |
jvfausto | 23:8d11d953ceeb | 13 | * joystick has analog out of 200-700, scale values between 1.3 and 3.3 |
jvfausto | 23:8d11d953ceeb | 14 | */ |
jvfausto | 23:8d11d953ceeb | 15 | #define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis |
jvfausto | 23:8d11d953ceeb | 16 | #define high 3.3f/3.3f //High power on joystick; used on x and y axis |
jvfausto | 23:8d11d953ceeb | 17 | #define low (1.7f/3.3f) //Low power on joystick; used on x and y axis |
jvfausto | 23:8d11d953ceeb | 18 | #define offset .035f //Joystick ajustment to be able to go strait. Chair dependent on manufactoring presision |
jvfausto | 23:8d11d953ceeb | 19 | #define process .1 //Defines default time delay in seconds |
ryanlin97 | 14:9caca9fde9b0 | 20 | |
jvfausto | 23:8d11d953ceeb | 21 | //Pin plug in for Nucleo-L432KC |
jvfausto | 23:8d11d953ceeb | 22 | #define xDir D9 //PWM Pins |
ryanlin97 | 14:9caca9fde9b0 | 23 | #define yDir D10 |
jvfausto | 23:8d11d953ceeb | 24 | #define Encoder1 D7 //Digital In Pull Up Pin |
ryanlin97 | 14:9caca9fde9b0 | 25 | #define Encoder2 D8 |
ryanlin97 | 14:9caca9fde9b0 | 26 | |
jvfausto | 23:8d11d953ceeb | 27 | #define Diameter 31.75 //Diameter of encoder wheel |
ryanlin97 | 12:921488918749 | 28 | /** Wheelchair class |
ryanlin97 | 12:921488918749 | 29 | * Used for controlling the smart wheelchair |
ryanlin97 | 12:921488918749 | 30 | */ |
jvfausto | 17:7f3b69300bb6 | 31 | |
ryanlin97 | 0:fc0c4a184482 | 32 | class Wheelchair |
ryanlin97 | 0:fc0c4a184482 | 33 | { |
ryanlin97 | 0:fc0c4a184482 | 34 | public: |
ryanlin97 | 12:921488918749 | 35 | /** Create Wheelchair Object with x,y pin for analog dc output |
ryanlin97 | 12:921488918749 | 36 | * serial for printout, and timer |
ryanlin97 | 12:921488918749 | 37 | */ |
jvfausto | 24:d2f234fbc20d | 38 | Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS); |
ryanlin97 | 12:921488918749 | 39 | |
ryanlin97 | 12:921488918749 | 40 | /** move using the joystick */ |
ryanlin97 | 3:a5e71bfdb492 | 41 | void move(float x_coor, float y_coor); |
ryanlin97 | 12:921488918749 | 42 | |
ryanlin97 | 12:921488918749 | 43 | /* turn right a certain amount of degrees using PID*/ |
ryanlin97 | 12:921488918749 | 44 | void pid_right(int deg); |
ryanlin97 | 12:921488918749 | 45 | |
ryanlin97 | 12:921488918749 | 46 | /* turn left a certain amount of degrees using PID*/ |
ryanlin97 | 12:921488918749 | 47 | void pid_left(int deg); |
ryanlin97 | 12:921488918749 | 48 | |
ryanlin97 | 12:921488918749 | 49 | /* drive the wheelchair forward */ |
ryanlin97 | 1:c0beadca1617 | 50 | void forward(); |
ryanlin97 | 12:921488918749 | 51 | |
ryanlin97 | 12:921488918749 | 52 | /* drive the wheelchair backward*/ |
ryanlin97 | 1:c0beadca1617 | 53 | void backward(); |
ryanlin97 | 12:921488918749 | 54 | |
ryanlin97 | 12:921488918749 | 55 | /* turn the wheelchair right*/ |
ryanlin97 | 1:c0beadca1617 | 56 | void right(); |
ryanlin97 | 12:921488918749 | 57 | |
ryanlin97 | 12:921488918749 | 58 | /* turn the wheelchair left*/ |
ryanlin97 | 1:c0beadca1617 | 59 | void left(); |
ryanlin97 | 12:921488918749 | 60 | |
ryanlin97 | 12:921488918749 | 61 | /* stop the wheelchair*/ |
ryanlin97 | 1:c0beadca1617 | 62 | void stop(); |
ryanlin97 | 12:921488918749 | 63 | |
ryanlin97 | 12:921488918749 | 64 | /* function to get imu data*/ |
ryanlin97 | 11:d14a1f7f1297 | 65 | void compass_thread(); |
jvfausto | 24:d2f234fbc20d | 66 | void velosity_thread(); |
jvfausto | 23:8d11d953ceeb | 67 | |
jvfausto | 23:8d11d953ceeb | 68 | /* move x millimiters foward using PID*/ |
jvfausto | 19:71a6621ee5c3 | 69 | void pid_forward(double mm); |
jvfausto | 23:8d11d953ceeb | 70 | |
jvfausto | 23:8d11d953ceeb | 71 | /* move x millimiters reverse using PID*/ |
jvfausto | 17:7f3b69300bb6 | 72 | void pid_reverse(double mm); |
jvfausto | 23:8d11d953ceeb | 73 | |
jvfausto | 23:8d11d953ceeb | 74 | /* gets the encoder distance moved since encoder reset*/ |
ryanlin97 | 12:921488918749 | 75 | float getDistance(); |
jvfausto | 23:8d11d953ceeb | 76 | |
jvfausto | 23:8d11d953ceeb | 77 | /* resets encoder*/ |
ryanlin97 | 12:921488918749 | 78 | void resetDistance(); |
jvfausto | 23:8d11d953ceeb | 79 | |
jvfausto | 23:8d11d953ceeb | 80 | /* function to to determine whether we are turning left or right*/ |
ryanlin97 | 12:921488918749 | 81 | void pid_turn(int deg); |
jvfausto | 23:8d11d953ceeb | 82 | |
jvfausto | 24:d2f234fbc20d | 83 | void pid_twistA(); |
jvfausto | 24:d2f234fbc20d | 84 | void pid_twistV(); |
jvfausto | 25:58ec657a44f2 | 85 | |
jvfausto | 25:58ec657a44f2 | 86 | void odomMsg(); |
jvfausto | 25:58ec657a44f2 | 87 | void showOdom(); |
jvfausto | 25:58ec657a44f2 | 88 | |
jvfausto | 23:8d11d953ceeb | 89 | /* functions with a predetermined path demmo*/ |
jvfausto | 19:71a6621ee5c3 | 90 | void desk(); |
jvfausto | 19:71a6621ee5c3 | 91 | void kitchen(); |
jvfausto | 19:71a6621ee5c3 | 92 | void desk_to_kitchen(); |
jvfausto | 25:58ec657a44f2 | 93 | |
ryanlin97 | 1:c0beadca1617 | 94 | private: |
jvfausto | 23:8d11d953ceeb | 95 | /* Pointers for the joystick speed*/ |
ryanlin97 | 3:a5e71bfdb492 | 96 | PwmOut* x; |
ryanlin97 | 3:a5e71bfdb492 | 97 | PwmOut* y; |
jvfausto | 23:8d11d953ceeb | 98 | |
jvfausto | 23:8d11d953ceeb | 99 | chair_BNO055* imu; // Pointer to IMU |
jvfausto | 23:8d11d953ceeb | 100 | Serial* out; // Pointer to Serial Monitor |
jvfausto | 23:8d11d953ceeb | 101 | Timer* ti; // Pointer to the timer |
jvfausto | 23:8d11d953ceeb | 102 | QEI* wheel; // Pointer to encoder |
jvfausto | 24:d2f234fbc20d | 103 | QEI* wheelS; // Pointer to encoder |
ryanlin97 | 0:fc0c4a184482 | 104 | }; |
ryanlin97 | 0:fc0c4a184482 | 105 | #endif |