File content as of revision 28:ad02cb329fe3:

#include "wheelchair.h"

bool manual_drive = false;                                                             // Variable changes between joystick and auto drive
double encoder_distance;                                                               // Keeps distanse due to original position

volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;              // Variables for PID
volatile double vIn, vOut, vDesired;                                                   // Variables for PID Velosity
volatile double vInS, vOutS, vDesiredS;                                                // Variables for PID Slave Wheel
volatile double yIn, yOut, yDesired;                                                   // Variables for PID turn velosity

double dist_old, curr_pos;                                                             // Variables for odometry position


PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);        // Distance PID object constructor
PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);               // Velosity PID Constructor
PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);              // Slave Velosity PID Constructor
PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);               // Angular Velosity PID Constructor


/* Thread measures current angular position */
void Wheelchair::compass_thread()
{
    curr_yaw = imu->yaw();
    z_angular = curr_yaw;
}

/* Thread measures velocity of wheels and distance traveled */
void Wheelchair::velocity_thread()
{
    curr_vel = wheel->getVelocity();
    curr_velS = wheelS->getVelocity();
    curr_pos = wheel->getDistance(53.975);
}

void Wheelchair::emergencyButton_thread ()
{
    while(1) {
        while(!e_button) {

            //Stop wheelchair
            Wheelchair::stop();
            pc.printf("E-button has been pressed\r\n");
            j_off.write(high);                              //turn off PCB
            j_on.write(0);                                  //make sure PCB not on
            //Reset Board
            NVIC_SystemReset();

        }

    }
}

void Wheelchair::assistSafe_thread()
{
    int ToFV[12];
    for(int i = 0; i < 6; i++) {                                            // reads from the ToF Sensors
        ToFV[i] = (*(ToF+i))->readFromOneSensor();
        //out->printf("%d ", ToFV[i]);
    }
    //out->printf("\r\n");
    int sensor1 = ToFV[1];
    int sensor4 = ToFV[5];
    //out->printf("%d, %d\r\n", sensor1, sensor4);
    /*if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 ||
    2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) &&
    (sensor1 < 1500 || sensor4 < 1500)) ||
    550 > sensor1 || 550 > sensor4)
    {
        //out->printf("i am in danger\r\n");
        if(x->read() > def)
        {
            x->write(def);
            forwardSafety = 1;
        }
    }
    else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 ||
    2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) &&
    (sensor1 < 1500 || sensor4 < 1500)) ||
    550 > sensor1 || 550 > sensor4)
    {
        //out->printf("i am in danger\r\n");
        if(x->read() > def)
        {
            x->write(def);
            forwardSafety = 1;
        }
    }
    else
        forwardSafety = 0;
    }
    */
}

/* Constructor for Wheelchair class */
Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS,
                       VL53L1X** ToFT)
{
    x_position = 0;
    y_position = 0;
    forwardSafety = 0;
    /* Initializes X and Y variables to Pins */
    x = new PwmOut(xPin);
    y = new PwmOut(yPin);
    /* Initializes IMU Library */
    out = pc;                                                                           // "out" is called for serial monitor
    out->printf("on\r\n");
    imu = new chair_BNO055(pc, time);
    Wheelchair::stop();                                                                 // Wheelchair is initially stationary
    imu->setup();                                                                       // turns on the IMU
    wheelS = qeiS;                                                                      // "wheel" is called for encoder
    wheel = qei;
    ToF = ToFT;                                                                         // passes pointer with addresses of ToF sensors

    for(int i = 0; i < 12; i++) {                                             // initializes the ToF Sensors
        (*(ToF+i))->initReading(0x31+((0x02)*i), 50000);
    }

    out->printf("wheelchair setup done \r\n");                                          // Make sure it initialized; prints in serial monitor
    ti = time;


    myPID.SetMode(AUTOMATIC);                                                           // PID mode: Automatic
}

/* Move wheelchair with joystick on manual mode */
void Wheelchair::move(float x_coor, float y_coor)
{
    /* Scales one joystick measurement to the chair's joystick measurement */
    float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
    float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;

    /* Sends the scaled joystic values to the chair */
    x->write(scaled_x);
    y->write(scaled_y);
}

