Diff: main.cpp

Revision:

12:12c162dc8893

Parent:

11:c8b6a2b314c3

Child:

13:0adbf6a5de37

--- a/main.cpp	Thu Oct 27 14:10:49 2016 +0000
+++ b/main.cpp	Fri Oct 28 11:46:44 2016 +0000
@@ -30,7 +30,7 @@
 float Slide_Radius = 12.5;
 float Slide_Multiplier = 1;
 float k1 = 1;
-float k2 = 0.01f;
+float k2 = 0.1f;
 float k3 = 0.1f;
 float Start_slow = 40;
 float Start_lock = 0;
@@ -69,6 +69,9 @@
 float Lift_Deriv_delta_position = 0;
 float Lift_Prev_delta_position = 0;
 float Lift_PI;
+bool Lift_Switch = false;
+Timeout Lift_Timeout;
+bool Lift_Timeout_Switch = true;
 
 void Slide_Controller();
 void Lift_Controller();
@@ -85,10 +88,15 @@
     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 );
+    notch_50_0.add( &bq03 ).add( &bq04 ).add( &bq05 );
+    notch_50_1.add( &bq13 ).add( &bq14 ).add( &bq15 );
+    notch_50_2.add( &bq23 ).add( &bq24 ).add( &bq25 );
+    high_pass_0.add( &bq06 ).add( &bq07 );
+    high_pass_1.add( &bq16 ).add( &bq17 );
+    high_pass_2.add( &bq26 ).add( &bq27 );
+    low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
+    low_pass_1.add( &bq19 ).add( &bq110 ).add( &bq111 );
+    low_pass_2.add( &bq29 ).add( &bq210 ).add( &bq211 );
     
     change_state.attach( &calibrate,1);
     change_state2.attach( &run,11);
@@ -128,7 +136,7 @@
     Slide_Angle =       Slide_Revolutions*2*pi;
     Slide_Position =    Slide_Angle*Slide_Radius + 135;
             
-    Slide_Desired_speed= (-0)*Slide_Multiplier;
+    Slide_Desired_speed= (-Norm_EMG_1+Norm_EMG_0)*Slide_Multiplier;
     
     
     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
@@ -145,8 +153,8 @@
     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;
+    pc.printf("%f - %f - %f \r\n",Norm_EMG_0, Norm_EMG_1, Slide_Delta_speed);
+    Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
     if (Slide_PI<0){
         SlideMotor_Direction = 0;
     }else{
@@ -157,15 +165,24 @@
     //return k1*Delta_speed + k2*Int_delta_speed;
 }
 
+void Lift_Timeout_Return(){
+        Lift_Timeout_Switch = true;
+    }
+
 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
     
     Lift_Counts =       Lift_Encoder.getPulses();
     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 = (-NomNom)*40*Lift_Multiplier;
+    if(Norm_EMG_0 > 0.6 && Lift_Timeout_Switch == true){
+        //Lift_Switch = !Lift_Switch;
+        Lift_Timeout_Switch = false;
+        Lift_Timeout.attach(Lift_Timeout_Return, 1);
+    }
+    
+    Lift_Desired_position = Lift_Switch*150;
     //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
@@ -174,7 +191,7 @@
     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;