Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
Diff: wheelchair.h
- Revision:
- 29:5714715ab1f5
- Parent:
- 26:c842070aa0b8
- Child:
- 31:9283c8cd5362
diff -r 674ad4175a15 -r 5714715ab1f5 wheelchair.h --- a/wheelchair.h Wed Jun 26 04:23:39 2019 +0000 +++ b/wheelchair.h Fri Jul 05 19:38:55 2019 +0000 @@ -1,7 +1,8 @@ #ifndef wheelchair #define wheelchair - -/* Importing libraries into wheelchair.h */ +/************************************************************************* +* Importing libraries into wheelchair.h * +**************************************************************************/ #include "chair_BNO055.h" #include "PID.h" #include "QEI.h" @@ -10,113 +11,209 @@ #include <ros.h> #include <geometry_msgs/Twist.h> #include <nav_msgs/Odometry.h> - +#include <std_msgs/Float64.h> +#include <std_msgs/Float64MultiArray.h> -/* -* Joystick has analog out of 200-700, scale values between 1.3 and 3.3 -*/ +/************************************************************************* +* 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 - -/* Pin plug in for Nucleo-L432KC */ -#define xDir PA_6 //* PWM Pins */ +/************************************************************************* +*Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)* +**************************************************************************/ +#define xDir PA_6 //* PWM Pins */ #define yDir PA_5 #define Encoder1 D7 //*Digital In Pull Up Pin */ #define Encoder2 D8 #define Diameter 31.75 //Diameter of encoder wheel -#define maxDecelerationSlow 130 +#define maxDecelerationSlow 120 #define maxDecelerationFast 30 #define ToFSensorNum 12 - +#define USE_USBCON -/** Wheelchair class - * Used for controlling the smart wheelchair - */ - +/************************************************************************* +* * +* Wheelchair class * +* Used for controlling the Smart Wheelchair * +* * +**************************************************************************/ class Wheelchair { public: - /** Create Wheelchair Object with x,y pin for analog dc output - * serial for printout, and timer - */ + /************************************************************************* + * Create Wheelchair constructor with x,y pin for analog-to-dc output * + * serial for printout, and time. This is also used to initialize some * + * variables for the timer,encoders and time-of-flight sensors * + **************************************************************************/ Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS, - VL53L1X** ToF); - - /** Move using the joystick */ + VL53L1X** ToF); + + /************************************************************************* + * This method is to move using the joystick * + **************************************************************************/ void move(float x_coor, float y_coor); - - /* Turn right a certain amount of degrees using PID*/ - void pid_right(int deg); - - /* Turn left a certain amount of degrees using PID*/ - void pid_left(int deg); - - /* Drive the wheelchair forward */ + + /************************************************************************* + * This method is to drive the wheelchair forward manually (NO PID used)* + ************************************************************************ */ void forward(); - - /* Drive the wheelchair in reverse*/ + /************************************************************************* + * This method is to drive the wheelchair backwards (NO PID used) * + ************************************************************************ */ void backward(); - - /* Turn the wheelchair right*/ + + /************************************************************************* + * This method is to turn the wheelchair right manually (NO PID used) * + ************************************************************************ */ void right(); - - /* Turn the wheelchair left*/ + + /************************************************************************* + * This method is to turn the wheelchair left manually (NO PID used) * + ************************************************************************ */ void left(); - - /* Stop the wheelchair*/ + + /************************************************************************* + * This method stops the wheelchair and reset the joystick position * + ************************************************************************ */ void stop(); - - /* Functions to get IMU data*/ + + /************************************************************************* + * 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(); + //void tof_encoder_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 * + * or crash into something. If the sensors detect something close to * + * the chair, then the chair will safely halt and allow movement in the * + * direction opposed to where an object is detected. * + ************************************************************************ */ void assistSafe_thread(); - - /* Move x millimiters foward using PID*/ + /************************************************************************* + * 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 * + *************************************************************************/ + void pid_twistA(); - /* Obtain angular position for Twist message */ + /************************************************************************* + * This method computes the relative velocity for Twist message in ROS * + *************************************************************************/ + void pid_twistV(); + + /************************************************************************* + * This method computes the angular position for Twist message in ROS * + *************************************************************************/ double getTwistZ(); - /* Gets the encoder distance moved since encoder reset*/ - float getDistance(); - - /* Resets encoder*/ - void resetDistance(); - - /* Function to determine whether we are turning left or right*/ - void pid_turn(int deg); + /************************************************************************* + * This method calculates the relative position of the chair everytime * + * the encoders reset by setting its old position as the origin to * + * calculate the new position. Moreover, this function is important to * + * be able to publish (send) the information to ROS * + *************************************************************************/ + void odomMsg(); - /* Function to obtain angular and linear velocity */ - void pid_twistA(); - void pid_twistV(); - - /* Function to publish and show Odometry message */ - void odomMsg(); + /************************************************************************* + * This method prints the Odometry message to the serial monitor * + *************************************************************************/ void showOdom(); - - /* Functions with a predetermined path (demo) */ + + /************************************************************************* + * (not being used) Functions with a predetermined path (demo) * + *************************************************************************/ + + void desk(); void kitchen(); void desk_to_kitchen(); - - /* Variables for postion, angle, and velocity */ + + /************************************************************************* + * Public Variables * + **************************************************************************/ double x_position; double y_position; - double z_angular; + double z_angular; double curr_vel; double z_twistA; double linearV; double angularV; double vel; - double test1, test2; + double test1, test2; bool forwardSafety; double curr_yaw, curr_velS; // Variable that contains current relative angle - + int ToFV[12]; + //Needed to send encoder values to ROS + double dist_new; + double dist_newS; private: /* Pointers for the joystick speed */ @@ -130,6 +227,6 @@ QEI* wheelS; // Pointer to encoder VL53L1X** ToF; // Arrays of pointers to ToF sensors - + }; #endif \ No newline at end of file