/* Automatic mode: move forward and update x,y coordinate sent to chair */
void Wheelchair::forward()
{
    //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS);
    if(forwardSafety == 0) {
        x->write(high);
        y->write(def+offset);
    }
    out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975));
}

/* Automatic mode: move in reverse and update x,y coordinate sent to chair */
void Wheelchair::backward()
{
    x->write(low);
    y->write(def);
}

/* Automatic mode: move right and update x,y coordinate sent to chair */
void Wheelchair::right()
{
    x->write(def);
    y->write(low);
}

/* Automatic mode: move left and update x,y coordinate sent to chair */
void Wheelchair::left()
{
    x->write(def);
    y->write(high);
}

/* Stop the wheelchair */
void Wheelchair::stop()
{
    x->write(def);
    y->write(def);
}

/* Counter-clockwise is -
 * Clockwise is +
 * Range of deg: 0 to 360
 * This constructor takes in an angle from user and adjusts for turning right
 */
void Wheelchair::pid_right(int deg)
{
    bool overturn = false;                                                              //Boolean if angle over 360˚

    out->printf("pid right\r\r\n");
    x->write(def);                                                                      // Update x sent to chair to be stationary
    Setpoint = curr_yaw + deg;                                                          // Relative angle we want to turn
    pid_yaw = curr_yaw;                                                                 // Sets pid_yaw to angle input from user

    /* Turns on overturn boolean if setpoint over 360˚ */
    if(Setpoint > 360) {
        overturn = true;
    }

    myPID.SetTunings(5.5,0, 0.0035);                                                    // Sets the constants for P and D
    myPID.SetOutputLimits(0, def-low-.15);                                              // Limit is set to the differnce between def and low
    myPID.SetControllerDirection(DIRECT);                                               // PID mode: Direct

    /* PID stops when approaching a litte less than desired angle */
    while(pid_yaw < Setpoint - 3) {
        /* PID is set to correct angle range if angle greater than 360˚*/
        if(overturn && curr_yaw < Setpoint-deg-1) {
            pid_yaw = curr_yaw + 360;
        } else {
            pid_yaw = curr_yaw;
        }

        myPID.Compute();                                                                // Does PID calculations
        double tempor = -Output+def;                                                    // Temporary value with the voltage output
        y->write(tempor);                                                               // Update y sent to chair

        /* Prints to serial monitor the current angle and setpoint */
        out->printf("curr_yaw %f\r\r\n", curr_yaw);
        out->printf("Setpoint = %f \r\n", Setpoint);

        wait(.05);                                                                      // Small delay (milliseconds)
    }

    /* Saftey stop for wheelchair */
    Wheelchair::stop();
    out->printf("done \r\n");
}

/* Counter-clockwise is -
 * Clockwise is +
 * Range of deg: 0 to 360
 * This constructor takes in an angle from user and adjusts for turning left
 */
void Wheelchair::pid_left(int deg)
{
    bool overturn = false;                                                              //Boolean if angle under 0˚

    out->printf("pid Left\r\r\n");
    x->write(def);                                                                      // Update x sent to chair to be stationary
    Setpoint = curr_yaw - deg;                                                          // Relative angle we want to turn
    pid_yaw = curr_yaw;                                                                 // Sets pid_yaw to angle input from user

    /* Turns on overturn boolean if setpoint less than 0˚ */
    if(Setpoint < 0) {
        overturn = true;
    }

    myPID.SetTunings(5,0, 0.004);                                                       // Sets the constants for P and D
    myPID.SetOutputLimits(0,high-def-.12);                                              //Limit is set to the differnce between def and low
    myPID.SetControllerDirection(REVERSE);                                              // PID mode: Reverse

    /* PID stops when approaching a litte more than desired angle */
    while(pid_yaw > Setpoint+3) {
        /* PID is set to correct angle range if angle less than 0˚ */
        if(overturn && curr_yaw > Setpoint+deg+1) {
            pid_yaw = curr_yaw - 360;
        } else {
            pid_yaw = curr_yaw;
        }

        myPID.Compute();                                                                // Does PID calculations
        double tempor = Output+def;                                                     // Temporary value with the voltage output
        y->write(tempor);                                                               // Update y sent to chair

        /* Prints to serial monitor the current angle and setpoint */
        out->printf("curr_yaw %f\r\n", curr_yaw);
        out->printf("Setpoint = %f \r\n", Setpoint);

        wait(.05);                                                                      // Small delay (milliseconds)
    }

    /* Saftey stop for wheelchair */
    Wheelchair::stop();
    out->printf("done \r\n");

}

