Diff: main.cpp

Revision:

24:f8482c47a385

Parent:

23:d1a3d537460f

Child:

25:d94275968963

--- a/main.cpp	Thu Oct 31 12:11:54 2019 +0000
+++ b/main.cpp	Thu Oct 31 12:45:16 2019 +0000
@@ -71,20 +71,20 @@
     bool emg3_active;
 
 // 3.3 EMG Variables **********************************************************
-static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
-static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad LowPassFilter_motor1( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 ); 
+static BiQuad LowPassFilter_motor2( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 ); 
 
-static BiQuad highfilter0(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
-static BiQuad LowPassFilter0( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad highfilter0(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );   
+static BiQuad LowPassFilter0( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01); 
     
-static BiQuad highfilter1(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
-static BiQuad LowPassFilter1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad highfilter1(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );   
+static BiQuad LowPassFilter1( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 ); 
    
-static BiQuad highfilter2(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
-static BiQuad LowPassFilter2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad highfilter2(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );   
+static BiQuad LowPassFilter2( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 ); 
    
-static BiQuad highfilter3(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
-static BiQuad LowPassFilter3( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
+static BiQuad highfilter3(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );   
+static BiQuad LowPassFilter3( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 ); 
 
 double emg0_raw_signal ; double emg1_raw_signal ; double emg2_raw_signal ; double emg3_raw_signal ;
 double high_emg0_signal ; double high_emg1_signal ; double high_emg2_signal ; double high_emg3_signal ;
@@ -217,10 +217,10 @@
         
     void motor_calibration()
     {
-        // Calibration motor 2
+        // Calibration motor 1
         motor1DirectionPin=0; //direction of the motor
         motor1=1.0;
-        wait(0.1);
+        wait(0.3);
         while (abs(positie_verschil_motor1)>5)
         {
             motor1=0.2  ; 
@@ -235,7 +235,7 @@
         // Calibration motor 2
         motor2DirectionPin=0; //direction of the motor
         motor2=1.0;
-        wait(0.1);
+        wait(0.3);
         while (abs(positie_verschil_motor2)>5)
         {
             motor2=0.2  ; 
@@ -254,16 +254,16 @@
     double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
     {
         //Proportional part
-        kp_motor1 = 1 ;      // moet nog getweaked worden
+        kp_motor1 = 4.9801 ;      // moet nog getweaked worden
         Up_motor1 = kp_motor1 * error1_motor1;
     
         //Integral part
-        Ki_motor1 = 0.1;     // moet nog getweaked worden
+        Ki_motor1 = 22.6073;     // moet nog getweaked worden
         error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
         Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
     
         //Derivative part 
-        Kd_motor1 = 0.1 ;// moet nog getweaked worden  
+        Kd_motor1 = 0.012757 ;// moet nog getweaked worden  
         error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
         error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
         Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
@@ -639,7 +639,7 @@
 // 5.2 Run state-machine(s) ****************************************************
 state_machine() ;
 // 5.3 Run controller(s) *******************************************************
-motor1_calibrated=true;motor2_calibrated=true;
+
 RKI();
 
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);