Affordable Smart Wheelchair
Revised for integration
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 25:58ec657a44f2
- Parent:
- 24:d2f234fbc20d
--- a/wheelchair.cpp Sat Oct 27 10:10:14 2018 +0000
+++ b/wheelchair.cpp Sat Oct 27 16:47:50 2018 +0000
@@ -1,27 +1,33 @@
#include "wheelchair.h"
-
+
bool manual_drive = false; // Variable Changes between joystick and auto drive
double curr_yaw, curr_vel, curr_velS; // Variable that contains current relative angle
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;
volatile double vInS, vOutS, vDesiredS;
volatile double yIn, yOut, yDesired;
+double z_angular, dist_old, curr_pos;
+double x_position = 0;
+double y_position = 0;
+
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);
PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);
PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);
-
+
void Wheelchair::compass_thread() { // Thread that measures which angle we are at
curr_yaw = imu->yaw();
+ z_angular = curr_yaw;
}
void Wheelchair::velosity_thread() {
curr_vel = wheel->getVelosity();
curr_velS = wheelS->getVelosity();
+ curr_pos = wheel->getDistance(53.975);
}
Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS) // Function Constructor for Wheelchair class
@@ -41,41 +47,41 @@
ti = time;
myPID.SetMode(AUTOMATIC); // set PID to automatic
}
-
+
void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual
{
-
+
float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the
float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement
x->write(scaled_x); // Sends the scaled joystic values to the chair
y->write(scaled_y);
}
-
+
void Wheelchair::forward() // In auto to move foward
{
x->write(high);
y->write(def+offset);
}
-
+
void Wheelchair::backward() // In auto to move reverse
{
x->write(low);
y->write(def);
}
-
+
void Wheelchair::right() // In auto to move right
{
x->write(def);
y->write(low);
}
-
+
void Wheelchair::left() // In auto to move left
{
x->write(def);
y->write(high);
}
-
+
void Wheelchair::stop() // Stops the chair
{
x->write(def);
@@ -115,13 +121,13 @@
out->printf("curr_yaw %f\r\r\n", curr_yaw);
out->printf("Setpoint = %f \r\n", Setpoint);
-
+
wait(.05); // Small delay
}
Wheelchair::stop(); // Safety Stop
out->printf("done \r\n");
}
-
+
void Wheelchair::pid_left(int deg) // Takes in degree and turns left
{
bool overturn = false; //Boolean if we have to turn under relative 0˚
@@ -154,19 +160,19 @@
}
Wheelchair::stop(); // Safety Stop
}
-
+
void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left
-
+
if(deg > 180) { // If deg > 180 turn left: coterminal angle
deg -= 360;
}
-
+
else if(deg < -180) { // If deg < -180 turn right: coterminal angle
deg+=360;
}
int turnAmt = abs(deg); // Makes sure input angle is positive
-
+
if(deg >= 0){
Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree
}
@@ -180,10 +186,10 @@
Input = 0; // Initializes imput to cero: 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); // Limits to the differnce between High and Def
myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct
@@ -197,7 +203,7 @@
Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID
wait(.05); // Slight Delay: *****Test without
myPIDDistance.Compute(); // Compute Output for chair
-
+
tempor = Output + def; // Temporary output variable
x->write(tempor); // Sends to chair x output
out->printf("distance %f\r\n", Input);
@@ -206,7 +212,7 @@
}
void Wheelchair::pid_reverse(double mm)
{
-
+
}
void Wheelchair::pid_twistA()
{
@@ -214,7 +220,7 @@
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); // Limits to the differnce between def and low
PIDAngularV.SetControllerDirection(DIRECT); // PID mode Direct
@@ -244,7 +250,7 @@
PIDAngularV.Compute();
temporA += yOut; // Temporary value with the voltage output
y->write(temporA);
- out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
+ //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
wait(.05);
}
}
@@ -256,7 +262,7 @@
char c;
x->write(def);
y->write(def);
-
+ wheel->reset();
PIDVelosity.SetTunings(.00005,0, 0.00); // Sets the constants for P and D
PIDSlaveV.SetTunings(.007,0.000001, 0.000001); // Sets the constants for P and D
PIDVelosity.SetOutputLimits(-.005, .005); // Limits to the differnce between def and low
@@ -280,12 +286,13 @@
if(c == 'r')
{
yDesired = 0;
+ dist_old = 0;
return;
}
if(vDesired >= 0)
{
- PIDVelosity.SetTunings(.00001,0, 0.00); // Sets the constants for P and D
- PIDVelosity.SetOutputLimits(-.003, .003); // Limits to the differnce between def and low
+ PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D
+ PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low
}
else
{
@@ -302,10 +309,25 @@
temporS += vOutS;
x->write(temporV);
y->write(temporS);
- out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
+ //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
+ Wheelchair::odomMsg();
wait(.01);
}
}
+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;
+ }
+ void Wheelchair::showOdom(){
+ out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
+}
float Wheelchair::getDistance() {
return wheel->getDistance(Diameter);
}
@@ -321,7 +343,7 @@
Wheelchair::pid_right(87);
Wheelchair::pid_forward(3658);
}
-
+
void Wheelchair::kitchen() {
Wheelchair::pid_forward(5461);
Wheelchair::pid_right(87);
@@ -329,9 +351,10 @@
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