/* This constructor determines whether to turn left or right */
void Wheelchair::pid_turn(int deg)
{

    /* Sets angle to coterminal angle for left turn if deg > 180
     * Sets angle to coterminal angle for right turn if deg < -180
     */
    if(deg > 180) {
        deg -= 360;
    } else if(deg < -180) {
        deg +=360;
    }

    /* Makes sure angle inputted to function is positive */
    int turnAmt = abs(deg);

    /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */
    if(deg >= 0) {
        Wheelchair::pid_right(turnAmt);
    } else {
        Wheelchair::pid_left(turnAmt);
    }

}

/* This constructor takes in distance to travel and adjust to move forward */
void Wheelchair::pid_forward(double mm)
{
    mm -= 20;                                                                           // Makes sure distance does not overshoot
    Input = 0;                                                                          // Initializes input to zero: Test latter w/o
    wheel->reset();                                                                     // Resets encoders so that they start at 0

    out->printf("pid foward\r\n");

    double tempor;                                                                      // Initializes Temporary variable for x input
    Setpoint = mm;                                                                      // Initializes the setpoint to desired value

    myPIDDistance.SetTunings(5.5,0, 0.0015);                                            // Sets constants for P and D
    myPIDDistance.SetOutputLimits(0,high-def-.15);                                      // Limit set to difference between high and def
    myPIDDistance.SetControllerDirection(DIRECT);                                       // PID mode: Direct

    y->write(def+offset);                                                               // Update y to make chair stationary

    /* Chair stops moving when Setpoint is reached */
    while(Input < Setpoint) {

        if(out->readable()) {                                                           // Emergency Break
            break;
        }

        Input = wheel->getDistance(53.975);                                             // Gets distance from Encoder into PID
        wait(.05);                                                                      // Slight Delay: *****Test without
        myPIDDistance.Compute();                                                        // Compute distance traveled by chair

        tempor = Output + def;                                                          // Temporary output variable
        x->write(tempor);                                                               // Update x sent to chair

        /* Prints to serial monitor the distance traveled by chair */
        out->printf("distance %f\r\n", Input);
    }

}

/* This constructor returns the relative angular position of chair */
double Wheelchair::getTwistZ()
{
    return imu->gyro_z();
}

/* This constructor computes the relative angle for Twist message in ROS */
void Wheelchair::pid_twistA()
{
    /* Initialize variables for angle and update x,y sent to chair */
    char c;
    double temporA = def;
    y->write(def);
    x->write(def);

    PIDAngularV.SetTunings(.00015,0, 0.00);                                             // Sets the constants for P and D
    PIDAngularV.SetOutputLimits(-.1, .1);                                               // Limit set to be in range specified
    PIDAngularV.SetControllerDirection(DIRECT);                                         // PID mode: Direct

    /* Computes angular position of wheelchair while turning */
    while(1) {
        yDesired = angularV;

        /* Update and set all variable so that the chair is stationary
         * if the desired angle is zero
         */
        if(yDesired == 0) {
            x->write(def);
            y->write(def);
            yDesired = 0;
            return;
        }

        /* Continuously updates with current angle measured by IMU */
        yIn = imu->gyro_z();
        PIDAngularV.Compute();
        temporA += yOut;                                                                // Temporary value with the voltage output
        y->write(temporA);                                                              // Update y sent to chair

        //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
        wait(.05);                                                                      // Small delay (milliseconds)
    }

}

