void def_foot_outer_fsm(fsm* fo_fsm)
{
//cout << "defining ankle fsm.. " << endl;
// numStates is the number of states defined in structure.h
int numStates = 7;
// numInpts is the number of inputs defined in structure.h
int numInputs = 8;
// construct the fsm
fsm fsm1(numStates, STATE_FO_startstance, numInputs);
char* name = (char*) "foot_outer_fsm";
fsm1.set_name(name);
// associate the states with actions
fsm1.state_def(STATE_FO_stance, ACT_FO_stance_entry, ACT_FO_stance, ACT_FO_stance);
fsm1.state_def(STATE_FO_prepush, ACT_FO_prepush_entry, ACT_FO_prepush, ACT_FO_prepush);
fsm1.state_def(STATE_FO_afterpush, ACT_FO_afterpush_entry, ACT_FO_afterpush, ACT_FO_afterpush);
fsm1.state_def(STATE_FO_flipup, ACT_FO_flipup_entry, ACT_FO_flipup, ACT_FO_flipup);
fsm1.state_def(STATE_FO_flipdown, ACT_FO_flipdown_entry, ACT_FO_flipdown, ACT_FO_flipdown);
fsm1.state_def(STATE_FO_stop, ACT_FO_stop, ACT_FO_stop, ACT_FO_stop);
fsm1.state_def(STATE_FO_startstance, ACT_FO_startstance_entry, ACT_FO_startstance, ACT_FO_startstance);
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_FO_stance, COND_FO_stance_to_prepush, STATE_FO_prepush);
fsm1.trans_def(STATE_FO_stance, COND_FO_stance_to_afterpush, STATE_FO_afterpush); // Emergency transition
fsm1.trans_def(STATE_FO_prepush, COND_FO_prepush_to_afterpush, STATE_FO_afterpush);
fsm1.trans_def(STATE_FO_afterpush, COND_FO_afterpush_to_flipup, STATE_FO_flipup);
fsm1.trans_def(STATE_FO_flipup, COND_FO_flipup_to_flipdown, STATE_FO_flipdown);
fsm1.trans_def(STATE_FO_flipdown, COND_FO_flipdown_to_stance, STATE_FO_stance);
fsm1.trans_def(STATE_FO_startstance, COND_FO_startstance_to_afterpush, STATE_FO_afterpush); // Transition from start stance
fsm1.trans_def(STATE_FO_stance, COND_FO_stop, STATE_FO_stop);
fsm1.trans_def(STATE_FO_afterpush, COND_FO_stop, STATE_FO_stop);
fsm1.trans_def(STATE_FO_flipup, COND_FO_stop, STATE_FO_stop);
fsm1.trans_def(STATE_FO_flipdown, COND_FO_stop, STATE_FO_stop);
fsm1.print_state_transition_matrix();
// define exit state of fsm
fsm1.exit_state_def(STATE_FO_stop);
fsm1.set_sensor_input_function(get_foot_outer_input);
fsm1.set_state_communication_variable(&g_foot_outer_fsm_state);
//set_io_float(ID_MB_FOOT_OUTER_FSM_STATE, (float)g_foot_outer_fsm_state);
copy(fo_fsm, &fsm1);
}
void def_UI_fsm(fsm* h_fsm)
{
// numStates is the number of states in state machine
int numStates = 4;
// numInpts is the number of condition in state machine
int numInputs = 5;
// construct the fsm
fsm fsm1(numStates, STATE_UI_calibrate, numInputs);
char* name = (char*) "ui_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// associate the states with actions
fsm1.state_def(STATE_UI_calibrate, ACTION_UI_calibrate_entry, ACTION_UI_calibrate, ACTION_UI_calibrate);
fsm1.state_def(STATE_UI_standby, ACTION_UI_standby_entry, ACTION_UI_standby, ACTION_UI_standby_exit);
fsm1.state_def(STATE_UI_walk, ACTION_UI_walk_entry, ACTION_UI_walk, ACTION_UI_walk_exit);
fsm1.state_def(STATE_UI_stop, ACTION_UI_stop, ACTION_UI_stop, ACTION_UI_stop);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_UI_calibrate, COND_UI_calibrate_to_walk, STATE_UI_walk);
fsm1.trans_def(STATE_UI_standby, COND_UI_standby_to_calibrate, STATE_UI_calibrate);
fsm1.trans_def(STATE_UI_walk, COND_UI_walk_to_standby, STATE_UI_standby);
fsm1.trans_def(STATE_UI_standby, COND_UI_standby_to_walk, STATE_UI_walk);
fsm1.trans_def(STATE_UI_calibrate, COND_UI_stop, STATE_UI_stop);
fsm1.trans_def(STATE_UI_standby, COND_UI_stop, STATE_UI_stop);
fsm1.trans_def(STATE_UI_walk, COND_UI_stop, STATE_UI_stop);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
fsm1.exit_state_def(STATE_UI_stop);
fsm1.set_sensor_input_function(get_UI_conditions_input);
fsm1.set_state_communication_variable(&g_ui_fsm_state);
copy(h_fsm, &fsm1);
}
void def_steering_fsm(fsm* fi_fsm)
{
//cout << "defining ankle fsm.. " << endl;
// numStates is the number of states defined in structure.h
int numStates = 5;
// numInpts is the number of inputs defined in structure.h
int numInputs = 8;
// construct the fsm
fsm fsm1(numStates, STATE_S_innerlegfront, numInputs);
char* name = (char*) "steering_fsm";
fsm1.set_name(name);
// associate the states with actions
fsm1.state_def(STATE_S_innerlegfront, ACT_S_innerlegfront_entry, ACT_S_innerlegfront, ACT_S_innerlegfront);
fsm1.state_def(STATE_S_innerlegstance, ACT_S_innerlegstance_entry, ACT_S_innerlegstance, ACT_S_innerlegstance);
fsm1.state_def(STATE_S_innerlegback, ACT_S_innerlegback_entry, ACT_S_innerlegback, ACT_S_innerlegback);
fsm1.state_def(STATE_S_innerlegswing, ACT_S_innerlegswing_entry, ACT_S_innerlegswing, ACT_S_innerlegswing);
fsm1.state_def(STATE_S_stop, ACT_S_stop, ACT_S_stop, ACT_S_stop);
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_S_innerlegfront, COND_S_inlegfront_to_inlegstance, STATE_S_innerlegstance);
fsm1.trans_def(STATE_S_innerlegstance, COND_S_inlegstance_to_inlegback, STATE_S_innerlegback);
fsm1.trans_def(STATE_S_innerlegback, COND_S_inlegback_to_inlegswing, STATE_S_innerlegswing);
fsm1.trans_def(STATE_S_innerlegswing, COND_S_inlegswing_to_inlegfront, STATE_S_innerlegfront);
//Stop from any state based on COND
fsm1.trans_def(STATE_S_innerlegfront, COND_S_inlegfront_to_stop, STATE_S_stop);
fsm1.trans_def(STATE_S_innerlegstance, COND_S_inlegstance_to_stop, STATE_S_stop);
fsm1.trans_def(STATE_S_innerlegback, COND_S_inlegback_to_stop, STATE_S_stop);
fsm1.trans_def(STATE_S_innerlegswing, COND_S_inlegswing_to_stop, STATE_S_stop);
fsm1.print_state_transition_matrix();
// define exit state of fsm
fsm1.exit_state_def(STATE_S_stop);
fsm1.set_sensor_input_function(get_steering_input);
fsm1.set_state_communication_variable(&g_steering_fsm_state);
//set_io_float(ID_MB_FOOT_INNER_FSM_STATE, (float)g_foot_inner_fsm_state);
copy(fi_fsm, &fsm1);
}
void def_system_init_fsm(fsm* h_fsm)
{
//cout << endl;
//cout << "defining hip_fsm.. " << endl;
// numStates is the number of states defined in structure.h
int numStates = 3;
// numInpts is the number of inputs defined in structure.h
int numInputs = 2;
// construct the fsm
fsm fsm1(numStates, STATE_SI_system_start, numInputs);
char* name = (char*) "system_init_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// associate the states with actions
fsm1.state_def(STATE_SI_system_start, ACTION_SI_system_start, ACTION_SI_system_start, ACTION_SI_system_start);
fsm1.state_def(STATE_SI_system_run, ACTION_SI_system_run, ACTION_SI_system_run, ACTION_SI_system_run);
fsm1.state_def(STATE_SI_stop, ACTION_SI_stop, ACTION_SI_stop, ACTION_SI_stop);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_SI_system_start, COND_SI_all_ready, STATE_SI_system_run);
fsm1.trans_def(STATE_SI_system_run, COND_SI_stop, STATE_SI_stop);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
fsm1.exit_state_def(STATE_SI_stop);
fsm1.set_sensor_input_function(get_system_init_conditions_input);
fsm1.set_state_communication_variable(&g_system_init_fsm_state);
//set_io_float(ID_MB_SYSTEM_INIT_FSM_STATE, (float)g_system_init_fsm_state);
copy(h_fsm, &fsm1);
}
void def_rock_hip_fsm(fsm* rh_fsm)
{
// numStates is the number of states in the state machine
int numStates = 3;
// numInpts is the number of conditions defined in hip_fsm_conditions.h
int numInputs = 5;
// construct the fsm
// define start state
fsm fsm1(numStates, STATE_rock_HIP_set, numInputs);
char* name = (char*) "rock_hip_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
// associate the states with actions
fsm1.state_def(STATE_rock_HIP_hold, ACT_rock_HIP_hold_entry, ACT_rock_HIP_hold, ACT_rock_HIP_hold);
fsm1.state_def(STATE_rock_HIP_set, ACT_rock_HIP_set_entry, ACT_rock_HIP_set, ACT_rock_HIP_set);
fsm1.state_def(STATE_rock_HIP_free, ACT_rock_HIP_free_entry, ACT_rock_HIP_free, ACT_rock_HIP_free);
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
//These are the arrows in the state machine diagram
fsm1.trans_def(STATE_rock_HIP_hold, COND_rock_HIP_hold_to_set, STATE_rock_HIP_set);
fsm1.trans_def(STATE_rock_HIP_hold, COND_rock_HIP_hold_to_free, STATE_rock_HIP_free);
fsm1.trans_def(STATE_rock_HIP_set, COND_rock_HIP_set_to_free, STATE_rock_HIP_free);
fsm1.trans_def(STATE_rock_HIP_free, COND_rock_HIP_free_to_hold, STATE_rock_HIP_hold);
fsm1.trans_def(STATE_rock_HIP_free, COND_rock_HIP_free_to_set, STATE_rock_HIP_set);
fsm1.print_state_transition_matrix();
//define exit state of fsm
//fsm1.exit_state_def(STATE_H_stop);
fsm1.set_sensor_input_function(get_rock_hip_sensor_input);
fsm1.set_state_communication_variable(&g_rock_hip_fsm_state);
//This is set in ui_fsm currently
//set_io_float(ID_MB_HIP_FSM_STATE, (float)g_hip_fsm_state);
copy(rh_fsm, &fsm1);
}
void def_foot_outer_fsm(fsm* fi_fsm)
{
// numStates is the number of states in the feet innner state machine
int numStates = 2;
// numInpts is the number of conditions
int numInputs = 2;
// starting state
fsm fsm1(numStates, STATE_FO_flipdown, numInputs);
// Give it a name
char* name = (char*) "foot_outer_fsm";
fsm1.set_name(name);
// associate the states with actions
fsm1.state_def(STATE_FO_flipup, ACT_FO_flipup);
fsm1.state_def(STATE_FO_flipdown, ACT_FO_flipdown);
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_FO_flipup, COND_FO_flipup_to_flipdown, STATE_FO_flipdown);
fsm1.trans_def(STATE_FO_flipdown, COND_FO_flipdown_to_flipup, STATE_FO_flipup);
fsm1.print_state_transition_matrix();
// define exit state of fsm
//fsm fsm1(numStates, STATE_FI_flipdown, numInputs);
fsm1.set_sensor_input_function(get_foot_outer_input);
fsm1.set_state_communication_variable(&g_foot_outer_fsm_state);
copy(fi_fsm, &fsm1);
}
int main( int argc , char* argv[] ){
int symbol;
int i = 0;
printf( "Enter input string\n" );
while ( (symbol = getchar()) !=EOF ){
if ( symbol == 48 || symbol == 49 ){
i++;
fsm1( symbol );
fsm2( symbol );
fsm3( symbol, i );
}
}
if ( state_a == 3 )
printf( "\tLanguage1 accepts input string\n" );
else if ( state_a != 3)
printf( "\tLanguage1 rejects input string\n" );
if ( state_b == 2 )
printf("\tLanguage2 accepts input string\n" );
else if ( state_b != 2)
printf( "\tLanguage2 rejects input string\n" );
if ( state_c == 1 )
printf( "\tLanguage3 accepts input string\n" );
else if ( state_c != 1 )
printf( "\tLanguage3 rejects input string\n" );
return 0;
}
void def_hip_fsm(fsm* h_fsm)
{
//cout << endl;
//cout << "defining hip_fsm.. " << endl;
// numStates is the number of states defined in structure.h
int numStates = 10;
// numInpts is the number of inputs defined in structure.h
int numInputs = 12;
// construct the fsm
// define start state
fsm fsm1(numStates, STATE_HI_starthold, numInputs);
char* name = (char*) "hip_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
// associate the states with actions
fsm1.state_def(STATE_HI_starthold, ACT_HI_starthold_entry, ACT_HI_starthold, ACT_HI_starthold);
fsm1.state_def(STATE_HI_preswing, ACT_HI_preswing, ACT_HI_preswing, ACT_HI_preswing);
fsm1.state_def(STATE_HI_premid, ACT_HI_premid_entry, ACT_HI_premid, ACT_HI_premid);
fsm1.state_def(STATE_HI_aftermid, ACT_HI_aftermid_entry, ACT_HI_aftermid, ACT_HI_aftermid);
fsm1.state_def(STATE_HO_preswing, ACT_HO_preswing, ACT_HO_preswing, ACT_HO_preswing);
fsm1.state_def(STATE_HO_premid, ACT_HO_premid_entry, ACT_HO_premid, ACT_HO_premid);
fsm1.state_def(STATE_HO_aftermid, ACT_HO_aftermid_entry, ACT_HO_aftermid, ACT_HO_aftermid);
fsm1.state_def(STATE_H_stop, ACT_H_stop, ACT_H_stop, ACT_H_stop);
fsm1.state_def(STATE_HI_ehold, ACT_HI_ehold_entry, ACT_HI_ehold, ACT_HI_ehold); //Emergency inner hold state
fsm1.state_def(STATE_HO_ehold, ACT_HO_ehold_entry, ACT_HO_ehold, ACT_HO_ehold); //Emergency outer hold state
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_HI_starthold, COND_HI_starthold_to_HO_preswing, STATE_HO_preswing);
fsm1.trans_def(STATE_HO_preswing, COND_HO_preswing_to_HO_premid, STATE_HO_premid);
fsm1.trans_def(STATE_HO_premid, COND_HO_premid_to_HO_aftermid, STATE_HO_aftermid);
fsm1.trans_def(STATE_HO_aftermid, COND_HO_aftermid_to_HO_ehold, STATE_HO_ehold);
fsm1.trans_def(STATE_HO_aftermid, COND_HO_aftermid_to_HI_preswing, STATE_HI_preswing); //Added 4/4/2010
fsm1.trans_def(STATE_HO_ehold, COND_HO_ehold_to_HI_preswing, STATE_HI_preswing);
fsm1.trans_def(STATE_HI_preswing, COND_HI_preswing_to_HI_premid, STATE_HI_premid);
fsm1.trans_def(STATE_HI_premid, COND_HI_premid_to_HI_aftermid, STATE_HI_aftermid);
fsm1.trans_def(STATE_HI_aftermid, COND_HI_aftermid_to_HI_ehold, STATE_HI_ehold);
fsm1.trans_def(STATE_HI_aftermid, COND_HI_aftermid_to_HO_preswing, STATE_HO_preswing); //Added 4/4/2010
fsm1.trans_def(STATE_HI_ehold, COND_HI_ehold_to_HO_preswing, STATE_HO_preswing);
fsm1.trans_def(STATE_HI_preswing, COND_H_stop, STATE_H_stop);
fsm1.trans_def(STATE_HO_preswing, COND_H_stop, STATE_H_stop);
fsm1.print_state_transition_matrix();
// define exit state of fsm
fsm1.exit_state_def(STATE_H_stop);
fsm1.set_sensor_input_function(get_hip_sensor_input);
fsm1.set_state_communication_variable(&g_hip_fsm_state);
//set_io_float(ID_MB_HIP_FSM_STATE, (float)g_hip_fsm_state);
copy(h_fsm, &fsm1);
}
void def_foot_inner_fsm(fsm* fi_fsm)
{
// numStates is the number of states in the feet innner state machine
int numStates = 9;
// numInpts is the number of conditions
int numInputs = 12;
// starting state
fsm fsm1(numStates, STATE_FI_startflipdown, numInputs);
// Give it a name
char* name = (char*) "foot_inner_fsm";
fsm1.set_name(name);
// associate the states with actions
fsm1.state_def(STATE_FI_stance, ACT_FI_stance_entry, ACT_FI_stance, ACT_FI_stance);
fsm1.state_def(STATE_FI_prepush, ACT_FI_prepush_entry, ACT_FI_prepush, ACT_FI_prepush);
fsm1.state_def(STATE_FI_afterpush, ACT_FI_afterpush_entry, ACT_FI_afterpush, ACT_FI_afterpush);
fsm1.state_def(STATE_FI_flipup, ACT_FI_flipup_entry, ACT_FI_flipup, ACT_FI_flipup);
fsm1.state_def(STATE_FI_flipdown, ACT_FI_flipdown_entry, ACT_FI_flipdown, ACT_FI_flipdown);
fsm1.state_def(STATE_FI_stop, ACT_FI_stop, ACT_FI_stop, ACT_FI_stop);
fsm1.state_def(STATE_FI_startflipdown, ACT_FI_startflipdown_entry, ACT_FI_startflipdown, ACT_FI_startflipdown); //Start state
fsm1.state_def(STATE_FI_stopstance, ACT_FI_stopstance_entry, ACT_FI_stopstance, ACT_FI_stopstance); //Stop stance
fsm1.state_def(STATE_FI_startafterpush, ACT_FI_startafterpush_entry, ACT_FI_startafterpush, ACT_FI_startafterpush); //Start state
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
fsm1.trans_def(STATE_FI_stance, COND_FI_stance_to_prepush, STATE_FI_prepush);
fsm1.trans_def(STATE_FI_stance, COND_FI_stance_to_afterpush, STATE_FI_afterpush); //Emergency transition
fsm1.trans_def(STATE_FI_prepush, COND_FI_prepush_to_afterpush, STATE_FI_afterpush);
fsm1.trans_def(STATE_FI_afterpush, COND_FI_afterpush_to_flipup, STATE_FI_flipup);
fsm1.trans_def(STATE_FI_flipup, COND_FI_flipup_to_flipdown, STATE_FI_flipdown);
fsm1.trans_def(STATE_FI_flipdown, COND_FI_flipdown_to_stance, STATE_FI_stance);
fsm1.trans_def(STATE_FI_startflipdown, COND_FI_startflipdown_to_stance, STATE_FI_stance); // Transition from start flipdown
fsm1.trans_def(STATE_FI_stance, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_prepush, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_afterpush, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_flipup, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_flipdown, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_stopstance, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_startafterpush, COND_FI_startflipdown, STATE_FI_startflipdown);
fsm1.trans_def(STATE_FI_stance, COND_FI_stance_to_stopstance, STATE_FI_stopstance); //Stop stance transition
fsm1.trans_def(STATE_FI_prepush, COND_FI_prepush_to_stopstance, STATE_FI_stopstance);
fsm1.trans_def(STATE_FI_stopstance, COND_FI_stopstance_to_startafterpush, STATE_FI_startafterpush); //Start in afterpush from stopping
fsm1.print_state_transition_matrix();
// define exit state of fsm
fsm1.exit_state_def(STATE_FI_stop);
fsm1.set_sensor_input_function(get_foot_inner_input);
fsm1.set_state_communication_variable(&g_foot_inner_fsm_state);
copy(fi_fsm, &fsm1);
}
void def_foot_outer_fsm(fsm* fo_fsm)
{
// numStates is the number of states in the state machine
int numStates = 9;
// numInpts is the number of conditions in the state machine
int numInputs = 12;
// start state
fsm fsm1(numStates, STATE_FO_startstance, numInputs);
char* name = (char*) "foot_outer_fsm";
fsm1.set_name(name);
// associate the states with actions
fsm1.state_def(STATE_FO_stance, ACT_FO_stance_entry, ACT_FO_stance, ACT_FO_stance);
fsm1.state_def(STATE_FO_prepush, ACT_FO_prepush_entry, ACT_FO_prepush, ACT_FO_prepush);
fsm1.state_def(STATE_FO_afterpush, ACT_FO_afterpush_entry, ACT_FO_afterpush, ACT_FO_afterpush);
fsm1.state_def(STATE_FO_flipup, ACT_FO_flipup_entry, ACT_FO_flipup, ACT_FO_flipup);
fsm1.state_def(STATE_FO_flipdown, ACT_FO_flipdown_entry, ACT_FO_flipdown, ACT_FO_flipdown);
fsm1.state_def(STATE_FO_stop, ACT_FO_stop, ACT_FO_stop, ACT_FO_stop);
fsm1.state_def(STATE_FO_startstance, ACT_FO_startstance_entry, ACT_FO_startstance, ACT_FO_startstance);
fsm1.state_def(STATE_FO_stopstance, ACT_FO_stopstance_entry, ACT_FO_stopstance, ACT_FO_stopstance); //Stop stance
fsm1.state_def(STATE_FO_startafterpush, ACT_FO_startafterpush_entry, ACT_FO_startafterpush, ACT_FO_startafterpush); //Stop stance
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
//THese are the arrows in the state machine diagram
fsm1.trans_def(STATE_FO_stance, COND_FO_stance_to_prepush, STATE_FO_prepush);
fsm1.trans_def(STATE_FO_stance, COND_FO_stance_to_afterpush, STATE_FO_afterpush); // Emergency transition
fsm1.trans_def(STATE_FO_prepush, COND_FO_prepush_to_afterpush, STATE_FO_afterpush);
fsm1.trans_def(STATE_FO_afterpush, COND_FO_afterpush_to_flipup, STATE_FO_flipup);
fsm1.trans_def(STATE_FO_flipup, COND_FO_flipup_to_flipdown, STATE_FO_flipdown);
fsm1.trans_def(STATE_FO_flipdown, COND_FO_flipdown_to_stance, STATE_FO_stance);
fsm1.trans_def(STATE_FO_startstance, COND_FO_startstance_to_afterpush, STATE_FO_afterpush); // Transition from start stance
//Allow to transition to startstance from all states
fsm1.trans_def(STATE_FO_stance, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_prepush, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_afterpush, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_flipup, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_flipdown, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_stopstance, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_startafterpush, COND_FO_startstance, STATE_FO_startstance);
fsm1.trans_def(STATE_FO_stance, COND_FO_stance_to_stopstance, STATE_FO_stopstance); //Stop stance transition
fsm1.trans_def(STATE_FO_prepush, COND_FO_prepush_to_stopstance, STATE_FO_stopstance);
fsm1.trans_def(STATE_FO_stopstance, COND_FO_stopstance_to_startafterpush, STATE_FO_startafterpush); //Start in afterpush from stopping
fsm1.print_state_transition_matrix();
// define exit state of fsm
fsm1.exit_state_def(STATE_FO_stop);
fsm1.set_sensor_input_function(get_foot_outer_input);
fsm1.set_state_communication_variable(&g_foot_outer_fsm_state);
//This assignment is done in ui_fsm
//set_io_float(ID_MB_FOOT_OUTER_FSM_STATE, (float)g_foot_outer_fsm_state);
copy(fo_fsm, &fsm1);
}
void def_hip_fsm(fsm* h_fsm)
{
// numStates is the number of states in the state machine
int numStates = 10;
// numInpts is the number of conditions defined in hip_fsm_conditions.h
int numInputs = 15;
// construct the fsm
// define start state
fsm fsm1(numStates, STATE_HI_starthold, numInputs);
char* name = (char*) "hip_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
// associate the states with actions
fsm1.state_def(STATE_HI_starthold, ACT_HI_starthold_entry, ACT_HI_starthold, ACT_HI_starthold); //Beginning start hold state
fsm1.state_def(STATE_HI_preswing, ACT_HI_preswing_entry, ACT_HI_preswing, ACT_HI_preswing);
fsm1.state_def(STATE_HI_premid, ACT_HI_premid_entry, ACT_HI_premid, ACT_HI_premid_exit);
fsm1.state_def(STATE_HI_aftermid, ACT_HI_aftermid_entry, ACT_HI_aftermid, ACT_HI_aftermid);
fsm1.state_def(STATE_HO_preswing, ACT_HO_preswing_entry, ACT_HO_preswing, ACT_HO_preswing);
fsm1.state_def(STATE_HO_premid, ACT_HO_premid_entry, ACT_HO_premid, ACT_HO_premid_exit);
fsm1.state_def(STATE_HO_aftermid, ACT_HO_aftermid_entry, ACT_HO_aftermid, ACT_HO_aftermid);
fsm1.state_def(STATE_H_stop, ACT_H_stop, ACT_H_stop, ACT_H_stop); //Stop state
fsm1.state_def(STATE_HI_ehold, ACT_HI_ehold_entry, ACT_HI_ehold, ACT_HI_ehold); //Emergency inner hold state
fsm1.state_def(STATE_HO_ehold, ACT_HO_ehold_entry, ACT_HO_ehold, ACT_HO_ehold); //Emergency outer hold state
fsm1.print_state_transition_matrix();
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
//These are the arrows in the state machine diagram
fsm1.trans_def(STATE_HI_starthold, COND_HI_starthold_to_HO_preswing, STATE_HO_preswing);
fsm1.trans_def(STATE_HO_preswing, COND_HO_preswing_to_HO_premid, STATE_HO_premid);
fsm1.trans_def(STATE_HO_premid, COND_HO_premid_to_HO_aftermid, STATE_HO_aftermid);
fsm1.trans_def(STATE_HO_aftermid, COND_HO_aftermid_to_HI_preswing, STATE_HI_preswing);
fsm1.trans_def(STATE_HO_ehold, COND_HO_ehold_to_HI_preswing, STATE_HI_preswing);
fsm1.trans_def(STATE_HI_preswing, COND_HI_preswing_to_HI_premid, STATE_HI_premid);
fsm1.trans_def(STATE_HI_premid, COND_HI_premid_to_HI_aftermid, STATE_HI_aftermid);
fsm1.trans_def(STATE_HI_aftermid, COND_HI_aftermid_to_HO_preswing, STATE_HO_preswing);
fsm1.trans_def(STATE_HI_ehold, COND_HI_ehold_to_HO_preswing, STATE_HO_preswing);
fsm1.trans_def(STATE_HI_premid, COND_HI_premid_to_HI_ehold, STATE_HI_ehold);
fsm1.trans_def(STATE_HO_premid, COND_HO_premid_to_HO_ehold, STATE_HO_ehold);
fsm1.trans_def(STATE_HI_aftermid, COND_HI_aftermid_to_HI_ehold, STATE_HI_ehold);
fsm1.trans_def(STATE_HO_aftermid, COND_HO_aftermid_to_HO_ehold, STATE_HO_ehold);
fsm1.trans_def(STATE_HI_preswing, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HI_premid, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HI_aftermid, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HI_ehold, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HO_preswing, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HO_premid, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HO_aftermid, COND_HI_starthold, STATE_HI_starthold);
fsm1.trans_def(STATE_HO_ehold, COND_HI_starthold, STATE_HI_starthold);
fsm1.print_state_transition_matrix();
//define exit state of fsm
fsm1.exit_state_def(STATE_H_stop);
fsm1.set_sensor_input_function(get_hip_sensor_input);
fsm1.set_state_communication_variable(&g_hip_fsm_state);
//This is set in ui_fsm currently
//set_io_float(ID_MB_HIP_FSM_STATE, (float)g_hip_fsm_state);
copy(h_fsm, &fsm1);
}
void def_top_fsm(fsm* h_fsm)
{
// numStates is the number of states in state machine
int numStates = 7;
// numInpts is the number of condition in state machine
int numInputs = 11;
// construct the fsm
fsm fsm1(numStates, STATE_top_walk_normal, numInputs);
char* name = (char*) "top_fsm";
fsm1.set_name(name);
//debug
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// associate the states with actions
fsm1.state_def(STATE_top_walk_normal, ACTION_top_walk_normal_entry, ACTION_top_walk_normal, ACTION_top_walk_normal);
fsm1.state_def(STATE_top_walk_slow, ACTION_top_walk_slow_entry, ACTION_top_walk_slow, ACTION_top_walk_slow);
fsm1.state_def(STATE_top_walk_fast, ACTION_top_walk_fast_entry, ACTION_top_walk_fast, ACTION_top_walk_fast);
fsm1.state_def(STATE_top_walk_stop, ACTION_top_walk_stop_entry, ACTION_top_walk_stop, ACTION_top_walk_stop);
fsm1.state_def(STATE_top_walk_start, ACTION_top_walk_start_entry, ACTION_top_walk_start, ACTION_top_walk_start);
fsm1.state_def(STATE_top_stop, ACTION_top_stop, ACTION_top_stop, ACTION_top_stop);
fsm1.state_def(STATE_top_temp, ACTION_top_temp, ACTION_top_temp, ACTION_top_temp);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
///////////////////////////////////////////////////////////////////////////////////////////////////
// define the transitions
// a transition needs to be defined for every state for every input
// for the sensors which dont make a difference it should stay in the same state
//fsm1.trans_def(STATE_top_walk_normal, COND_top_walk_normal_to_walk_stop, STATE_top_walk_stop);
fsm1.trans_def(STATE_top_walk_normal, COND_top_walk_normal_to_walk_fast, STATE_top_walk_fast);
fsm1.trans_def(STATE_top_walk_normal, COND_top_walk_normal_to_temp, STATE_top_temp);
//fsm1.trans_def(STATE_top_walk_normal, COND_top_walk_normal_to_walk_slow, STATE_top_walk_slow);
//fsm1.trans_def(STATE_top_walk_stop, COND_top_walk_stop_to_walk_start, STATE_top_walk_start); //on rc command
fsm1.trans_def(STATE_top_walk_stop, COND_top_walk_stop_to_walk_normal, STATE_top_walk_normal); //on pressing fsm_reset
//fsm1.trans_def(STATE_top_walk_start, COND_top_walk_start_to_walk_normal, STATE_top_walk_normal); //on pressing fsm_reset
fsm1.trans_def(STATE_top_walk_fast, COND_top_walk_normal, STATE_top_walk_normal);
fsm1.trans_def(STATE_top_walk_slow, COND_top_walk_normal, STATE_top_walk_normal);
fsm1.trans_def(STATE_top_temp, COND_top_temp_to_walk_slow, STATE_top_walk_slow);
fsm1.trans_def(STATE_top_temp, COND_top_temp_to_walk_stop, STATE_top_walk_stop);
///////////////////////////////////////////////////////////////////////////////////////////////////
fsm1.print_state_transition_matrix();
fsm1.exit_state_def(STATE_top_stop);
fsm1.set_sensor_input_function(get_top_conditions_input);
fsm1.set_state_communication_variable(&g_top_fsm_state);
copy(h_fsm, &fsm1);
}
