Diff: main.cpp

Revision:

3:1d43dd4f37eb

Parent:

2:57523bb4e9c6

Child:

4:e59a99c5aa08

--- a/main.cpp	Mon Oct 24 11:31:09 2016 +0000
+++ b/main.cpp	Mon Oct 24 11:43:45 2016 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "QEI.h"
+
 #include "emg.h"
 
 #define pi 3.14159265359;
@@ -74,9 +75,23 @@
     Controller.attach(&Ticker_Flag,1/Frequency);
     pc.baud(9600);
     
+    
+    Lift_Input_force =  Potmeter.read();
+    Slide_Input_force = Potmeter2.read();
+    
+    notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
+    high_pass.add( &bq4 ).add( &bq5 );
+    
+    change_state.attach( &calibrate,5);
+    change_state2.attach( &run,10);
+    emgSampleTicker.attach( &emgSample, 0.002);
+    
     while (true) {
-        
-        //pc.printf("\n\r%f", Lift_Counts);
+        pc.printf("\n\r%f", Norm_EMG_0);
+         
+        if (go_emgSample == true){
+                EMG_filter();
+        }
         
         if (Controller_Flag == true){
             Slide_Controller();
@@ -84,20 +99,7 @@
             Controller_Flag = false;
         }
         
-        Lift_Input_force =  Potmeter.read();
-        Slide_Input_force = Potmeter2.read();
-        
-        notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
-        high_pass.add( &bq4 ).add( &bq5 );
-        
-        change_state.attach( &calibrate,5);
-        change_state2.attach( &run,10);
-        emgSampleTicker.attach( &emgSample, 0.002);
-        while( true ){
-            if(go_emgSample == true){
-                EMG_filter();
-            }
-       }  
+
         
         
         
@@ -115,7 +117,7 @@
     Slide_Angle =       Slide_Revolutions*2*pi;
     Slide_Position =    Slide_Angle*Slide_Radius + 135;
             
-    Slide_Desired_speed= (Slide_Input_force*2 -1)*Slide_Multiplier;
+    Slide_Desired_speed= (Norm_EMG_0*2 -1)*Slide_Multiplier;
    
     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
         Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
@@ -147,7 +149,7 @@
     Lift_Position =     Lift_Angle*Lift_Radius;
     
     Lift_Desired_position = (Lift_Input_force*2-1)*30*Lift_Multiplier;
-    pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
+    //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