Affordable Smart Wheelchair
Fully integrated ToF/IMU codes
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchairControlSideTof/wheelchair.cpp
- Revision:
- 18:a10277a63b53
- Child:
- 20:3c1b58654e67
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelchairControlSideTof/wheelchair.cpp Tue Jul 02 17:27:51 2019 +0000
@@ -0,0 +1,647 @@
+#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
+// int* ToFDataPointer1;
+// int* ToFDataPointer2;
+
+int ledgeArrayLF[150];
+int ledgeArrayRF[150];
+int* ToFDataPointer1 = ledgeArrayLF;
+int* ToFDataPointer2 = ledgeArrayRF;
+statistics LFTStats(ToFDataPointer1, 149, 1);
+statistics RFTStats(ToFDataPointer2, 149, 1);
+int k = 0;
+
+double dist_old, curr_pos; // Variables for odometry position
+double outlierToF[4];
+
+
+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::ToFSafe_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");
+
+ k++;
+
+ if (k == 150) {
+ k = 0;
+ }
+
+ ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
+ ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
+ /*for(int i = 0; i < 100; i++)
+ {
+ out->printf("%d, ",ledgeArrayRF[i]);
+ }
+ out->printf("\r\n");*/
+// statistics LFTStats(ToFDataPointer1, 99, 1);
+// statistics RFTStats(ToFDataPointer1, 99, 1);
+ //out->printf("Right Mean: %f ", RFTStats.mean());
+ //out->printf("Std Dev: % f", RFTStats.stdev());
+ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
+ //out->printf("Left Mean: %f ", LFTStats.mean());
+ //out->printf("Std Dev: %f ", LFTStats.stdev());
+ outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
+ //out->printf("New outliers: %f, %f\n", outlierToF[0], outlierToF[1]);
+
+
+
+ for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors
+ runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
+ }
+
+ int sensor1 = ToFV[0];
+ int sensor4 = ToFV[3];
+ //out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]);
+ 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; // You cannot move forward
+ }
+ }
+ 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 if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) {
+ forwardSafety = 1;
+ out->printf("I'M STOPPING BECAUSE OF A LEDGE\r\n");
+ }
+
+ else
+ forwardSafety = 0;
+
+ /*-------Side Tof begin----------*/
+
+ int sensor3 = ToFV[2]; //front left
+ int sensor6 = ToFV[5]; //front right
+ int sensor9 = ToFV[8]; //back
+ int sensor12 = ToFV[11]; //back
+
+ //float currAngularVelocity = IMU DATA; //Current angular velocity from IMU
+ //float angle; //from IMU YAW, convert to cm
+ //float arcLength = angle * WHEELCHAIR_RADIUS; //S = r*Ө
+
+ //Clear the front side first, else continue going straight or can't turn
+ //After clearing the front sideand movinf forward, check if can clear
+ // the back when turning
+
+ //Check if can clear side
+
+ //When either sensors too close to the wall, can't turn
+ if((sensor3 <= MIN_WALL_LENGTH) || (sensor6 <= MIN_WALL_LENGTH) ||
+ (sensor12 <= MIN_WALL_LENGTH)) {
+ sideSafety = 1;
+ out-> printf("Detecting wall!\n");
+ }
+
+ //Check whether safe to keep turnin, user control <-- make sure
+ //currAngularVelocity is in correct units. Know the exact moment you can
+ //stop the chair going at a certain speed before its too late
+ //else if((currAngularVelocity * currAngularVelocity > 2 *
+ // MAX_ANGULAR_DECELERATION * angle) && (sensor3 <= angle ||
+ // sensor6 <= angle)) {
+ // sideSafety = 1; //Not safe to turn
+ //}
+ //Safe to continue turning
+ else {
+ sideSafety = 0;
+ }
+
+ /*-------Side Tof end -----------*/
+}
+
+/* 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;
+ for(int i = 0; i < 10; i++)
+ {
+ (*(ToF+1))->readFromOneSensor();
+ (*(ToF+1))->readFromOneSensor();
+ }
+ for(int i = 0; i < 150; i++)
+ {
+ ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor();
+ ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
+ }
+
+
+ //statistics LFTStats(ToFDataPointer1, 99, 1);
+// //ToFDataPointer = ledgeArrayRF;
+ //statistics RFTStats(ToFDataPointer2, 99, 1);
+
+
+ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
+ outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
+
+ //out->printf("Left outlier = %f\n", outlierToF[0]);
+ //out->printf("Right outlier = %f\n", outlierToF[1]);
+
+ //out->printf("Left statistics = %f, %f\n", LFTStats.mean(), LFTStats.stdev());
+ //out->printf("Right statistics = %f, %f\n", RFTStats.mean(), RFTStats.stdev());
+ 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()
+{
+ //if safe to move, from ToFSafety
+ if(sideSafety == 0) {
+ out->printf("Moving right!\n");
+ x->write(def);
+ y->write(low);
+ }
+}
+
+ /* Automatic mode: move left and update x,y coordinate sent to chair */
+void Wheelchair::left()
+{
+ //if safe to move, from ToFSafety
+ if(sideSafety == 0) {
+ out->printf("Moving left!\n");
+ 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);
+}
\ No newline at end of file