Affordable Smart Wheelchair
Revised for integration
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 23:8d11d953ceeb
- Parent:
- 20:f42db4ae16f0
- Child:
- 24:d2f234fbc20d
--- a/wheelchair.cpp Fri Aug 31 20:00:01 2018 +0000
+++ b/wheelchair.cpp Tue Oct 16 23:03:40 2018 +0000
@@ -1,414 +1,202 @@
#include "wheelchair.h"
-bool manual_drive = false;
-volatile float north;
-//volatile double curr_yaw;
-double curr_yaw;
-double encoder_distance;
-char myString[64];
-volatile double Setpoint, Output, Input, Input2;
-volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;
-PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
-PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0025, P_ON_E, DIRECT);
+bool manual_drive = false; // Variable Changes between joystick and auto drive
+double curr_yaw; // 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
-//PID myPIDDistance2(&Input2, &Output2, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT);
-//QEI wheel_right(D0, D1, NC, 450);
-void Wheelchair::compass_thread() {
+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
+
+void Wheelchair::compass_thread() { // Thread that measures which angle we are at
curr_yaw = imu->yaw();
- north = boxcar(imu->angle_north());
-
}
-Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei)
+Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) // Function Constructor for Wheelchair class
{
- x = new PwmOut(xPin);
+ //Initializes X and Y variables to Pins
+ x = new PwmOut(xPin);
y = new PwmOut(yPin);
+
+ // Initializes IMU Library
imu = new chair_BNO055(pc, time);
- //imu = new chair_MPU9250(pc, time);
- Wheelchair::stop();
- imu->setup();
- out = pc;
- wheel = qei;
- out->printf("wheelchair setup done \r\n");
+ Wheelchair::stop(); // Wheelchair is not moving when initializing
+ imu->setup(); // turns on the IMU
+ out = pc; // "out" is called for serial monitor
+ wheel = qei; // "wheel" is called for encoder
+ out->printf("wheelchair setup done \r\n"); // make sure it initialized
ti = time;
- //wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
- myPID.SetMode(AUTOMATIC);
+ myPID.SetMode(AUTOMATIC); // set PID to automatic
}
-/*
-* joystick has analog out of 200-700, scale values between 1.3 and 3.3
-*/
-void Wheelchair::move(float x_coor, float y_coor)
+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;
- float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
-
- // lowPass(scaled_x);
- //lowPass(scaled_y);
+ 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);
+ x->write(scaled_x); // Sends the scaled joystic values to the chair
y->write(scaled_y);
-
- //out->printf("yaw %f\r\r\n", imu->yaw());
-
}
-void Wheelchair::forward()
+void Wheelchair::forward() // In auto to move foward
{
x->write(high);
y->write(def+offset);
- // out->printf("distance %f\r\n", wheel_right.getDistance(37.5));
}
-void Wheelchair::backward()
+void Wheelchair::backward() // In auto to move reverse
{
x->write(low);
y->write(def);
}
-void Wheelchair::right()
+void Wheelchair::right() // In auto to move right
{
x->write(def);
y->write(low);
}
-void Wheelchair::left()
+void Wheelchair::left() // In auto to move left
{
x->write(def);
y->write(high);
}
-void Wheelchair::stop()
+void Wheelchair::stop() // Stops the chair
{
x->write(def);
y->write(def);
}
// counter clockwise is -
// clockwise is +
-void Wheelchair::pid_right(int deg)
+void Wheelchair::pid_right(int deg) // Takes in degree and turns right
{
- bool overturn = false;
+ bool overturn = false; //Boolean if we have to turn over relative 360˚
- out->printf("pid right\r\r\n");
- x->write(def);
- Setpoint = curr_yaw + deg;
- pid_yaw = curr_yaw;
- if(Setpoint > 360) {
- // Setpoint -= 360;
+ out->printf("pid right\r\r\n");
+ x->write(def); // Not moving fowards or reverse
+ Setpoint = curr_yaw + deg; // Relative angle we want to turn
+ pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
+
+ if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚
overturn = true;
}
- myPID.SetTunings(5.5,0, 0.0035);
- myPID.SetOutputLimits(0, def-low-.15);
- myPID.SetControllerDirection(DIRECT);
- while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) {
- if(overturn && curr_yaw < Setpoint-deg-1)
+
+ myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D
+ myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low
+ myPID.SetControllerDirection(DIRECT); // PID mode Direct
+
+ while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching
+ // a little less than desired angle
+ if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary
{
pid_yaw = curr_yaw + 360;
}
else
pid_yaw = curr_yaw;
- myPID.Compute();
- double tempor = -Output+def;
- y->write(tempor);
- out->printf("curr_yaw %f\r\r\n", curr_yaw);
+
+ myPID.Compute(); // Does PID calculations
+ double tempor = -Output+def; // Temporary value with the voltage output
+ y->write(tempor); // Sends to chair y output command
+
+ out->printf("curr_yaw %f\r\r\n", curr_yaw);
out->printf("Setpoint = %f \r\n", Setpoint);
- wait(.05);
+ wait(.05); // Small delay
}
- Wheelchair::stop();
+ Wheelchair::stop(); // Safety Stop
out->printf("done \r\n");
- }
+}
-void Wheelchair::pid_left(int deg)
+void Wheelchair::pid_left(int deg) // Takes in degree and turns left
{
- bool overturn = false;
+ bool overturn = false; //Boolean if we have to turn under relative 0˚
- out->printf("pid Left\r\r\n");
- x->write(def);
- Setpoint = curr_yaw - deg;
- pid_yaw = curr_yaw;
- if(Setpoint < 0) {
- // Setpoint += 360;
+ out->printf("pid Left\r\r\n");
+ x->write(def); // Not moving fowards or reverse
+ Setpoint = curr_yaw - deg; // Relative angle we want to turn
+ pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
+ if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚
overturn = true;
}
- myPID.SetTunings(5,0, 0.004);
- myPID.SetOutputLimits(0,high-def-.12);
- myPID.SetControllerDirection(REVERSE);
- while(pid_yaw > Setpoint+3){//pid_yaw < Setpoint + 2) {
- myPID.Compute();
- if(overturn && curr_yaw > Setpoint+deg+1)
+ myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D
+ myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def
+ myPID.SetControllerDirection(REVERSE); // PID mode Reverse
+ while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching
+ // a little more than desired angle
+ if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary
{
pid_yaw = curr_yaw - 360;
}
else
pid_yaw = curr_yaw;
- double tempor = Output+def;
-
- y->write(tempor);
+
+ myPID.Compute(); // Does PID calculations
+ double tempor = Output+def; // Temporary value with the voltage output
+ y->write(tempor); // Sends to chair y output command
+
out->printf("curr_yaw %f\r\n", curr_yaw);
- wait(.05);
+ wait(.05); // Small Delay
}
- Wheelchair::stop();
- }
+ Wheelchair::stop(); // Safety Stop
+}
-void Wheelchair::pid_turn(int deg) {
- if(deg > 180) {
+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) {
+ else if(deg < -180) { // If deg < -180 turn right: coterminal angle
deg+=360;
}
- int turnAmt = abs(deg);
- ti->reset();
+ int turnAmt = abs(deg); // Makes sure input angle is positive
if(deg >= 0){
- Wheelchair::pid_right(turnAmt);
- }
+ Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree
+ }
else {
- Wheelchair::pid_left(turnAmt);
- }
+ Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree
}
+}
void Wheelchair::pid_forward(double mm)
{
- mm -= 20;
- Input = 0;
- wheel->reset();
+ mm -= 20; // Makes sure distance does not overshoot
+ 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;
- Setpoint = mm;
+ double tempor; // Initializes Temporary variable for x input
+ Setpoint = mm; // Initializes the setpoint to desired value
- // Setpoint = wheel_right.getDistance(37.5)+mm;
- myPIDDistance.SetTunings(5,0, 0.004);
- myPIDDistance.SetOutputLimits(0,high-def-.15);
- myPIDDistance.SetControllerDirection(DIRECT);
- y->write(def+offset);
- while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
- if(out->readable())
+ 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
+ y->write(def+offset); // Sets chair to not turn
+
+ while(Input < Setpoint){ // Stop moving when reaching setpoint
+
+ if(out->readable()) // Emergency Break
break;
- Input = wheel->getDistance(53.975);
- //out->printf("input foward %d\r\n", wheel->getPulses());
- wait(.05);
- myPIDDistance.Compute();
+
+ 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;
- x->write(tempor);
+ tempor = Output + def; // Temporary output variable
+ x->write(tempor); // Sends to chair x output
out->printf("distance %f\r\n", Input);
}
-
}
void Wheelchair::pid_reverse(double mm)
{
- /* qei.putc('r');
- out->printf("pid reverse\r\n");
- double tempor;
- Setpoint2 = mm;
-
- // Setpoint = wheel_right.getDistance(37.5)+mm;
- myPIDDistance.SetTunings(5,0, 0.004);
- myPIDDistance.SetOutputLimits(0,def);
- myPIDDistance.SetControllerDirection(REVERSE);
- y->write(def);
- while(encoder_distance > Setpoint2+5){//pid_yaw < Setpoint + 2) {
- int i;
- qei.putc('h');
- //qei.gets(myString, 10);
- ti->reset();
-
- for (i=0; myString[i-1] != '\n'; i++) {
- while (true) {
- //pc.printf("%f\r\n", ti.read());
- if (ti->read() > .02) break;
- if (qei.readable()) {
- myString[i]= qei.getc();
- break;
- }
- }
- }
- myString[i-1] = 0;
- double tempor = atof(myString);
- out->printf("displacement = %f\r\n", tempor);
- if(abs(tempor - encoder_distance) < 500)
- {
- encoder_distance = tempor;
- out->printf("this is fine\r\n");
- }
- for(i = 0; i < 64; i++)
- {
- myString[i] = 0;
- }
- Input = encoder_distance;
- out->printf("input foward %f\r\n", Input);
- wait(.1);
- myPIDDistance.Compute();
-
- // get value from encoder2
- qei.putc('k');
- ti->reset();
-
- for (i=0; myString[i-1] != '\n'; i++) {
- while (true) {
- //pc.printf("%f\r\n", ti.read());
- if (ti->read() > .02) break;
- if (qei.readable()) {
- myString[i]= qei.getc();
- break;
- }
- }
- }
- myString[i-1] = 0;
- double tempor2 = atof(myString);
- out->printf("displacement = %f\r\n", tempor2);
-
- if(abs(tempor - encoder_distance) < 500)
- {
- encoder_distance = tempor;
- out->printf("this is fine\r\n");
- }
- if(abs(tempor2 - encoder_distance2) < 500)
- {
- encoder_distance2 = tempor2;
- out->printf("this is fine\r\n");
- }
- for(i = 0; i < 64; i++)
- {
- myString[i] = 0;
- }
- tempor = Output;
- x->write(tempor);
- out->printf("distance %f\r\n", encoder_distance);
- }*/
}
-double Wheelchair::turn_right(int deg)
-{
- bool overturn = false;
- out->printf("turning right\r\n");
-
- double start = curr_yaw;
- double final = start + deg;
-
- if(final > 360) {
- final -= 360;
- overturn = true;
- }
-
- out->printf("start %f, final %f\r\n", start, final);
-
- double curr = -1;
- while(curr <= final - 30) {
- Wheelchair::right();
- if( out->readable()) {
- out->printf("stopped\r\n");
- Wheelchair::stop();
- return;
- }
- curr = curr_yaw;
- if(overturn && curr > (360 - deg) ) {
- curr = 0;
- }
- }
-
- out->printf("done turning start %f final %f\r\n", start, final);
- Wheelchair::stop();
-
- //delete me
- wait(5);
-
- float correction = final - curr_yaw;
- out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
- Wheelchair::turn_left(abs(correction));
- Wheelchair::stop();
-
- wait(5);
- out->printf("curr_yaw %f\r\n", curr_yaw);
- return final;
-}
-
-double Wheelchair::turn_left(int deg)
-{
- bool overturn = false;
- out->printf("turning left\r\n");
-
- double start = curr_yaw;
- double final = start - deg;
-
- if(final < 0) {
- final += 360;
- overturn = true;
- }
-
- out->printf("start %f, final %f\r\n", start, final);
-
- double curr = 361;
- while(curr >= final) {
- Wheelchair::left();
- if( out->readable()) {
- out->printf("stopped\r\n");
- Wheelchair::stop();
- return;
- }
- curr = curr_yaw;
-
- if(overturn && curr >= 0 && curr <= start ) {
- curr = 361;
- }
- }
-
- out->printf("done turning start %f final %f\r\n", start, final);
- Wheelchair::stop();
-
- //delete me
- wait(2);
- /*
- float correction = final - curr_yaw;
- out->printf("final pos %f actual pos %f\r\n", final, curr_yaw);
- Wheelchair::turn_right(abs(correction));
- Wheelchair::stop();
-*/
- return final;
-}
-
-void Wheelchair::turn(int deg)
-{
- if(deg > 180) {
- deg -= 360;
- }
-
- else if(deg < -180) {
- deg+=360;
- }
-
- double finalpos;
- int turnAmt = abs(deg);
- //ti->reset();
- /*
- if(deg >= 0){
- finalpos = Wheelchair::turn_right(turnAmt);
- }
- else {
- finalpos = Wheelchair::turn_left(turnAmt);
- }
- */
- wait(2);
-
- float correction = finalpos - curr_yaw;
- out->printf("final pos %f actual pos %f\r\n", finalpos, curr_yaw);
-
-
- //if(abs(correction) > turn_precision) {
- out->printf("correcting %f\r\n", correction);
- //ti->reset();
- Wheelchair::turn_left(curr_yaw - finalpos);
- return;
- //}
-
-}
float Wheelchair::getDistance() {
return wheel->getDistance(Diameter);
@@ -417,6 +205,7 @@
void Wheelchair::resetDistance(){
wheel->reset();
}
+/*Predetermined paths For Demmo*/
void Wheelchair::desk() {
Wheelchair::pid_forward(5461);
Wheelchair::pid_right(87);