Jesus Fausto
1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Revision 30:b24d73663499, committed 2020-06-03
- Comitter:
- jvfausto
- Date:
- Wed Jun 03 20:25:18 2020 +0000
- Parent:
- 29:4519f4cdcb5d
- Commit message:
- For sharing
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 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()
{
diff -r 4519f4cdcb5d -r b24d73663499 wheelchair.h
--- a/wheelchair.h Mon Jul 01 16:36:47 2019 +0000
+++ b/wheelchair.h Wed Jun 03 20:25:18 2020 +0000
@@ -9,32 +9,40 @@
#include "VL53L1X.h"
#include "statistics.h"
#include "Watchdog.h"
-
+
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
-
+
/*************************************************************************
* Joystick has analog out of 200-700, scale values between 1.3 and 3.3 *
* Here are some global constants for joystick *
**************************************************************************/
-#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
-#define offset .03f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
-#define process .1 //Defines default time delay in seconds
+#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
+#define offset .03f // Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
+#define process .1 // Defines default time delay in seconds
/*************************************************************************
*Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)*
**************************************************************************/
-#define xDir PA_6 //* PWM Pins */
+#define xDir PA_6 // PWM Pins
#define yDir PA_5
-#define Encoder1 D7 //*Digital In Pull Up Pin */
+#define Encoder1 D7 // Digital In Pull Up Pin
#define Encoder2 D8
-#define Diameter 31.75 //Diameter of encoder wheel
+#define Diameter 31.75 // Diameter of encoder wheel
#define maxDecelerationSlow 120
#define maxDecelerationFast 30
#define ToFSensorNum 12
-
+
+/*************************************************************************
+*IMU definitions for turning wheelchair
+**************************************************************************/
+#define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm)
+#define MAX_ANGULAR_DECELERATION 60 //found through testing, max
+ //acceleration at which chair can
+ //stop while turning. In degree per sec
+#define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm)
/*************************************************************************
* *
* Wheelchair class *
@@ -51,12 +59,12 @@
**************************************************************************/
Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
VL53L1X** ToF);
-
+
/*************************************************************************
* This method is to move using the joystick *
**************************************************************************/
void move(float x_coor, float y_coor);
-
+
/*************************************************************************
* This method is to drive the wheelchair forward manually (NO PID used)*
************************************************************************ */
@@ -65,37 +73,37 @@
* This method is to drive the wheelchair backwards (NO PID used) *
************************************************************************ */
void backward();
-
+
/*************************************************************************
* This method is to turn the wheelchair right manually (NO PID used) *
************************************************************************ */
void right();
-
+
/*************************************************************************
* This method is to turn the wheelchair left manually (NO PID used) *
************************************************************************ */
void left();
-
+
/*************************************************************************
* This method stops the wheelchair and reset the joystick position *
************************************************************************ */
void stop();
-
+
/*************************************************************************
* This method is a thread that will obtain the IMU information such *
* as the gyroscope x,y axis. Z-axis is not used. *
************************************************************************ */
void compass_thread();
-
+
/*************************************************************************
* This method is a thread that will calculate the velocity of the *
* wheechair using the encoder values this is being obatined. *
************************************************************************ */
void velocity_thread();
-
+
//not being used
void rosCom_thread();
-
+
/*************************************************************************
* This method is a thread that iterates through all the sensor's *
* values and determines whether or not the wheelchair is about to hit *
@@ -103,61 +111,61 @@
* the chair, then the chair will safely halt and allow movement in the *
* direction opposed to where an object is detected. *
************************************************************************ */
- void assistSafe_thread();
-
+ void ToFSafe_thread();
+
/*************************************************************************
* This method is a thread that will constantly be checking the value *
* of the emergency button. If the button is pressed, then the chair *
* will stop and the entire system will reset. *
************************************************************************ */
void emergencyButton_thread();
-
+
/*************************************************************************
* This method gets the encoder values and calculates the distance since*
* the last encoder reset. *
**************************************************************************/
float getDistance();
-
+
/*************************************************************************
* This method resets the encoder value to recalculate distance *
**************************************************************************/
void resetDistance();
-
-
+
+
/*************************************************************************
* *
* PID Control Methods *
* *
*************************************************************************/
-
-
+
+
/*************************************************************************
* This method moves the wheelchair x-millimiters forward using PID *
**************************************************************************/
void pid_forward(double mm);
-
+
/*************************************************************************
* This method turns the chair right a certain amount of degrees using PID*
**************************************************************************/
void pid_right(int deg);
-
+
/*************************************************************************
* This method turns the chair left a certain amount of degrees using PID *
**************************************************************************/
void pid_left(int deg);
-
+
/*************************************************************************
* This method determines whether to turn the wheelchair left or right *
**************************************************************************/
void pid_turn(int deg);
-
-
+
+
/*************************************************************************
* *
* ROS-Embed Communication Methods *
* *
*************************************************************************/
-
+
/*************************************************************************
* This method computes the relative angle for Twist message in ROS *
*************************************************************************/
@@ -185,14 +193,14 @@
* This method prints the Odometry message to the serial monitor *
*************************************************************************/
void showOdom();
-
+
/*************************************************************************
* (not being used) Functions with a predetermined path (demo) *
*************************************************************************/
void desk();
void kitchen();
void desk_to_kitchen();
-
+
/*************************************************************************
* Public Variables *
**************************************************************************/
@@ -206,33 +214,36 @@
double vel;
double test1, test2;
bool forwardSafety;
+ bool sideSafety; //to check if can turn
double curr_yaw, curr_velS; // Variable that contains current relative angle
-
+
private:
int runningAverage[4];
// Expected data used to compare whether of not there is a ledge. This serves as a ground base
// Array is used for calibrating the time of flight sensors
// Used to calculate stdev and mean on
- int ledgeArrayLF[100];
- int ledgeArrayRF[100];
-
+
+
+
/* 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
QEI* wheel; // Pointer to encoder
QEI* wheelS; // Pointer to encoder
VL53L1X** ToF; // Arrays of pointers to ToF sensors
-
-
+
+
+
+
};
#endif
\ No newline at end of file