Affordable Smart Wheelchair
Fully integrated ToF/IMU codes
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchairControlSideTof/wheelchair.h
- Revision:
- 18:a10277a63b53
- Child:
- 19:5376c858e574
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/wheelchair.h Tue Jul 02 17:27:51 2019 +0000
@@ -0,0 +1,249 @@
+#ifndef wheelchair
+#define wheelchair
+/*************************************************************************
+* Importing libraries into wheelchair.h *
+**************************************************************************/
+#include "chair_BNO055.h"
+#include "PID.h"
+#include "QEI.h"
+#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
+/*************************************************************************
+*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 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 *
+* Used for controlling the Smart Wheelchair *
+* *
+**************************************************************************/
+class Wheelchair
+{
+public:
+ /*************************************************************************
+ * 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);
+
+ /*************************************************************************
+ * 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)*
+ ************************************************************************ */
+ void forward();
+ /*************************************************************************
+ * 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 *
+ * 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 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 *
+ *************************************************************************/
+ void pid_twistA();
+
+ /*************************************************************************
+ * 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();
+
+ /*************************************************************************
+ * 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();
+
+ /*************************************************************************
+ * 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 *
+ **************************************************************************/
+ double x_position;
+ double y_position;
+ double z_angular;
+ double curr_vel;
+ double z_twistA;
+ double linearV;
+ double angularV;
+ 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
+
+
+
+ /* 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