Diff: main.cpp

Revision:

52:45200bbde108

Parent:

51:b40967b1fe5c

--- a/main.cpp	Mon Oct 26 16:06:26 2015 +0000
+++ b/main.cpp	Wed Oct 28 18:57:19 2015 +0000
@@ -30,7 +30,7 @@
  
 
 //      [DEFINE CONSTANTS]      //
-HIDScope scope(3); // Aantal HIDScope kanalen
+HIDScope scope(4); // Aantal HIDScope kanalen
 MODSERIAL pc(USBTX,USBRX);
 Ticker control_tick;
 Ticker T1;
@@ -123,8 +123,9 @@
 
 void keep_in_range                  (double * in, double min, double max);  
 
-double pwm_strike;
 double f; double pwm_average; double smp;
+double position_strike, position_turn, pwm_strike, pwm_turn;
+double        conversion_counts_to_degrees=0.085877862594198; 
 //==========================//
 //      [MAIN FUNCTION]     //
 //==========================//
@@ -133,62 +134,113 @@
     control_tick.attach(&ControlGo, (float)1/Fs);
     pc.baud(115200);
     //double n=0;
-    calibration(); // calibreer min en max positie
+    //calibration(); // calibreer min en max positie
     f=1;
     smp=0;
     while(1)
     {
-       // if(control_go)
-//        {
-//            //n++;
-//            //pc.printf("n %f",n);
-//            sample_filter();
-//            scope.set(0,EMG_left_Bicep);                  //left bicep unfiltered
-//            scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
-//            scope.set(2,moving_average_left);     //
-//            scope.send();
-//            control_go = 0;
-//        }
+        if(f==1)
+        {
+        pwm_motor_strike=0;
+        pwm_motor_turn=0; 
+        pc.printf("start /n");            
+        }
+        if(f==512)
+        {
+        pwm_motor_strike=1;
+        motordirection_strike=1;
+        pwm_motor_turn=1;   
+        motordirection_turn=1;
+        }
+        
+        if(f==2048)
+        {
+        pc.printf("stop /n");
+        pwm_motor_strike=0;
+        pwm_motor_turn=0;    
+        }
+        
+        if(f==2560) // same cycle but referesed direction
+        {
+        pwm_motor_strike=0;
+        pwm_motor_turn=0; 
+        pc.printf("start /n");            
+        }
+        if(f==3072)
+        {
+        pwm_motor_strike=1;
+        motordirection_strike=0;
+        pwm_motor_turn=1;   
+        motordirection_turn=0;
+        }
+        
+        if(f==4500)
+        {
+        pc.printf("stop /n");
+        pwm_motor_strike=0;
+        pwm_motor_turn=0;
+        f=0;    
+        }
+        
         yellow();
         if(control_go)
-        {
+        { 
         control_go=0;
-        sample_filter();
-            scope.set(0,EMG_left_Bicep);                  //left bicep unfiltered
-            scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
-            scope.set(2,moving_average_left);     //
+        f++;
+        
+        if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200))                   // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
+                { motor_turn.reset(); } 
+        
+         position_turn = conversion_counts_to_degrees * motor_turn.getPulses();                 // Convert counts to degrees
+         pwm_strike=pwm_motor_strike;
+         pwm_turn=pwm_motor_turn;
+         
+         if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+                { motor_strike.reset(); } 
+        
+         position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
+         
+            scope.set(0,pwm_strike);                  //ik weet niet of dit weergegeven kan worden
+            scope.set(1,pwm_turn);                    //Filtered signal
+            scope.set(2,position_turn);             //
+            scope.set(3,position_strike);             //
             scope.send();
-        double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
-        double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
-        pwm_strike=signal_above_threshold/max_signal;
-        keep_in_range(&pwm_strike, 0,1); 
-        pwm_strike=pwm_strike*pwm_strike;
-        
-        pwm_average=pwm_strike+pwm_average/f;
-        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;  }
-if (pwm_average > 0.2)   {   ledgreen2  = ledon;  }
-if (pwm_average > 0.3)   {   ledyellow1 = ledon;  }
-if (pwm_average > 0.5)   {   ledyellow2 = ledon;  }
-if (pwm_average > 0.7)   {   ledred1    = ledon;  }
-if (pwm_average > 0.9)   {   ledred2    = ledon;  } 
+            
+            
+//        double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
+//        double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
+//        pwm_strike=signal_above_threshold/max_signal;
+//        keep_in_range(&pwm_strike, 0,1); 
+//        pwm_strike=pwm_strike*pwm_strike;
+//        
+//        pwm_average=pwm_strike+pwm_average/f;
+//        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;  }
+//if (pwm_average > 0.2)   {   ledgreen2  = ledon;  }
+//if (pwm_average > 0.3)   {   ledyellow1 = ledon;  }
+//if (pwm_average > 0.5)   {   ledyellow2 = ledon;  }
+//if (pwm_average > 0.7)   {   ledred1    = ledon;  }
+//if (pwm_average > 0.9)   {   ledred2    = ledon;  } 
 
-        if(smp>512)
-        {
-        pwm_motor_strike=speed;
-        pwm_motor_turn=speed;
-        green();
-        pwm_motor_strike=0;
-        wait(3);
-        f=1;
-        smp=0;}
-        }
+  //      if(smp>512)
+//        {
+//        pwm_motor_strike=speed;
+//        pwm_motor_turn=speed;
+//        green();
+//        pwm_motor_strike=0;
+//        wait(3);
+//        f=1;
+//        smp=0;}
+//        }
     } // while end   
 } // main end