Affordable Smart Wheelchair / wheelchaircontrol2

Revised for integration

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

wheelchair.h@26:e06e67411fc9, 2019-06-26 (annotated)

Committer:
t1jain
Date:
Wed Jun 26 18:20:11 2019 +0000
Revision:
26:e06e67411fc9
Parent:
21:3489cffad196 
Revised for integration

Who changed what in which revision?

UserRevisionLine numberNew 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();
t1jain 26:e06e67411fc9 77
t1jain 26:e06e67411fc9 78 void emergencyButton_thread();
jvfausto 21:3489cffad196 79
jvfausto 21:3489cffad196 80
jvfausto 21:3489cffad196 81 /* Move x millimiters foward using PID*/
jvfausto 19:71a6621ee5c3 82 void pid_forward(double mm);
jvfausto 21:3489cffad196 83
jvfausto 21:3489cffad196 84 /* Obtain angular position for Twist message */
jvfausto 21:3489cffad196 85 double getTwistZ();
jvfausto 21:3489cffad196 86
jvfausto 21:3489cffad196 87 /* Gets the encoder distance moved since encoder reset*/
ryanlin97 12:921488918749 88 float getDistance();
jvfausto 21:3489cffad196 89
jvfausto 21:3489cffad196 90 /* Resets encoder*/
ryanlin97 12:921488918749 91 void resetDistance();
jvfausto 21:3489cffad196 92
jvfausto 21:3489cffad196 93 /* Function to determine whether we are turning left or right*/
ryanlin97 12:921488918749 94 void pid_turn(int deg);
jvfausto 21:3489cffad196 95
jvfausto 21:3489cffad196 96 /* Function to obtain angular and linear velocity */
jvfausto 21:3489cffad196 97 void pid_twistA();
jvfausto 21:3489cffad196 98 void pid_twistV();
jvfausto 21:3489cffad196 99
jvfausto 21:3489cffad196 100 /* Function to publish and show Odometry message */
jvfausto 21:3489cffad196 101 void odomMsg();
jvfausto 21:3489cffad196 102 void showOdom();
jvfausto 21:3489cffad196 103
jvfausto 21:3489cffad196 104 /* Functions with a predetermined path (demo) */
jvfausto 19:71a6621ee5c3 105 void desk();
jvfausto 19:71a6621ee5c3 106 void kitchen();
jvfausto 19:71a6621ee5c3 107 void desk_to_kitchen();
jvfausto 21:3489cffad196 108
jvfausto 21:3489cffad196 109 /* Variables for postion, angle, and velocity */
jvfausto 21:3489cffad196 110 double x_position;
jvfausto 21:3489cffad196 111 double y_position;
jvfausto 21:3489cffad196 112 double z_angular;
jvfausto 21:3489cffad196 113 double curr_vel;
jvfausto 21:3489cffad196 114 double z_twistA;
jvfausto 21:3489cffad196 115 double linearV;
jvfausto 21:3489cffad196 116 double angularV;
jvfausto 21:3489cffad196 117 double vel;
jvfausto 21:3489cffad196 118 double test1, test2;
jvfausto 21:3489cffad196 119 bool forwardSafety;
jvfausto 21:3489cffad196 120 double curr_yaw, curr_velS; // Variable that contains current relative angle
jvfausto 21:3489cffad196 121
jvfausto 21:3489cffad196 122
ryanlin97 1:c0beadca1617 123 private:
jvfausto 21:3489cffad196 124 /* Pointers for the joystick speed */
ryanlin97 3:a5e71bfdb492 125 PwmOut* x;
ryanlin97 3:a5e71bfdb492 126 PwmOut* y;
jvfausto 21:3489cffad196 127
jvfausto 21:3489cffad196 128 chair_BNO055* imu; // Pointer to IMU
jvfausto 21:3489cffad196 129 Serial* out; // Pointer to Serial Monitor
jvfausto 21:3489cffad196 130 Timer* ti; // Pointer to the timer
jvfausto 21:3489cffad196 131 QEI* wheel; // Pointer to encoder
jvfausto 21:3489cffad196 132 QEI* wheelS; // Pointer to encoder
jvfausto 21:3489cffad196 133 VL53L1X** ToF; // Arrays of pointers to ToF sensors
ryanlin97 1:c0beadca1617 134
jvfausto 21:3489cffad196 135
ryanlin97 0:fc0c4a184482 136 };
ryanlin97 0:fc0c4a184482 137 #endif