added code for PCB on and off, added pointers to e_button and pwm pins
Dependencies: QEI2 chair_BNO055 PID VL53L1X_Filter
Dependents: wheelchairControlSumer2019
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