Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.h@26:662693bd7f31, 2019-06-27 (annotated)

Committer:
jvfausto
Date:
Thu Jun 27 16:32:12 2019 +0000
Revision:
26:662693bd7f31
Parent:
21:3489cffad196
Child:
27:da718b990837
a

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 26:662693bd7f31 21 #define offset .03f //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 26:662693bd7f31 30 #define maxDecelerationSlow 120
jvfausto 26:662693bd7f31 31 #define maxDecelerationFast 30
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