Working file for communication with ROS
Dependencies: QEI2 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);