Diff: wheelchair.h

Revision:

21:3489cffad196

Parent:

20:f42db4ae16f0

Child:

26:662693bd7f31

--- a/wheelchair.h	Fri Aug 31 20:00:01 2018 +0000
+++ b/wheelchair.h	Fri Apr 19 23:04:01 2019 +0000
@@ -1,37 +1,37 @@
 #ifndef wheelchair
 #define wheelchair
 
+/* Importing libraries into wheelchair.h */
 #include "chair_BNO055.h"
 #include "PID.h"
 #include "QEI.h"
+#include "VL53L1X.h"
+
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
-//#include "BufferedSerial.h"
+#include <nav_msgs/Odometry.h>
 
-//#include "chair_MPU9250.h"
 
-#define turn_precision 10
-#define def (2.5f/3.3f)
-#define high 3.3f/3.3f
-#define offset .02742f
-#define low (1.7f/3.3f)
-#define process .1
+/*
+* Joystick has analog out of 200-700, scale values between 1.3 and 3.3
+*/             
+#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 .035f                    //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
+#define process .1                      //Defines default time delay in seconds
 
-/* for big mbed board
-#define xDir D12 //top right two pins
-#define yDir D13 //top left two pins
-#define Encoder1 D0
-#define Encoder2 D1
-*/
+/* Pin plug in for Nucleo-L432KC */
+#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 maxDeceleration 130
 
-//for small mbed board
-#define xDir D9
-#define yDir D10
-#define Encoder1 D7
-#define Encoder2 D8
+#define ToFSensorNum 12
 
-#define EncoderReadRate 1200
-#define Diameter 31.75
+
 /** Wheelchair class
  * Used for controlling the smart wheelchair
  */
@@ -42,60 +42,94 @@
     /** Create Wheelchair Object with x,y pin for analog dc output
      * serial for printout, and timer
      */
-    Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel);
+    Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
+     VL53L1X** ToF);
     
-    /** move using the joystick */
+    /** Move using the joystick */
     void move(float x_coor, float y_coor);
     
-    /* turn right a certain amount of degrees (overshoots)*/
-    double turn_right(int deg);
-    
-    /* turn left a certain amount of degrees (overshoots)*/
-    double turn_left(int deg);
-    
-    /* turn right a certain amount of degrees using PID*/
+    /* Turn right a certain amount of degrees using PID*/
     void pid_right(int deg);
     
-    /* turn left a certain amount of degrees using PID*/
+    /* Turn left a certain amount of degrees using PID*/
     void pid_left(int deg);
     
-    /* turning function that turns any direction */
-    void turn(int deg);
-    
-    /* drive the wheelchair forward */
+    /* Drive the wheelchair forward */
     void forward();
     
-    /* drive the wheelchair backward*/
+    /* Drive the wheelchair in reverse*/
     void backward();
     
-    /* turn the wheelchair right*/
+    /* Turn the wheelchair right*/
     void right();
     
-    /* turn the wheelchair left*/
+    /* Turn the wheelchair left*/
     void left();
     
-    /* stop the wheelchair*/
+    /* Stop the wheelchair*/
     void stop();
     
-    /* function to get imu data*/
+    /* Functions to get IMU data*/
     void compass_thread();
-    void distance_thread();
+    void velocity_thread();
+    void rosCom_thread();
+    void assistSafe_thread();
+
+    
+    /* Move x millimiters foward using PID*/
     void pid_forward(double mm);
-    void pid_reverse(double mm);
+    
+    /* Obtain angular position for Twist message */
+    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);
+    
+    /* Function to obtain angular and linear velocity */
+    void pid_twistA();
+    void pid_twistV();
+    
+    /* Function to publish and show Odometry message */
+    void odomMsg();
+    void showOdom();
+    
+    /* Functions with a predetermined path (demo) */
     void desk();
     void kitchen();
     void desk_to_kitchen();
+    
+    /* Variables for postion, angle, and velocity */
+    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;
+    double curr_yaw, curr_velS;                                                            // Variable that contains current relative angle
+
+    
 private:
+    /* Pointers for the joystick speed */
     PwmOut* x;
     PwmOut* y;
-    chair_BNO055* imu;
-    Serial* out;
-    Timer* tim;
-    Timer* ti;
-    QEI* wheel;
+    
+    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