Diff: main.cpp

Revision:

2:57523bb4e9c6

Parent:

1:83fccd7d8483

Child:

3:1d43dd4f37eb

--- a/main.cpp	Mon Oct 24 10:54:55 2016 +0000
+++ b/main.cpp	Mon Oct 24 11:31:09 2016 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include "QEI.h"
+#include "emg.h"
+
 #define pi 3.14159265359;
 
 PwmOut Motor1_pwm(D5);
@@ -7,6 +9,7 @@
 PwmOut Motor2_pwm(D6);
 DigitalOut LiftMotor_Direction(D7);
 AnalogIn Potmeter(A0);
+AnalogIn Potmeter2(A1);
 QEI Slide_Encoder(D12,D13,NC,64);
 QEI Lift_Encoder(D10,D11,NC,64);
 Serial pc(USBTX,USBRX);
@@ -70,49 +73,33 @@
     Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
     Controller.attach(&Ticker_Flag,1/Frequency);
     pc.baud(9600);
+    
     while (true) {
         
-        Slide_Counts =      Slide_Encoder.getPulses();
-        Slide_Revolutions = Slide_Counts /(32*131);
-        Slide_Angle =       Slide_Revolutions*2*pi;
-        Slide_Position =    Slide_Angle*Slide_Radius + 135;
-        
-        Lift_Counts =       Lift_Encoder.getPulses();
-        Lift_Revolutions =  Lift_Counts /(32*131);
-        Lift_Angle =        Lift_Revolutions*2*pi;
-        Lift_Position =     Lift_Angle*Lift_Radius;
         //pc.printf("\n\r%f", Lift_Counts);
         
         if (Controller_Flag == true){
             Slide_Controller();
             Lift_Controller();
+            Controller_Flag = false;
         }
         
-        if (pc.readable()) {
-            Key = pc.getc();
-            switch(Key) {                         //Check to see which Key key...
-                case 0x41:                        //It was the UP Key key...
-                    pc.printf("\n\r UP!");
-                    Lift_Input_force = -1;
-                    break;
-                case 0x42:                        //It was the DOWN Key key...
-                    pc.printf("\n\r DOWN!");
-                    Lift_Input_force = 1;
-                    break;
-                case 0x43:                        //It was the RIGHT Key key...
-                    //pc.printf("\n\r RIGHT!");
-                     Slide_Input_force = 1;   
-                    break;
-                case 0x44:                        //It was the LEFT Key key...
-                    //pc.printf("\n\r LEFT!");
-                    Slide_Input_force = -1; 
-                    break;
+        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();
             }
-        } else { 
-            Slide_Input_force = 0;
-            wait(1/Frequency);
-        }   
-        //Motor1_pwm.write(Slide_Controller(Input_force));
+       }  
+        
+        
         
     }
     return 0;
@@ -122,8 +109,13 @@
 }
 
 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
-    
-    Slide_Desired_speed= Slide_Input_force*Slide_Multiplier;
+
+    Slide_Counts =      Slide_Encoder.getPulses();
+    Slide_Revolutions = Slide_Counts /(32*131);
+    Slide_Angle =       Slide_Revolutions*2*pi;
+    Slide_Position =    Slide_Angle*Slide_Radius + 135;
+            
+    Slide_Desired_speed= (Slide_Input_force*2 -1)*Slide_Multiplier;
    
     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
         Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
@@ -149,14 +141,13 @@
 
 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
     
-    Lift_Desired_position = (Potmeter.read()*2-1)*30*Lift_Multiplier;
-     pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
-    if (Lift_Desired_position < Lift_Start){
-      // Lift_Desired_position = Lift_Start;
-    }
-    if (Lift_Desired_position > Lift_End){
-     //  Lift_Desired_position = Lift_End;
-    }
+    Lift_Counts =       Lift_Encoder.getPulses();
+    Lift_Revolutions =  Lift_Counts /(32*131);
+    Lift_Angle =        Lift_Revolutions*2*pi;
+    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);
     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