Diff: main.cpp

Revision:

11:c8b6a2b314c3

Parent:

9:1cb2d5ab51e6

Child:

12:da003af4d373

--- a/main.cpp	Thu Oct 27 11:50:16 2016 +0000
+++ b/main.cpp	Thu Oct 27 14:10:49 2016 +0000
@@ -14,6 +14,12 @@
 AnalogIn Potmeter2(A1);
 QEI Slide_Encoder(D12,D13,NC,64);
 QEI Lift_Encoder(D10,D11,NC,64);
+
+BiQuadChain Hz_1;
+BiQuad bq0(0.05852855368, 0.11705710736, 0.05852855368,-1.05207469728, 0.28586907478);
+BiQuad bq1(0.06463239794, 0.12926479589, 0.06463239794,-1.16338171052, 0.42191097989);
+BiQuad bq2(0.07902502847, 0.15805005694, 0.07902502847,-1.42439823874, 0.7409311811);
+
 //Serial pc(USBTX,USBRX);
 Ticker Controller;
 
@@ -24,7 +30,7 @@
 float Slide_Radius = 12.5;
 float Slide_Multiplier = 1;
 float k1 = 1;
-float k2 = 0.02f;
+float k2 = 0.01f;
 float k3 = 0.1f;
 float Start_slow = 40;
 float Start_lock = 0;
@@ -46,7 +52,7 @@
 
 float Lift_Radius = 10;
 float Lift_Multiplier = 1;
-float Lift_k1 = 0.08;
+float Lift_k1 = 0.2;
 float Lift_k2 = 0.05;
 float Lift_k3 = 0.01;
 float Lift_Start = 0;
@@ -79,12 +85,13 @@
     Lift_Input_force =  Potmeter.read();
     Slide_Input_force = Potmeter2.read();
     
+    Hz_1.add( &bq0 ).add( &bq1 ).add( &bq2 );
     notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 );
     high_pass.add( &bq6 ).add( &bq7 );
     low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 );
     
-    change_state.attach( &calibrate,5);
-    change_state2.attach( &run,15);
+    change_state.attach( &calibrate,1);
+    change_state2.attach( &run,11);
     emgSampleTicker.attach( &emgSample, 0.005); //200Hz
     
 //    treshold = (cali_max-cali_min)*treshold_multiplier;
@@ -121,8 +128,9 @@
     Slide_Angle =       Slide_Revolutions*2*pi;
     Slide_Position =    Slide_Angle*Slide_Radius + 135;
             
-    Slide_Desired_speed= (Norm_EMG_0*2 -1)*Slide_Multiplier;
-   
+    Slide_Desired_speed= (-0)*Slide_Multiplier;
+    
+    
     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
         Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
         
@@ -130,11 +138,15 @@
     if (Slide_Position > End_slow && Slide_Desired_speed < 0){
        Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
     }
+    
     Slide_Prev_delta_speed = Slide_Delta_speed;
     Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
     Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
-    
-    Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
+    if (Slide_Int_delta_speed > 1){Slide_Int_delta_speed = 1;}
+    if (Slide_Int_delta_speed < -1){Slide_Int_delta_speed = -1;}
+    Slide_Int_delta_speed *= 1/1.3;
+    //pc.printf("\r\n%f - %f", Slide_Int_delta_speed, Slide_Delta_speed);
+    Slide_PI = k1*Slide_Delta_speed + 0*Slide_Int_delta_speed;
     if (Slide_PI<0){
         SlideMotor_Direction = 0;
     }else{
@@ -151,15 +163,19 @@
     Lift_Revolutions =  Lift_Counts /(32*131);
     Lift_Angle =        Lift_Revolutions*2*pi;
     Lift_Position =     Lift_Angle*Lift_Radius;
+    float NomNom = Hz_1.step(Norm_EMG_0);
     
-    Lift_Desired_position = (Lift_Input_force*2-1)*30*Lift_Multiplier;
+    Lift_Desired_position = (-NomNom)*40*Lift_Multiplier;
     //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
     Lift_Prev_delta_position = Lift_Delta_position;                             
     Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
     Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
-    Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;
     
-    Lift_PI = Lift_k1*Lift_Delta_position + Lift_k2*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
+    Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;   //D
+    if (Lift_Int_delta_position > 1){Lift_Int_delta_position = 1;}
+    if (Lift_Int_delta_position < -1){Lift_Int_delta_position = -1;}
+    pc.printf("\r\n%f - %f", NomNom, Norm_EMG_0);
+    Lift_PI = Lift_k1*Lift_Delta_position + 0*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
     if (Lift_PI<0){
         LiftMotor_Direction = 1;
     }else{