Diff: main.cpp

Revision:

93:e1d7744585fd

Parent:

92:276a1a2272fa

Child:

94:3316f7fa3f50

--- a/main.cpp	Thu Oct 29 17:27:32 2015 +0000
+++ b/main.cpp	Thu Oct 29 18:17:11 2015 +0000
@@ -402,11 +402,11 @@
     minimum_L=fabs(EMG_Left_Bicep_filtered)+minimum_L;          // Take previous sample EMG_Left_Bicep_filtered and add the new value
     minimum_R=fabs(EMG_Right_Bicep_filtered)+minimum_R;   
     c++;                             // Every sample c is increased by one until the statement c<2560 is false
-    pc.printf("%f \n\r %f \n\r" ,EMG_Left_Bicep_filtered, minimum_L);
+    //pc.printf("%f \n\r %f \n\r" ,EMG_Left_Bicep_filtered, minimum_L);
     wait(0.001953125);               // wait one sample
     }
     
-    pc.printf("Finished minimum calibration \n\r");
+    //pc.printf("Finished minimum calibration \n\r");
     
     moving_average_left=minimum_L/100;        // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
     moving_average_right=minimum_R/100;
@@ -439,7 +439,7 @@
                                                 while(1)
                                                     { // inf while loop strike until finished start
                                                             if (n==0)                                                    // Change the reference point of the PID Strike controller
-                                                                    {   reference_strike=Hit;  n=1;  error_count=0;   }   // Execute once (n is set to one and only gets executed if n equals zero)
+                                                                    {   reference_strike=0;  n=1;  error_count=0;   }   // Execute once (n is set to one and only gets executed if n equals zero)
                            
                                                             if (looptimerflag == true)                                   // Loop that executes the strike controller every sample (inside the controller the loudness is regulated)
                                                                     {  
@@ -455,7 +455,7 @@
                                                                                     yellow(); 
                                                                                     if(k==0)                             // Change reference point of the PID Strike controller back to the original position
                                                                                         {  
-                                                                                            p=1;  reference_strike=0; error_count=0; k=1; smp=0;
+                                                                                            p=1;  reference_strike=60; error_count=0; k=1; smp=0;
                                                                                             pc.printf("return \n\r");
                                                                                         }       
                     //pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r k=%f, er_cnt= %f", reference_strike, error_strike, error_strike_average, k, error_count); // LINE USED FOR TESTING
@@ -467,7 +467,7 @@
                                                                                             execute_plant_strike();
                                                                                         }   
                                     
-                     printf(" %f \n\r",error_strike_average);  // LINE USED FOR TESTING
+                     //printf(" %f \n\r",error_strike_average);  // LINE USED FOR TESTING
                                     
                                                                                     if (fabs(error_strike_average) < 2 && error_count>100)         // If error is small enough and at least 100 samples have passed since the return execute new action
                                                                                         {          
@@ -629,6 +629,36 @@
                         }
         }
         calibration_starting_position_complete:;
+        
+        while(1) 
+                                                                                { // go to initial position 
+                                                                                    yellow(); 
+                                                                                    if(k==0)                             // Change reference point of the PID Strike controller back to the original position
+                                                                                        {  
+                                                                                            p=1;  reference_strike=60; error_count=0; k=1; smp=0;
+                                                                                            pc.printf("return \n\r");
+                                                                                        }       
+                    //pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r k=%f, er_cnt= %f", reference_strike, error_strike, error_strike_average, k, error_count); // LINE USED FOR TESTING
+                            
+                                                                                    if (looptimerflag == true)           // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
+                                                                                        {
+                                                                                            looptimerflag=false;
+                                                                                            activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
+                                                                                            execute_plant_strike();
+                                                                                        }   
+                                    
+                     //printf(" %f \n\r",error_strike_average);  // LINE USED FOR TESTING
+                                    
+                                                                                    if (fabs(error_strike_average) < 2 && error_count>100)         // If error is small enough and at least 100 samples have passed since the return execute new action
+                                                                                        {          
+                                                                                            yellow();
+                                                                                            pc.printf("new action \n\r");
+                                                                                            deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
+                                                                                            execute_plant_strike();
+                                                                                            goto calibration_position_strike_complete;                                   
+                                                                                        }
+                                                                                }       // inf while loop return after strike end 
+              calibration_position_strike_complete:;
         }
         
 //                                          ___________________________