File content as of revision 36:24c12658bef1:

#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