Jesus Fausto
1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Diff: wheelchair.cpp
- Revision:
- 30:b24d73663499
- Parent:
- 27:da718b990837
diff -r 4519f4cdcb5d -r b24d73663499 wheelchair.cpp
--- a/wheelchair.cpp Mon Jul 01 16:36:47 2019 +0000
+++ b/wheelchair.cpp Wed Jun 03 20:25:18 2020 +0000
@@ -1,4 +1,3 @@
-
#include "wheelchair.h"
bool manual_drive = false; // Variable changes between joystick and auto drive
@@ -9,9 +8,20 @@
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
@@ -19,14 +29,14 @@
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()
{
@@ -34,11 +44,29 @@
curr_velS = wheelS->getVelocity();
curr_pos = wheel->getDistance(53.975);
}
-
-void Wheelchair::assistSafe_thread()
+
+void Wheelchair::emergencyButton_thread ()
+{
+ while(1) {
+ while(!e_button) {
+
+ //Stop wheelchair
+ Wheelchair::stop();
+ printf("E-button has been pressed\r\n");
+ off->write(high); // Turn off PCB
+ on->write(0); // Make sure PCB not on
+ //Reset Board
+ NVIC_SystemReset();
+
+ }
+
+ }
+}
+
+void Wheelchair::ToFSafe_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]);
@@ -46,43 +74,101 @@
//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");*/
+
+ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
+ outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
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;
+ 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;
+ forwardSafety = 1; // You cannot move forward
}
}
+
+ 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;
+ }
+
+ // 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)
@@ -90,9 +176,11 @@
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");
@@ -103,31 +191,37 @@
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;
- for(int i = 0; i < 100; i++)
+ 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+1))->readFromOneSensor();
+ ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
}
- int* aaa = ledgeArrayLF;
- statistics LFTStats(aaa, 100, 0);
- out->printf("stadistics = %f, %f", LFTStats.mean(), LFTStats.stdev());
+
+ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
+ outlierToF[1] = RFTStats.mean() + 2*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);
@@ -155,15 +249,21 @@
/* Automatic mode: move right and update x,y coordinate sent to chair */
void Wheelchair::right()
{
- x->write(def);
- y->write(low);
+ //if safe to move, from ToFSafety
+ if(sideSafety == 0) {
+ 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);
+ //if safe to move, from ToFSafety
+ if(sideSafety == 0) {
+ x->write(def);
+ y->write(high);
+ }
}
/* Stop the wheelchair */
@@ -172,7 +272,7 @@
x->write(def);
y->write(def);
}
-
+
/* Counter-clockwise is -
* Clockwise is +
* Range of deg: 0 to 360
@@ -180,7 +280,7 @@
*/
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
@@ -247,7 +347,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 */
@@ -277,7 +377,7 @@
/* Saftey stop for wheelchair */
Wheelchair::stop();
out->printf("done \r\n");
-
+
}
/* This constructor determines whether to turn left or right */
@@ -308,9 +408,9 @@
{
Wheelchair::pid_left(turnAmt);
}
-
+
}
-
+
/* This constructor takes in distance to travel and adjust to move forward */
void Wheelchair::pid_forward(double mm)
{
@@ -336,31 +436,30 @@
{
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);
@@ -396,7 +495,7 @@
}
}
-
+
/* This constructor computes the relative velocity for Twist message in ROS */
void Wheelchair::pid_twistV()
{
@@ -434,7 +533,7 @@
{
x->write(def);
y->write(def);
-
+
vel = 0;
vDesired = 0;
dist_old = 0;
@@ -482,7 +581,7 @@
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
*/
@@ -498,26 +597,26 @@
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 */
+
+/* This constructor resets the wheel encoders */
void Wheelchair::resetDistance()
{
wheel->reset();
}
-
-
+
+
/*Predetermined paths For Demmo*/
void Wheelchair::desk()
{