Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

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?

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 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