/* --19-- Holds the outer feet in stance and the inner feet are
* flipped up. The hip motor is disabled */
void debug_singleStanceOuter(void) {
float kp_ank = 7.0;
float kd_ank = 1.0;
trackAbs_ankOut(0.0, kp_ank, kd_ank);
trackRel_ankInn(0.2, kp_ank, kd_ank);
disable_hip();
}
/* 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();
}
/* 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;
}
}
/* --2-- Hold both ankles at zero absolute angle */
void test_trackAbs_ankle() {
float kp = 7.0;
float kd = 1.0;
float phi = 0.0; // Absolute tracking reference angle
trackAbs_ankOut(phi, kp, kd);
trackAbs_ankInn(phi, kp, kd);
disable_hip();
}
/* --17-- Set the motor controllers to hold the feet of the robot in the correct
* configuration for double stance standing, while turning off the hip motor.
* The user can then rock the robot up onto the inner
* or outer feet to check that the contact sensors are working well */
void test_doubleStanceContact(void) {
float kp_ank = 7.0;
float kd_ank = 1.0;
trackAbs_ankOut(0.0, kp_ank, kd_ank);
trackAbs_ankInn(0.0, kp_ank, kd_ank);
disable_hip();
}
/* --10-- The robot holds the inner feet while the outer feet alternate
* between flip-up and flip-down to hold. Used to test the basic
* wrapper functions on the ankles */
void test_flipUpDownHold_inner(void) {
float period = 2.0;
float upDownTest;
float time = getTime();
upDownTest = SquareWave(time, period, -1.0, 1.0);
if (upDownTest > 0.0) {
flipUp_ankOut();
} else {
flipDown_ankOut();
}
disable_hip();
holdStance_ankInn();
}
/* Wrapper function.
* The hip holds various angles based on the contact configuration.
* @param qh = magnitude (positive) of the hip angle during single stance.
* double stance = do nothing
* flight = hold zero
* single stance outer = hold pos
* single stance inner = hold neg */
void hipHold(float qh) {
switch (STATE_contactMode) {
case CONTACT_S0:
trackRel_hip(qh, LABVIEW_HIP_KP, LABVIEW_HIP_KD);
break;
case CONTACT_S1:
trackRel_hip(-qh, LABVIEW_HIP_KP, LABVIEW_HIP_KD);
break;
case CONTACT_DS:
disable_hip();
break;
case CONTACT_FL:
trackRel_hip(0.0, LABVIEW_HIP_KP, LABVIEW_HIP_KD);
break;
}
}
/* --3-- Have the outer ankles track a square wave and the inner ankles track
* a triangle wave. The max and min values are set based on reasonable values
* for flip-up and push-off target angles. The reference angles are sent out
* on ID_CTRL_TEST_W0 and ID_CTRL_TEST_W1. */
void test_trackRel_ankle() {
float kp = 7.0;
float kd = 1.0;
float max = 2.6; // Push-off
float min = 0.2; // Flip-up
float period = 2.0;
float q0, q1; // Target angles
float time = getTime();
q0 = SquareWave(time, period, min, max);
q1 = TriangleWave(time, period, min, max);
mb_io_set_float(ID_CTRL_TEST_W0, q0);
mb_io_set_float(ID_CTRL_TEST_W1, q1);
trackRel_ankOut(q0, kp, kd);
trackRel_ankInn(q1, kp, kd);
disable_hip();
}
/* 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;
}
}