1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Diff: wheelchair.h
- Revision:
- 30:b24d73663499
- Parent:
- 29:4519f4cdcb5d
diff -r 4519f4cdcb5d -r b24d73663499 wheelchair.h --- a/wheelchair.h Mon Jul 01 16:36:47 2019 +0000 +++ b/wheelchair.h Wed Jun 03 20:25:18 2020 +0000 @@ -9,32 +9,40 @@ #include "VL53L1X.h" #include "statistics.h" #include "Watchdog.h" - + #include <ros.h> #include <geometry_msgs/Twist.h> #include <nav_msgs/Odometry.h> - + /************************************************************************* * Joystick has analog out of 200-700, scale values between 1.3 and 3.3 * * Here are some global constants for joystick * **************************************************************************/ -#define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis -#define high 3.3f/3.3f //High power on joystick; used on x and y axis -#define low (1.7f/3.3f) //Low power on joystick; used on x and y axis -#define offset .03f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision -#define process .1 //Defines default time delay in seconds +#define def (2.5f/3.3f) // Default axis on joystick to stay neutral; used on x and y axis +#define high 3.3f/3.3f // High power on joystick; used on x and y axis +#define low (1.7f/3.3f) // Low power on joystick; used on x and y axis +#define offset .03f // Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision +#define process .1 // Defines default time delay in seconds /************************************************************************* *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)* **************************************************************************/ -#define xDir PA_6 //* PWM Pins */ +#define xDir PA_6 // PWM Pins #define yDir PA_5 -#define Encoder1 D7 //*Digital In Pull Up Pin */ +#define Encoder1 D7 // Digital In Pull Up Pin #define Encoder2 D8 -#define Diameter 31.75 //Diameter of encoder wheel +#define Diameter 31.75 // Diameter of encoder wheel #define maxDecelerationSlow 120 #define maxDecelerationFast 30 #define ToFSensorNum 12 - + +/************************************************************************* +*IMU definitions for turning wheelchair +**************************************************************************/ +#define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm) +#define MAX_ANGULAR_DECELERATION 60 //found through testing, max + //acceleration at which chair can + //stop while turning. In degree per sec +#define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm) /************************************************************************* * * * Wheelchair class * @@ -51,12 +59,12 @@ **************************************************************************/ Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS, VL53L1X** ToF); - + /************************************************************************* * This method is to move using the joystick * **************************************************************************/ void move(float x_coor, float y_coor); - + /************************************************************************* * This method is to drive the wheelchair forward manually (NO PID used)* ************************************************************************ */ @@ -65,37 +73,37 @@ * This method is to drive the wheelchair backwards (NO PID used) * ************************************************************************ */ void backward(); - + /************************************************************************* * This method is to turn the wheelchair right manually (NO PID used) * ************************************************************************ */ void right(); - + /************************************************************************* * This method is to turn the wheelchair left manually (NO PID used) * ************************************************************************ */ void left(); - + /************************************************************************* * This method stops the wheelchair and reset the joystick position * ************************************************************************ */ void stop(); - + /************************************************************************* * This method is a thread that will obtain the IMU information such * * as the gyroscope x,y axis. Z-axis is not used. * ************************************************************************ */ void compass_thread(); - + /************************************************************************* * This method is a thread that will calculate the velocity of the * * wheechair using the encoder values this is being obatined. * ************************************************************************ */ void velocity_thread(); - + //not being used void rosCom_thread(); - + /************************************************************************* * This method is a thread that iterates through all the sensor's * * values and determines whether or not the wheelchair is about to hit * @@ -103,61 +111,61 @@ * the chair, then the chair will safely halt and allow movement in the * * direction opposed to where an object is detected. * ************************************************************************ */ - void assistSafe_thread(); - + void ToFSafe_thread(); + /************************************************************************* * This method is a thread that will constantly be checking the value * * of the emergency button. If the button is pressed, then the chair * * will stop and the entire system will reset. * ************************************************************************ */ void emergencyButton_thread(); - + /************************************************************************* * This method gets the encoder values and calculates the distance since* * the last encoder reset. * **************************************************************************/ float getDistance(); - + /************************************************************************* * This method resets the encoder value to recalculate distance * **************************************************************************/ void resetDistance(); - - + + /************************************************************************* * * * PID Control Methods * * * *************************************************************************/ - - + + /************************************************************************* * This method moves the wheelchair x-millimiters forward using PID * **************************************************************************/ void pid_forward(double mm); - + /************************************************************************* * This method turns the chair right a certain amount of degrees using PID* **************************************************************************/ void pid_right(int deg); - + /************************************************************************* * This method turns the chair left a certain amount of degrees using PID * **************************************************************************/ void pid_left(int deg); - + /************************************************************************* * This method determines whether to turn the wheelchair left or right * **************************************************************************/ void pid_turn(int deg); - - + + /************************************************************************* * * * ROS-Embed Communication Methods * * * *************************************************************************/ - + /************************************************************************* * This method computes the relative angle for Twist message in ROS * *************************************************************************/ @@ -185,14 +193,14 @@ * This method prints the Odometry message to the serial monitor * *************************************************************************/ void showOdom(); - + /************************************************************************* * (not being used) Functions with a predetermined path (demo) * *************************************************************************/ void desk(); void kitchen(); void desk_to_kitchen(); - + /************************************************************************* * Public Variables * **************************************************************************/ @@ -206,33 +214,36 @@ double vel; double test1, test2; bool forwardSafety; + bool sideSafety; //to check if can turn double curr_yaw, curr_velS; // Variable that contains current relative angle - + private: int runningAverage[4]; // Expected data used to compare whether of not there is a ledge. This serves as a ground base // Array is used for calibrating the time of flight sensors // Used to calculate stdev and mean on - int ledgeArrayLF[100]; - int ledgeArrayRF[100]; - + + + /* Pointers for the joystick speed */ PwmOut* x; PwmOut* y; - + //Pointers for PCB PwmOut* on; PwmOut* off; - + DigitalIn* e_button; //Pointer to e_button - + chair_BNO055* imu; // Pointer to IMU Serial* out; // Pointer to Serial Monitor Timer* ti; // Pointer to the timer QEI* wheel; // Pointer to encoder QEI* wheelS; // Pointer to encoder VL53L1X** ToF; // Arrays of pointers to ToF sensors - - + + + + }; #endif \ No newline at end of file