Revision 9:5140b3a95dc9, committed 2015-10-23

Comitter:

Zeekat

Date:

Fri Oct 23 19:30:39 2015 +0000

Parent:

8:649a5f555b7b

Commit message:

working version. smooth to ref

Changed in this revision

--- a/main.cpp	Fri Oct 23 11:16:46 2015 +0000
+++ b/main.cpp	Fri Oct 23 19:30:39 2015 +0000
@@ -86,7 +86,7 @@
     double phi_two;
 
 // define start up variables (is to create a delayed start up instead of going to the ref value inmediately)
-    double start_up_time = 2;
+    double start_up_time = 3;
     double start_loops = start_up_time*controlfreq;
     int rc1 = 0;            // counters in function to enable slow start up
     int rc2 = 0;
@@ -95,6 +95,9 @@
 double reset_phi_one = 0;
 double reset_phi_two = 0;
 
+double phi_one_curr = 0;
+double phi_two_curr = 0;
+
 // REFERENCE SETTINGS
     double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
  // Define storage variables for reference values (also start position)
@@ -229,17 +232,6 @@
     return y;
     }
 
-// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
-// (niet netjes maar werkt)
-double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
-{
-    double vt = ut- a1t*v1t-a2t*v2t;
-    double yt = b0t*vt+b1t*v1t+b2t*v2t;
-    v2t = v1t;
-    v1t = vt;
-    return yt;
-}
-
 // PID Controller given in sheets
 // aangepast om zelfde filter te gebruiken en om de termen te splitsen
 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
@@ -300,20 +292,20 @@
 
 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
     double u1t = EMG_int.read();
-    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
-    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
-    double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
     double y4t = abs(y3t);
-    double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
     // update global variables
     output2 = y5t;
     output2_amp = y5t*emg_gain2; 
     
-    //scope.set(0,output1);
-    //scope.set(1,output2);
-    //scope.set(0,output1_amp);
-    //scope.set(1,output2_amp);
-    //scope.send();
+    scope.set(0,output1);
+    scope.set(1,output2);
+    scope.set(0,output1_amp);
+    scope.set(1,output2_amp);
+    scope.send();
  }
  
  
@@ -418,13 +410,9 @@
     if(rc1 < start_loops)
     {
         rc1++;
-        reference1 = ((double) rc1/start_loops)*reference1;
+        reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
     }
-    else
-    {
-        reference1 = reference1;
-    }
-    
+  
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
     double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
@@ -449,13 +437,8 @@
     if(rc2 < start_loops)
     {
         rc2++;
-        reference2 = ((double) rc2/start_loops)*reference2;
+        reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
     }
-    else
-    {
-        reference2 = reference2;
-    }    
-
     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
     double error2 = (reference2 - rads2);                    // determine the error (reference - position)
     
@@ -541,9 +524,12 @@
         {
             reset = !reset;     
             wait(reset_button.read() == 1);
-            wait(0.3);
+            phi_one_curr = phi_one;
+            phi_two_curr = phi_two;
             rc1 = 0;
-            rc2 = 0;             
+            rc2 = 0;     
+            wait(0.3);
+        
         }
         //////////////////////////////////////////////////
         // Main Control stuff and options