Diff: main.cpp

Revision:

51:b40967b1fe5c

Parent:

50:c4c9daf7b74c

Child:

52:45200bbde108

--- a/main.cpp	Mon Oct 26 15:17:56 2015 +0000
+++ b/main.cpp	Mon Oct 26 16:06:26 2015 +0000
@@ -27,7 +27,7 @@
 QEI          motor_turn(D12,D13,NC,32);                           QEI        motor_strike(D9,D10,NC,32);                                // TURN - STRIKE : Encoder for motor
 PwmOut       pwm_motor_turn(D5);                                  PwmOut     pwm_motor_strike(D6);                                      // TURN - STRIKE : Pwm for motor
 DigitalOut   motordirection_turn(D4);                             DigitalOut motordirection_strike(D7);                                 // TURN - STRIKE : Direction of the motor
-       
+ 
 
 //      [DEFINE CONSTANTS]      //
 HIDScope scope(3); // Aantal HIDScope kanalen
@@ -168,6 +168,8 @@
         f++;
         smp++;
         pc.printf("Pwm=%f \n\r", pwm_average);
+        double speed=fabs(pwm_average);
+        
         
 if (pwm_average < 0.1)   {   leduit();            }
 if (pwm_average > 0.1)   {   ledgreen1  = ledon;  }
@@ -179,10 +181,11 @@
 
         if(smp>512)
         {
-        pwm_motor_strike=fabs(pwm_strike);
+        pwm_motor_strike=speed;
+        pwm_motor_turn=speed;
         green();
-        wait(5);
         pwm_motor_strike=0;
+        wait(3);
         f=1;
         smp=0;}
         }
@@ -239,17 +242,29 @@
                                   
 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
     {
+        
+     //ZONDER NOTCH 
     EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
     
     EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
     EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
+
+    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
     
-    //EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
-    //EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
     
-    //EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
-    
-    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
+//    EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
+//    
+//    EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
+//    
+//    EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
+//    EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
+//    
+//    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
+//    
+//    
+//    EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
+//    
+//    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
     }
     
 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)