Maybe I can jump start your logic flows:
The functions in bbot_user_routines.c don’t do anything until they are called.
The calling is initiated from the main switch loop located in Process_Data_From_Master_uP(void) . The main switch loop has three phases (data flow paths) which are entered consecutively based upon meeting the criteria for entering the next phase. Each phase is responsible for calling and processing its own sensor data.
Phase 0 is the init phase. It only reads raw sensor data for initializing. When done forces phase 1.
Phase 1 is the engage phase. Calling engage() reads and processes sensor data and waits for the engage criteria to occur. When satisfied, it forces phase 2 and initializes the cum encoder and the tilt angle filter.
Phase 2 Here rc controller cmds are read and balance , rotation and motor command conditioning functions are called. balance() is given the responsibility for making the calls to proc_sensor() which makes data available to all other functions.
The program will stay in phase 2 until an overtilt() function puts it back into the engage phase 1 where it waits again to engage.
Since you will always go through one of these phases each time you enter Process_Data_From_Master_uP(void) someone will always be calling read_sensor(), but only phase 1 and 2 will call process_sensors().
At the end of the sw(phase) after the motor cmds are set, there is a counter test which allows you to limit the print calls to keep you from getting swamped by 50/s calls. This is where I put print calls unless it is absolutely necessary to mess up a function. Most variables are global so any functions setting a value is good for all.
My processing logic reads like this:
void Process_Data_From_Master_uP(void) {
cum_1=cum_1+1.; // increment print counter
//************************** main balance phase loop
switch ( phase )
{
case 0
{ //init_sensors phase code
break;}
case 1
{//engage phase code
break;}
case 2 (default)
{//balance phase + stuff code
//read rc inputs
//compute the symmetric command for balancing
//compute the rotational command
//sum symmetric and rotational commands
//add biases to compensate for controller dz
//shut off motors if over tilt condition and reset to engage phase 1
}
}
pwm03=pwm_rt ;//right motor
pwm04=pwm_lt ;// left motor
//**end main balance loop ********************
// Now check if we want print
if(cum_1>25) //print out every 25 loops…adjust this for frequency of print
{
// some typical prints I used
printf(" az= %d az_tilt= %d az_raw = %d \n" ,(int)az, (int)az_tilt, (int)az_raw ) ;
printf(“pwm_cmd_sym = %d pwm_cmd_rt = %d pwm_cmd_lt = %d \n”, pwm_cmd_sym, pwm_cmd_rt, pwm_cmd_lt);
//comment these print statements when finished debugging
cum_1=0.;} // reset print loop counter
}
} //end Process_Data_From_Master_uP()
My functions are hierarchical and easiest to understand if you start from a switch phase function and work down to the core functions that support the top level function.