Diff: main.cpp

Revision:

12:da003af4d373

Parent:

11:c8b6a2b314c3

--- a/main.cpp	Thu Oct 27 14:10:49 2016 +0000
+++ b/main.cpp	Thu Oct 27 15:15:06 2016 +0000
@@ -15,17 +15,17 @@
 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);
+BiQuadChain extra;
+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;
 
 bool Controller_Flag=0;
-float Frequency = 30;
-float Frequency_PWM = 10000;
+const float Frequency = 30;
+const float Frequency_PWM = 10000;
 
 float Slide_Radius = 12.5;
 float Slide_Multiplier = 1;
@@ -70,6 +70,8 @@
 float Lift_Prev_delta_position = 0;
 float Lift_PI;
 
+float NomNom;
+
 void Slide_Controller();
 void Lift_Controller();
 void Ticker_Flag();
@@ -85,7 +87,7 @@
     Lift_Input_force =  Potmeter.read();
     Slide_Input_force = Potmeter2.read();
     
-    Hz_1.add( &bq0 ).add( &bq1 ).add( &bq2 );
+    extra.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 );
@@ -163,7 +165,8 @@
     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);
+    
+    NomNom = extra.step(Norm_EMG_0);
     
     Lift_Desired_position = (-NomNom)*40*Lift_Multiplier;
     //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
@@ -174,7 +177,9 @@
     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);
+    
+    //fixed tha print
+    pc.printf("%d, %d\r\n",Norm_EMG_0, NomNom);
     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;