/* Sends commands to the motors based on the current state of the
* walking finite state machine */
void sendMotorCommands(void) {
float push = CtrlWalk_ankPush;
switch (WALK_FSM_MODE_PREV) {
case Glide_Out:
holdStance_ankOut();
flipUp_ankInn();
hipGlide();
break;
case Push_Out:
flipDown_ankInn();
pushOff_ankOut(push);
hipHold(CtrlWalk_hipHold);
break;
case Glide_Inn:
flipUp_ankOut();
holdStance_ankInn();
hipGlide();
break;
case Push_Inn:
flipDown_ankOut();
pushOff_ankInn(push);
hipHold(CtrlWalk_hipHold);
break;
case Flight: // In the air, get ready for Glide_Out
holdStance_ankOut();
flipUp_ankInn();
disable_hip();
break;
}
}
/* Hold both feet in stance and disable the hip. Run the walking finite state
* machine, and check transitions based on LED sequence. */
void test_walkFsmTransitions(void) {
updateWalkFsm();
setWalkFsmLed();
disable_hip();
holdStance_ankOut();
holdStance_ankInn();
}
/* --9-- The robot holds the outer feet while the inner feet alternate
* between flip-up and flip-down to hold. Used to test the basic
* wrapper functions on the ankles */
void test_flipUpDownHold_outer(void) {
float period = 2.0;
float upDownTest;
float time = getTime();
upDownTest = SquareWave(time, period, -1.0, 1.0);
if (upDownTest > 0.0) {
flipUp_ankInn();
} else {
flipDown_ankInn();
}
disable_hip();
holdStance_ankOut();
}
/* Sends commands to the motors based on the current state of the
* walking finite state machine */
void sendMotorCommands(void) {
float hipTargetAngle;
float push = CtrlWalk_ankPush;
switch (WALK_FSM_MODE_PREV) {
case Glide_Out:
holdStance_ankOut();
flipUp_ankInn();
hipGlide();
break;
case Push1_Out:
case Push2_Out:
flipDown_ankInn();
pushOff_ankOut(push);
hipTargetAngle = getTargetHipAngle(STATE_th0, STATE_th1, CtrlWalk_critStepLen);
hipHold(hipTargetAngle);
break;
case Glide_Inn:
holdStance_ankInn();
flipUp_ankOut();
hipGlide();
break;
case Push1_Inn:
case Push2_Inn:
flipDown_ankOut();
pushOff_ankInn(push);
hipTargetAngle = getTargetHipAngle(STATE_th1, STATE_th0, CtrlWalk_critStepLen);
hipHold(hipTargetAngle);
break;
case Flight: // In the air, get ready for Glide_Out
holdStance_ankOut();
flipUp_ankInn();
disable_hip();
break;
}
}
/* --14-- Put the robot in double stance, and set the ID_CTRL_TEST_R0 to be a
* value between 0.0 (no push-off) and 1.0 (maximum push-off). The robot
* will then alternate between push-off and stance hold on the outer feet,
* waiting 4.0 seconds between each change. The inner feet remain in stance
* hold the entire time, and the hip just (weakly) holds a constant angle */
void test_pushOff_inner(void) {
float kp_hip = 10; // weak gains on the hip
float kd_hip = 0.5;
float qh_hold = 0.3; // hold this angle
float period = 8.0; // 4 seconds push, 4 seconds hold
float modeSignal; // -1.0 for push, 1.0 for hold
float push; // Amplitude of the push-off feed-forward term, from labVIEW
float time = getTime();
push = mb_io_get_float(ID_CTRL_TEST_R0);
modeSignal = SquareWave(time, period, -1.0, 1.0);
if (modeSignal < 0.0) {
pushOff_ankOut(push);
} else {
holdStance_ankOut();
}
holdStance_ankInn();
trackRel_hip(qh_hold, kp_hip, kd_hip);
}
/* --15-- Holds both feet in stance. The behavior of the hip:
* double stance = do nothing
* flight = hold zero
* single stance outer = hold pos
* single stance inner = hold neg */
void test_hipHold() {
holdStance_ankOut();
holdStance_ankInn();
hipHold(0.3);
}
/* --11-- Runs hip-scissor tracking, but using gains and set-points
* from labview and the constant parameters header file for walking */
void test_hipGlide_outer() {
holdStance_ankOut();
flipUp_ankInn();
hipGlide();
}
