/* This function is called once per loop, and checks the
* sensors that are used to trigger transitions between
* modes of the finite state machine */
void updateWalkFsm(void) {
WALK_FSM_MODE_PREV = WALK_FSM_MODE;
if (STATE_contactMode == CONTACT_FL) { // Then robot is in the air
WALK_FSM_MODE = Flight; // Give up on walking and enter flight mode
} else { // Run the normal walking finite state machine
switch (WALK_FSM_MODE_PREV) {
case Glide_Out:
if (STATE_th0 < CtrlWalk_critAngle) {
WALK_FSM_MODE = Push_Out;
}
break;
case Push_Out:
if (STATE_c1) { // If inner feet hit the ground
WALK_FSM_MODE = Glide_Inn;
computeHeelStrikeGeometry(); // Update the collision data
}
break;
case Glide_Inn:
if (STATE_th1 < CtrlWalk_critAngle) {
WALK_FSM_MODE = Push_Inn;
}
break;
case Push_Inn:
if (STATE_c0) { // If outer feet hit the ground
WALK_FSM_MODE = Glide_Out;
computeHeelStrikeGeometry(); // Update the collision data
}
break;
case Flight:
if (STATE_c0) { // If outer feet hit the ground
WALK_FSM_MODE = Glide_Out;
// } else if (STATE_c1) { // If inner feet hit the ground
// WALK_FSM_MODE = Glide_Inn;
}
break;
}
}
}
/* This function is called by gaitControl during walking, whenever there is a heel-strike,
* which should occur whenever the foot strikes the ground during walking. */
void triggerHeelStrikeUpdate(void) {
float newStepTime;
float stepDuration;
float stepLength;
/// STEP LENGTH:
stepLength = computeHeelStrikeGeometry();
/// STEP DURATION:
newStepTime = STATE_t; // In seconds
stepDuration = newStepTime - STATE_lastStepTimeSec; // Duration of the last step (seconds)
STATE_lastStepDuration = stepDuration;
STATE_lastStepTimeSec = newStepTime;
mb_io_set_float(ID_EST_LAST_STEP_DURATION, stepDuration);
/// Update the objective function:
logStepData(stepDuration, stepLength);
}
