added code for PCB on and off, added pointers to e_button and pwm pins

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

Dependents:   wheelchairControlSumer2019

Files at this revision

API Documentation at this revision

Comitter:
JesiMiranda
Date:
Fri Jun 28 19:35:51 2019 +0000
Parent:
27:0b1a837f123c
Commit message:
added code for PCB on and off, added pointers to e_button and pwm pins

Changed in this revision

wheelchair.cpp Show annotated file Show diff for this revision Revisions of this file
wheelchair.h Show annotated file Show diff for this revision Revisions of this file
diff -r 0b1a837f123c -r ad02cb329fe3 wheelchair.cpp
--- a/wheelchair.cpp	Thu Jun 27 18:08:29 2019 +0000
+++ b/wheelchair.cpp	Fri Jun 28 19:35:51 2019 +0000
@@ -1,9 +1,9 @@
 
 #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
@@ -12,64 +12,60 @@
 
 double dist_old, curr_pos;                                                             // Variables for odometry position
 
-DigitalOut signal(D5);                                                                 // Output to send "STOP" signal to wheelchair 
-DigitalIn button(D4, PullDown);                                                        // Emergency Stop button 
- 
+
 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;
+void Wheelchair::compass_thread()
+{
+    curr_yaw = imu->yaw();
+    z_angular = curr_yaw;
 }
 
 /* Thread measures velocity of wheels and distance traveled */
-void Wheelchair::velocity_thread() 
+void Wheelchair::velocity_thread()
 {
     curr_vel = wheel->getVelocity();
     curr_velS = wheelS->getVelocity();
     curr_pos = wheel->getDistance(53.975);
 }
 