/* This constructor computes the relative velocity for Twist message in ROS */
void Wheelchair::pid_twistV()
{
    /* Initializes variables as default */
    double temporV = def;
    double temporS = def+offset;
    vDesiredS = 0;
    x->write(def);
    y->write(def);
    wheel->reset();                                                                     // Resets the encoders
    /* Sets the constants for P and D */
    PIDVelosity.SetTunings(.0005,0, 0.00);
    PIDSlaveV.SetTunings(.005,0.000001, 0.000001);

    /* Limits to the range specified */
    PIDVelosity.SetOutputLimits(-.005, .005);
    PIDSlaveV.SetOutputLimits(-.002, .002);

    /* PID mode: Direct */
    PIDVelosity.SetControllerDirection(DIRECT);
    PIDSlaveV.SetControllerDirection(DIRECT);

    while(1) {
        linearV = .7;
        test1 = linearV*100;
        vel = curr_vel;
        vDesired = linearV*100;
        if(out->readable())
            return;
        /* Update and set all variable so that the chair is stationary
        * if the velocity is zero
        */
        if(linearV == 0) {
            x->write(def);
            y->write(def);

            vel = 0;
            vDesired = 0;
            dist_old = 0;
            return;
        }

        if(vDesired >= 0) {
            PIDVelosity.SetTunings(.000004,0, 0.00);                                    // Sets the constants for P and D
            PIDVelosity.SetOutputLimits(-.002, .002);                                   // Limits to the range specified
        } else {
            PIDVelosity.SetTunings(.000015,0, 0.00);                                    // Sets the constants for P and D
            PIDVelosity.SetOutputLimits(-.0005, .0005);                                 // Limits to range specified
        }

        /* Sets maximum value of variable to 1 */
        if(temporV >= 1.5) {
            temporV = 1.5;
        }
        /* Scales and makes some adjustments to velocity */
        vIn = curr_vel*100;
        vInS = curr_vel-curr_velS;
        PIDVelosity.Compute();
        PIDSlaveV.Compute();
        if(forwardSafety == 0) {
            temporV += vOut;
            temporS += vOutS;

            /* Updates x,y sent to Wheelchair and for Odometry message in ROS */
            x->write(temporV);
            test2 = temporV;
            y->write(temporS);
        } else {
            x->write(def);
            y->write(def);
        }
        //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
        Wheelchair::odomMsg();
        wait(.01);                                                                      // Small delay (milliseconds)
    }
}

/* This constructor calculates the relative position of the chair everytime the encoders reset
 * by setting its old position as the origin to calculate the new position
 */
void Wheelchair::odomMsg()
{
    double dist_new = curr_pos;
    double dist = dist_new-dist_old;
    double temp_x = dist*sin(z_angular*3.14159/180);
    double temp_y = dist*cos(z_angular*3.14159/180);

    x_position += temp_x;
    y_position += temp_y;

    dist_old = dist_new;
}

/* This constructor prints the Odometry message to the serial monitor */
void Wheelchair::showOdom()
{
    out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
}

/* This constructor returns the approximate distance based on the wheel diameter */
float Wheelchair::getDistance()
{
    return wheel->getDistance(Diameter);
}

/* This constructor resets the wheel encoder's */
void Wheelchair::resetDistance()
{
    wheel->reset();
}


/*Predetermined paths For Demmo*/
void Wheelchair::desk()
{
    Wheelchair::pid_forward(5461);
    Wheelchair::pid_right(87);
    Wheelchair::pid_forward(3658);
    Wheelchair::pid_right(87);
    Wheelchair::pid_forward(3658);
}

void Wheelchair::kitchen()
{
    Wheelchair::pid_forward(5461);
    Wheelchair::pid_right(87);
    Wheelchair::pid_forward(3658);
    Wheelchair::pid_left(90);
    Wheelchair::pid_forward(305);
}

void Wheelchair::desk_to_kitchen()
{
    Wheelchair::pid_right(180);
    Wheelchair::pid_forward(3700);
}