Revised for integration
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 27:d52b53419aeb
- Parent:
- 26:e06e67411fc9
--- a/wheelchair.cpp Wed Jun 26 18:20:11 2019 +0000 +++ b/wheelchair.cpp Wed Jun 26 22:58:33 2019 +0000 @@ -12,8 +12,8 @@ double dist_old, curr_pos; // Variables for odometry position -DigitalOut signal(D5); -DigitalIn button(D4, PullDown); +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 @@ -60,7 +60,7 @@ void Wheelchair::assistSafe_thread() { int ToFV[12]; - for(int i = 0; i < 9; i++) // reads from the ToF Sensors + for(int i = 0; i < 9; i++) // Reads from the ToF Sensors { ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); @@ -105,7 +105,7 @@ 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); } @@ -174,13 +174,14 @@ */ void Wheelchair::pid_right(int deg) { - bool overturn = false; //Boolean if angle over 360˚ + 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) { @@ -227,7 +228,7 @@ */ void Wheelchair::pid_left(int deg) { - bool overturn = false; //Boolean if angle under 0˚ + 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 @@ -241,7 +242,7 @@ } 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.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 */