Diff: wheelchair.cpp

Revision:

24:d2f234fbc20d

Parent:

23:8d11d953ceeb

Child:

25:58ec657a44f2

--- 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);
     }