heelstrike.c

Go to the documentation of this file.

#include <includes.h>

// constant states used by state machine: 
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)
static int hs_state = HS_LOOKING4HS; 
  
// things software_setup initiates:
static int hs_cmax = 0;
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)
static INT_VOID_F hs_get_left = intvoid;                           // Returns the analog signal from the left heel sensor. 
static INT_VOID_F hs_get_right = intvoid;                          //Returns the analog signal from the right heel sensor. 

// output of the algorithm: HS
static int hs_heelstrike = 0;                             // 0 if leg in air (not individual feet), 1 if on ground

// other variables I use
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
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| 
static int hs_counter = 0;                                                                                
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


void hs_init(float exp_filter_coeff, int confidence, int threshold,  int cmax, INT_VOID_F get_right, INT_VOID_F get_left){

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)
hs_confidence = int_to_fixed(confidence);
hs_threshold = int_to_fixed(threshold); 
hs_cmax = cmax;                                  
hs_get_left = get_left;                                    // Returns the analog signal from the left heel sensor. 
hs_get_right = get_right; 
}


void hs_update(void){

  switch (hs_state){ 
    case HS_IDLESWING:   break;
    case HS_DATACOLLECT: hs_calc_filtered_data(); break;
    case HS_LOOKING4HS:  hs_look4hs(); break;
    case HS_IDLESTANCE:  break;
    case HS_LOOKING4HUP: hs_look4hup(); break;
  }
}


void hs_set_state(float state){

  int next_state = (int)state;
  mcu_led_green_blink(10);
  
  if (next_state != hs_state){
    switch (next_state){   // RESET COUNTER ETC HERE 
      case HS_IDLESWING: 
            break;
      case HS_DATACOLLECT: 
            raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());                              
            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)
            break;
      case HS_LOOKING4HS: 
            break;
      case HS_IDLESTANCE: 
            break;
      case HS_LOOKING4HUP: 
            hs_foot_up_threshold = raw_sum;               // because I beleive the raw_sum still has the previous HS 's foot_sum_data
            break;
    }  
    hs_state = next_state;
  }
  
}


void hs_calc_filtered_data(void){
 
 raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
 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         
 raw_abs_dev = fixed_abs(raw_sum - filtered_sum);
 filtered_abs_dev = raw_abs_dev +  fixed_mult(hs_exp_fiter_coeff, (prev_filtered_abs_dev - raw_abs_dev)) ;  // same filter 
}

void hs_look4hs(void){
  
  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)
    raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
    if((raw_sum > filtered_sum+ fixed_mult(hs_confidence , filtered_abs_dev)) && (raw_sum > filtered_sum + hs_threshold) ){    
      hs_counter++;                                         // increase counter but stop calculating filter
      if(hs_counter >= hs_cmax){ hs_heelstrike = 1;}  
    }else{
      hs_counter = 0 ;                               // counter will be reset after any counter increase for a false HS
      hs_calc_filtered_data();                       // here filter """"MAY GO BAD"""" (if this statement is visited after 1 counter step, you are missing one filter sample)
    }
  }
}

void hs_look4hup(void){

  if (hs_heelstrike == 1){   // single crossig is good enough to detect, dont want to repeat the calculations 
    raw_sum =  int_to_fixed(hs_get_right() + hs_get_left());
    if(raw_sum < hs_foot_up_threshold ) {hs_heelstrike = 0;}
  }

}

int hs_get(void){
  return hs_heelstrike;
}

float hs_get_float(void){
  if(hs_heelstrike){return 1.0;}
  return 0.0;
}