/* --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; } }