-void Wheelchair::emergencyButton_thread () {
+void Wheelchair::emergencyButton_thread ()
+{
     while(1) {
-        signal = 0;
-        while(!button) {
-                    
-            //Send something to stop wheelchair
-            signal = 1;
-            
-            printf("Hello there!\n");
-            printf("I'm dead\n\n\n");
-            
+        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
-    {
+    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");
+    }
+    //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) && 
+    /*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)
     {
@@ -80,8 +76,8 @@
             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) && 
+    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)
     {
@@ -99,14 +95,14 @@
 }
 
 /* Constructor for Wheelchair class */
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, 
-VL53L1X** ToFT)
+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);                                                               
+    x = new PwmOut(xPin);
     y = new PwmOut(yPin);
     /* Initializes IMU Library */
     out = pc;                                                                           // "out" is called for serial monitor
@@ -115,68 +111,66 @@
     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
     imu->setup();                                                                       // turns on the IMU
     wheelS = qeiS;                                                                      // "wheel" is called for encoder
-    wheel = qei;   
+    wheel = qei;
     ToF = ToFT;                                                                         // passes pointer with addresses of ToF sensors
-    
-    for(int i = 0; i < 12; i++)                                               // initializes the 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)                                
+void Wheelchair::move(float x_coor, float y_coor)
 {
-  /* Scales one joystick measurement to the chair's joystick measurement */
+    /* 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);                                                         
+    /* 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()                                                              
+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);
+    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()                    
+void Wheelchair::backward()
 {
     x->write(low);
     y->write(def);
 }
- 
+
 /* Automatic mode: move right and update x,y coordinate sent to chair */
-void Wheelchair::right()                                                             
+void Wheelchair::right()
 {
     x->write(def);
     y->write(low);
 }
 
- /* Automatic mode: move left and update x,y coordinate sent to chair */
-void Wheelchair::left()                                                               
+/* 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()                                                       
+void Wheelchair::stop()
 {
     x->write(def);
     y->write(def);
@@ -185,137 +179,121 @@
 /* Counter-clockwise is -
  * Clockwise is +
  * Range of deg: 0 to 360
- * This constructor takes in an angle from user and adjusts for turning right 
+ * 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");                                
+
+    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) 
-    {                                                               
+    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 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 
-        {
+        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 
-        
+        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("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();                                                                 
+    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)                                                      
+void Wheelchair::pid_left(int deg)
 {
     bool overturn = false;                                                              //Boolean if angle under 0˚
-    
-    out->printf("pid Left\r\r\n");                                                     
+
+    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) 
-    {                                                                 
+    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;
-       }
-     
+    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();                                                                 
+
+    /* 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)
-    {                                                                   
+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;
     }
-    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);                        
+    if(deg >= 0) {
+        Wheelchair::pid_right(turnAmt);
+    } else {
+        Wheelchair::pid_left(turnAmt);
     }
 
 }
@@ -326,44 +304,43 @@
     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.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
-        {                                                    
+    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                    
+
+        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()
@@ -371,40 +348,38 @@
     /* Initialize variables for angle and update x,y sent to chair */
     char c;
     double temporA = def;
-    y->write(def); 
-    x->write(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)
-    {
+    while(1) {
         yDesired = angularV;
-     
+
         /* Update and set all variable so that the chair is stationary
          * if the desired angle is zero
          */
-        if(yDesired == 0)
-        {
+        if(yDesired == 0) {
             x->write(def);
             y->write(def);
             yDesired = 0;
             return;
         }
-         
+
         /* Continuously updates with current angle measured by IMU */
-        yIn = imu->gyro_z(); 
+        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()
@@ -417,30 +392,28 @@
     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);     
- 
+    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);                                            
- 
+    PIDVelosity.SetOutputLimits(-.005, .005);
+    PIDSlaveV.SetOutputLimits(-.002, .002);
+
     /* PID mode: Direct */
-    PIDVelosity.SetControllerDirection(DIRECT); 
-    PIDSlaveV.SetControllerDirection(DIRECT); 
- 
-    while(1)
-    {
+    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)
-        {
+        /* 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);
 
@@ -449,21 +422,17 @@
             dist_old = 0;
             return;
         }
-     
-        if(vDesired >= 0)
-        {
+
+        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
-        {
+        } 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)
-        {
+        if(temporV >= 1.5) {
             temporV = 1.5;
         }
         /* Scales and makes some adjustments to velocity */
@@ -471,18 +440,15 @@
         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
-        {
+        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);
         }
@@ -501,18 +467,18 @@
     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);
- }
+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()
@@ -527,8 +493,8 @@
 }
 
 
-/*Predetermined paths For Demmo*/    
-void Wheelchair::desk() 
+/*Predetermined paths For Demmo*/
+void Wheelchair::desk()
 {
     Wheelchair::pid_forward(5461);
     Wheelchair::pid_right(87);
@@ -536,8 +502,8 @@
     Wheelchair::pid_right(87);
     Wheelchair::pid_forward(3658);
 }
- 
-void Wheelchair::kitchen() 
+
+void Wheelchair::kitchen()
 {
     Wheelchair::pid_forward(5461);
     Wheelchair::pid_right(87);
@@ -545,7 +511,7 @@
     Wheelchair::pid_left(90);
     Wheelchair::pid_forward(305);
 }
- 
+
 void Wheelchair::desk_to_kitchen()
 {
     Wheelchair::pid_right(180);
diff -r 0b1a837f123c -r ad02cb329fe3 wheelchair.h
--- a/wheelchair.h	Thu Jun 27 18:08:29 2019 +0000
+++ b/wheelchair.h	Fri Jun 28 19:35:51 2019 +0000
@@ -14,7 +14,7 @@
 
 /*
 * 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
@@ -31,7 +31,6 @@
 #define maxDecelerationFast 30
 #define ToFSensorNum 12
 
-
 /** Wheelchair class
  * Used for controlling the smart wheelchair
  */
@@ -43,86 +42,91 @@
      * serial for printout, and timer
      */
     Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
-     VL53L1X** ToF);
-    
+               VL53L1X** ToF);
+
     /** 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 */
     void forward();
-    
+
     /* Drive the wheelchair in reverse*/
     void backward();
-    
+
     /* Turn the wheelchair right*/
     void right();
-    
+
     /* Turn the wheelchair left*/
     void left();
-    
+
     /* Stop the wheelchair*/
     void stop();
-    
+
     /* Functions to get IMU data*/
     void compass_thread();
     void velocity_thread();
     void rosCom_thread();
     void assistSafe_thread();
     void emergencyButton_thread();
-    
+
     /* Move x millimiters foward using PID*/
     void pid_forward(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 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
 
-    
+
 private:
     /* 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
@@ -130,6 +134,6 @@
     QEI* wheelS;                        // Pointer to encoder
     VL53L1X** ToF;                      // Arrays of pointers to ToF sensors
 
-    
+
 };
 #endif
\ No newline at end of file