heelstrike.c

Go to the documentation of this file.
00001 
00007 #include <includes.h>
00008 
00009 // constant states used by state machine: 
00010 static const int  HS_IDLESWING = 1, HS_DATACOLLECT = 2, HS_LOOKING4HS = 3, HS_IDLESTANCE = 4, HS_LOOKING4HUP = 5;   //int values "SHOULD" tally with those in main_brain (estimator.c)
00011 static int hs_state = HS_LOOKING4HS; 
00012   
00013 // things software_setup initiates:
00014 static int hs_cmax = 0;
00015 static fixed hs_exp_fiter_coeff, hs_threshold = 0, hs_confidence =0;                                      // should be a small number close but less than fixed 1  (for average to work properly)
00016 static INT_VOID_F hs_get_left = intvoid;                           // Returns the analog signal from the left heel sensor. 
00017 static INT_VOID_F hs_get_right = intvoid;                          //Returns the analog signal from the right heel sensor. 
00018 
00019 // output of the algorithm: HS
00020 static int hs_heelstrike = 0;                             // 0 if leg in air (not individual feet), 1 if on ground
00021 
00022 // other variables I use
00023 static fixed raw_sum = 0, filtered_sum = 0, prev_filtered_sum = 0;              // last 2 should never be initialised from 0... change them in hs_set_state
00024 static fixed raw_abs_dev =0, filtered_abs_dev = 0, prev_filtered_abs_dev = 0;   // raw_sum = sum of two feet analaog sensors,  raw_abs_dev = |raw_sum - filtered_sum| 
00025 static int hs_counter = 0;                                                                                
00026 static fixed hs_foot_up_threshold = 0;                                             // this will be set to be same as the raw_sum at the time of last heel_strike
00027 
00028 
00029 void hs_init(float exp_filter_coeff, int confidence, int threshold,  int cmax, INT_VOID_F get_right, INT_VOID_F get_left){
00030 
00031 hs_exp_fiter_coeff = float_to_fixed(exp_filter_coeff);      // should be a small number close but less than fixed 1  (for average to work properly)
00032 hs_confidence = int_to_fixed(confidence);
00033 hs_threshold = int_to_fixed(threshold); 
00034 hs_cmax = cmax;                                  
00035 hs_get_left = get_left;                                    // Returns the analog signal from the left heel sensor. 
00036 hs_get_right = get_right; 
00037 }
00038 
00039 
00040 void hs_update(void){
00041 
00042   switch (hs_state){ 
00043     case HS_IDLESWING:   break;
00044     case HS_DATACOLLECT: hs_calc_filtered_data(); break;
00045     case HS_LOOKING4HS:  hs_look4hs(); break;
00046     case HS_IDLESTANCE:  break;
00047     case HS_LOOKING4HUP: hs_look4hup(); break;
00048   }
00049 }
00050 
00051 
00052 void hs_set_state(float state){
00053 
00054   int next_state = (int)state;
00055   mcu_led_green_blink(10);
00056   
00057   if (next_state != hs_state){
00058     switch (next_state){   // RESET COUNTER ETC HERE 
00059       case HS_IDLESWING: 
00060             break;
00061       case HS_DATACOLLECT: 
00062             raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());                              
00063             prev_filtered_sum = raw_sum;                         // when you start collecting the filter should NOT start from 0, thats bad.. I am starting the prev_filter = current data  (other options average of a few data points)
00064             break;
00065       case HS_LOOKING4HS: 
00066             break;
00067       case HS_IDLESTANCE: 
00068             break;
00069       case HS_LOOKING4HUP: 
00070             hs_foot_up_threshold = raw_sum;               // because I beleive the raw_sum still has the previous HS 's foot_sum_data
00071             break;
00072     }  
00073     hs_state = next_state;
00074   }
00075   
00076 }
00077 
00078 
00079 void hs_calc_filtered_data(void){
00080  
00081  raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
00082  filtered_sum = raw_sum +  fixed_mult(hs_exp_fiter_coeff, (prev_filtered_sum - raw_sum)) ;  // new_filtered = a*prev_filtered + (1-a)*new_raw_data         
00083  raw_abs_dev = fixed_abs(raw_sum - filtered_sum);
00084  filtered_abs_dev = raw_abs_dev +  fixed_mult(hs_exp_fiter_coeff, (prev_filtered_abs_dev - raw_abs_dev)) ;  // same filter 
00085 }
00086 
00087 void hs_look4hs(void){
00088   
00089   if (hs_heelstrike == 0){  // because if its 1, I dont want to keep doing calculations if control comes to this state again (there might be rebounding etc that may garble it)
00090     raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
00091     if((raw_sum > filtered_sum+ fixed_mult(hs_confidence , filtered_abs_dev)) && (raw_sum > filtered_sum + hs_threshold) ){    
00092       hs_counter++;                                         // increase counter but stop calculating filter
00093       if(hs_counter >= hs_cmax){ hs_heelstrike = 1;}  
00094     }else{
00095       hs_counter = 0 ;                               // counter will be reset after any counter increase for a false HS
00096       hs_calc_filtered_data();                       // here filter """"MAY GO BAD"""" (if this statement is visited after 1 counter step, you are missing one filter sample)
00097     }
00098   }
00099 }
00100 
00101 void hs_look4hup(void){
00102 
00103   if (hs_heelstrike == 1){   // single crossig is good enough to detect, dont want to repeat the calculations 
00104     raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
00105     if(raw_sum < hs_foot_up_threshold ) {hs_heelstrike = 0;}
00106   }
00107 
00109 }
00110 
00111 int hs_get(void){
00112   return hs_heelstrike;
00113 }
00114 
00115 float hs_get_float(void){
00116   if(hs_heelstrike){return 1.0;}
00117   return 0.0;
00118 }
Generated on Tue Jun 29 16:36:14 2010 by  doxygen 1.6.3