Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 24:d2f234fbc20d
- Parent:
- 23:8d11d953ceeb
- Child:
- 25:58ec657a44f2
diff -r 8d11d953ceeb -r d2f234fbc20d wheelchair.cpp --- a/wheelchair.cpp Tue Oct 16 23:03:40 2018 +0000 +++ b/wheelchair.cpp Sat Oct 27 10:10:14 2018 +0000 @@ -1,20 +1,30 @@ #include "wheelchair.h" bool manual_drive = false; // Variable Changes between joystick and auto drive -double curr_yaw; // Variable that contains current relative angle +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; 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(); } +void Wheelchair::velosity_thread() { + curr_vel = wheel->getVelosity(); + curr_velS = wheelS->getVelosity(); + } -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) // Function Constructor for Wheelchair class +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS) // Function Constructor for Wheelchair class { //Initializes X and Y variables to Pins x = new PwmOut(xPin); @@ -25,7 +35,8 @@ 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 + wheelS = qeiS; // "wheel" is called for encoder + wheel = qei; out->printf("wheelchair setup done \r\n"); // make sure it initialized ti = time; myPID.SetMode(AUTOMATIC); // set PID to automatic @@ -197,7 +208,104 @@ { } +void Wheelchair::pid_twistA() +{ + char c; + 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 + while(1) + { + if(out->readable()) + c = out->getc(); + if(c == '0') + { + yDesired = 0; + } + if(c == '1') + yDesired = 30; + if(c == '2') + yDesired = 90; + if(c == '9') + yDesired = -30; + if(c == '8') + yDesired = -90; + if(c == 'r') + { + yDesired = 0; + return; + } + + yIn = imu->gyro_z(); + 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()); + wait(.05); + } +} +void Wheelchair::pid_twistV() +{ + double temporV = def; + double temporS = def; + vDesiredS = 0; + char c; + x->write(def); + y->write(def); + + 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 + PIDSlaveV.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low + PIDVelosity.SetControllerDirection(DIRECT); + PIDSlaveV.SetControllerDirection(DIRECT); + while(1) + { + if(out->readable()) + c = out->getc(); + if(c == '0') + vDesired = 0; + if(c == '3') + vDesired = 100; + if(c == '4') + vDesired = 150; + if(c == '7') + vDesired = -50; + if(c == '6') + vDesired = -100; + if(c == 'r') + { + yDesired = 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 + } + else + { + PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D + PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to the differnce between def and low + } + if(temporV >= 1) + temporV = 1; + vIn = curr_vel*100; + vInS = curr_vel-curr_velS; + PIDVelosity.Compute(); + PIDSlaveV.Compute(); + temporV += vOut; + 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); + wait(.01); + } +} float Wheelchair::getDistance() { return wheel->getDistance(Diameter); }