/* --4-- Hang the robot from the ceiling. Send a desired hip
* angle to the robot on ID_CTRL_TEST_R0, and the hip motor
* will produce the torque to cancel out the effect of the
* spring at that angle. If the hip is set to track cancel the
* measured state (not the target state), then you can use
* R1 and R2 to send kp and kd gains respectively.
*
* --> In flight mode, gravity compensation is disabled, since
* it is not clear where the reaction torque is coming from. */
void test_hipCompensation_flight() {
float target = mb_io_get_float(ID_CTRL_TEST_R0);
float kp = mb_io_get_float(ID_CTRL_TEST_R1);
float kd = mb_io_get_float(ID_CTRL_TEST_R2);
disable_ankOut();
disable_ankInn();
trackRel_hip(target, kp, kd);
}
/***************** ENTRY-POINT FUNCTION CALL *****************************
* *
**************************************************************************/
void mb_estimator_update(void) {
clear_UI_LED(); // Clears all LEDs that had been active on the previous cycle
if (INITIALIZE_ESTIMATOR) {
// Initialize the filter coefficients:
setFilterCoeff(&FC_FAST, FILTER_CUTOFF_FAST);
setFilterCoeff(&FC_SLOW, FILTER_CUTOFF_SLOW);
setFilterCoeff(&FC_VERY_SLOW, FILTER_CUTOFF_VERY_SLOW);
// Reset the joint angle rate filters
setFilterData(&FD_OUTER_LEG_ANGLE, ID_UI_ROLL);
setFilterData(&FD_UI_ANG_RATE_X, ID_UI_ANG_RATE_X);
setFilterData(&FD_MCH_ANG_RATE, ID_MCH_ANG_RATE);
setFilterData(&FD_MCFO_RIGHT_ANKLE_RATE, ID_MCFO_RIGHT_ANKLE_RATE);
setFilterData(&FD_MCFI_ANKLE_RATE, ID_MCFI_ANKLE_RATE);
// Reset the contact sensor filters
setFilterData(&FD_MCFO_LEFT_HEEL_SENSE, ID_MCFO_LEFT_HEEL_SENSE);
setFilterData(&FD_MCFO_RIGHT_HEEL_SENSE, ID_MCFO_RIGHT_HEEL_SENSE);
setFilterData(&FD_MCFI_LEFT_HEEL_SENSE, ID_MCFI_LEFT_HEEL_SENSE);
setFilterData(&FD_MCFI_RIGHT_HEEL_SENSE, ID_MCFI_RIGHT_HEEL_SENSE);
// Steering motor stuff:
setFilterData(&FD_MCSI_STEER_ANGLE, ID_MCSI_STEER_ANGLE);
// Robot orientation estimation
resetRobotOrientation();
getIntegralRateGyro(); // Run integral to log the current state of the rate gyro
// Set "once per step" variables:
STATE_lastStepLength = 0.0; // Initialize to zero, for lack of a better plan
STATE_lastStepTimeSec = STATE_t; // cpu clock time at last heel strike.
STATE_lastStepDuration = 0.0; // Duration of the last step (seconds)
STATE_lastEstTime = 0.001 * mb_io_get_float(ID_TIMESTAMP);
// Remember that we've initialized everything properly
INITIALIZE_ESTIMATOR = false;
}
STATE_t = 0.001 * mb_io_get_float(ID_TIMESTAMP); // Robot Time (converted to seconds)
runAllFilters();// Run the butterworth filters:
updateRobotOrientation();
sendTotalPower();
updateEnergyUsage(); // Must come after sendTotalPower()
// Update the state variables: (absolute orientation and rate)
updateRobotState();
// Update controller parameters from LabVIEW
updateParameters();
// Check if the robot fell down
checkIfRobotFellDown();
STATE_lastEstTime = STATE_t; // Update previous estimation time.
return;
}
/* --18-- Gives the user direct control over motor command current
* from LabVIEW, bypassing high-level motor control code.
* Use Carefully!
* ID_CTRL_TEST_R0 == outer ankle command current
* ID_CTRL_TEST_R1 == inner ankle command current
* ID_CTRL_TEST_R2 == hip joint command current */
void debug_directCurrentControl(void) {
mb_io_set_float(ID_MCFO_COMMAND_CURRENT, mb_io_get_float(ID_CTRL_TEST_R0));
mb_io_set_float(ID_MCFO_STIFFNESS, 0.0);
mb_io_set_float(ID_MCFO_DAMPNESS, 0.0);
mb_io_set_float(ID_MCFI_COMMAND_CURRENT, mb_io_get_float(ID_CTRL_TEST_R1));
mb_io_set_float(ID_MCFI_STIFFNESS, 0.0);
mb_io_set_float(ID_MCFI_DAMPNESS, 0.0);
mb_io_set_float(ID_MCH_COMMAND_CURRENT, mb_io_get_float(ID_CTRL_TEST_R2));
mb_io_set_float(ID_MCH_STIFFNESS, 0.0);
mb_io_set_float(ID_MCH_DAMPNESS, 0.0);
}
/* This function sums up the power used by all boards and sends it out as a single
* data value to LabVIEW */
void sendTotalPower(void) {
float power;
// Read CAN bus:
POWER_MCH = mb_io_get_float(ID_MCH_BATT_POWER);
POWER_MCFO = mb_io_get_float(ID_MCFO_BATT_POWER);
POWER_MCFI = mb_io_get_float(ID_MCFI_BATT_POWER);
POWER_OVERHEAD = mb_io_get_float(ID_CSR_MCU_POWER);
// Accumulate power use and send out
power = POWER_MCH + POWER_MCFO + POWER_MCFI + POWER_OVERHEAD;
mb_io_set_float(ID_EST_TOTAL_BATT_POWER, power);
}
/* Updates the robot's state.
* Joint angles are read directly from the sensors
* Robot angle (th0) is updated by runIntegral_outerLegAngle
* Joint rates (dth0, dqh, dq0, dq1) are computed by butterworth filters and updated there. */
void updateRobotState(void) {
// Read the joint angles of the robot
STATE_qh = mb_io_get_float(ID_MCH_ANGLE); // hip angle - between legs
STATE_q0 = mb_io_get_float(ID_MCFO_RIGHT_ANKLE_ANGLE); // outer ankle angle
STATE_q1 = mb_io_get_float(ID_MCFI_MID_ANKLE_ANGLE); // inner ankle angle
// Compute the absolute orientation of the robot's feet and legs
STATE_th1 = STATE_qh + STATE_th0; // absolute orientation of inner legs
STATE_phi0 = PARAM_Phi0 + STATE_th0 - STATE_q0; // absolute orientation of outer feet
STATE_phi1 = PARAM_Phi1 + STATE_qh + STATE_th0 - STATE_q1; // absolute orientation of inner feet
STATE_dth1 = STATE_dqh + STATE_dth0; // absolute orientation rate of inner legs
STATE_dphi0 = STATE_dth0 - STATE_dq0; // absolute orientation rate of outer feet
STATE_dphi1 = STATE_dqh + STATE_dth0 - STATE_dq1; // absolute orientation rate of inner feet
// Send back across network of data logging and diagnostics:
mb_io_set_float(ID_EST_STATE_TH1, STATE_th1);
mb_io_set_float(ID_EST_STATE_PHI0, STATE_phi0);
mb_io_set_float(ID_EST_STATE_PHI1, STATE_phi1);
mb_io_set_float(ID_EST_STATE_DTH1, STATE_dth1);
mb_io_set_float(ID_EST_STATE_DPHI0, STATE_dphi0);
mb_io_set_float(ID_EST_STATE_DPHI1, STATE_dphi1);
mb_io_set_float(ID_EST_STATE_CONTACTMODE, STATE_contactMode); // contact mode (e.g. flying, s0...)
mb_io_set_float(ID_EST_UI_FSM_MODE, UI_FSM_MODE); // mb_controller FSM (e.g. StandBy, WalkCtrl,...)
mb_io_set_float(ID_OPTIM_FSM_MODE, OPTIMIZE_FSM_MODE); // optimizeGait FSM (e.g. INIT, TRIAL,...)
// Figure out the contact mode:
if (STATE_c0 && !STATE_c1) { // Single stance outer
STATE_contactMode = CONTACT_S0;
} else if (!STATE_c0 && STATE_c1) { // single stance inner
STATE_contactMode = CONTACT_S1;
} else if (STATE_c0 && STATE_c1) { // double stance
STATE_contactMode = CONTACT_DS;
} else { // Flight
STATE_contactMode = CONTACT_FL;
resetRobotOrientation(); // use imu sensor fusion to reset the robot orientation
}
// Do a bit of harder math to figure out the horizontal component of the CoM motion
updateComKinematics();
// Send tracking errors: (computed in motorControl.c)
mb_io_set_float(ID_EST_MOTOR_QH_ERR, MOTOR_qh_trackErr);
mb_io_set_float(ID_EST_MOTOR_Q0_ERR, MOTOR_q0_trackErr);
mb_io_set_float(ID_EST_MOTOR_Q1_ERR, MOTOR_q1_trackErr);
}
/* --8-- Robot is standing in single stance on the inner feet.
* The outer legs should track a linear function of the inner
* leg angle, with the constant offset being set by R0 and the
* linear gain being set by R1. */
void test_hipScissorTrack_inner() {
float kp_hip = 14.0;
float kd_hip = 2.0;
float kp_ank = 7.0;
float kd_ank = 1.0;
float offset, rate;
offset = mb_io_get_float(ID_CTRL_TEST_R0);
rate = mb_io_get_float(ID_CTRL_TEST_R1);
trackRel_ankOut(0.2, kp_ank, kd_ank);
trackAbs_ankInn(0.0, kp_ank, kd_ank);
trackScissor_hip(rate, offset, kp_hip, kd_hip);
}
/* Computes the change in the angle of the outer legs of the robot
* that is expected due to the integral of the IMU rate signal over
* the last time step */
float getIntegralRateGyro(void) {
static float dth0_last = 0.0;
float dth0_gyro = mb_io_get_float(ID_UI_ANG_RATE_X);
float trapz = 0.5 * CLOCK_CYCLE_DURATION * (dth0_last + dth0_gyro); // Trapazoid integration
dth0_last = dth0_gyro;
return trapz;
}
/* Initializes the filter to a single value. */
void setFilterData(FilterData * FD, CAN_ID canId) {
float z = mb_io_get_float(canId); // Value of the data
FD->z0 = z;
FD->z1 = z;
FD->z2 = z;
FD->y0 = z;
FD->y1 = z;
FD->y2 = z;
FD->canId = canId;
}
/* Sets the estimate for the step length and the stance leg angle, assuming that the robot
* is in double stance. This is designed to be called once per step, by the walking finite
* state machine. Returns the step length. Posts other data as STATE variables and to CAN
* bus.*/
float computeHeelStrikeGeometry(void) {
float Slope = 0.0; // Assume flat ground for now
float x, y; // scalar distances, in coordinate system aligned with outer legs
float Phi0 = PARAM_Phi0;
float Phi1 = PARAM_Phi1;
float l = PARAM_l;
float d = PARAM_d;
float qh, q0, q1; // robot joint angles
float stepLength, stepAngle;
qh = mb_io_get_float(ID_MCH_ANGLE); // hip angle - between legs
q0 = mb_io_get_float(ID_MCFO_RIGHT_ANKLE_ANGLE); // outer ankle angle
q1 = mb_io_get_float(ID_MCFI_MID_ANKLE_ANGLE); // inner ankle angle
/* Ranger geometry:
* [x;y] = vector from outer foot virtual center to the inner foot
* virtual center, in a frame that is rotated such that qr = 0
* These functions were determined using computer math. The code can
* be found in:
* templates/Estimator/legAngleEstimator/Derive_Eqns.m
*/
x = l * Sin(qh) - d * Sin(Phi1 - q1 + qh) + d * Sin(Phi0 - q0);
y = l + d * Cos(Phi1 - q1 + qh) - l * Cos(qh) - d * Cos(Phi0 - q0);
stepLength = Sqrt(x * x + y * y); // Distance between two contact points
stepAngle = -(Atan(y / x) + Slope); // angle of the outer legs
mb_io_set_float(ID_EST_LAST_STEP_LENGTH, stepLength);
mb_io_set_float(ID_EST_LAST_STEP_ANGLE, stepAngle);
STATE_lastStepLength = stepLength;
heelStrikeGyroReset(stepAngle); // Reset the gyro estiamte for angles at heel-strike
return stepLength;
}
/* --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);
}
/* Entry-point function for all unit tests */
void runUnitTest(void) {
int testId = (int) (mb_io_get_float(ID_CTRL_UNIT_TEST_ID));
switch (testId) {
/**** Ranger Math ****/
case 1: test_waveFunctions(); break;
case 21: test_linInterpVar(); break;
/**** Low-Level Motor Control ****/
case 2: test_trackAbs_ankle(); break;
case 3: test_trackRel_ankle(); break;
case 4: test_hipCompensation_flight(); break;
case 5: test_hipCompensation_outer(); break;
case 6: test_hipCompensation_inner(); break;
case 7: test_hipScissorTrack_outer(); break;
case 8: test_hipScissorTrack_inner(); break;
/**** High-Level Motor Control ****/
case 9: test_flipUpDownHold_outer(); break;
case 10: test_flipUpDownHold_inner(); break;
case 11: test_hipGlide_outer(); break;
case 12: test_hipGlide_inner(); break;
case 13: test_pushOff_outer(); break;
case 14: test_pushOff_inner(); break;
case 15: test_hipHold(); break;
/**** Walking Controller ****/
case 16: walkControl_test(); break; // Calls a function in walkControl.c
/**** Estimation ****/
case 17: test_doubleStanceContact(); break;
/**** Debugging ****/
case 18: debug_directCurrentControl(); break;
case 19: debug_singleStanceOuter(); break;
case 20: debug_steeringMotors(); break;
}
}
/* Updates the Butterworth filter based on new sensor data. The filter runs
* with a period of 1ms, so that it can handle asynchronous data.
* @param canId = the CAN ID for the data channel of interest
* @return y = the filtered data at the current time step
*/
float runFilter(FilterCoeff * FC, FilterData * FD) {
float z = mb_io_get_float(FD->canId);
// Update sensor history:
FD->z2 = FD->z1;
FD->z1 = FD->z0;
FD->z0 = z;
// Update estimate history:
FD->y2 = FD->y1;
FD->y1 = FD->y0;
// Compute new estimate:
FD->y0 =
(FC->b0) * (FD->z0) +
(FC->b1) * (FD->z1) +
(FC->b2) * (FD->z2) -
(FC->a1) * (FD->y1) -
(FC->a2) * (FD->y2);
return (FD->y0);
}
/* Updates any controller parameters that are set from LabVIEW */
void updateParameters(void) {
LABVIEW_HIP_COMPENSATION = mb_io_get_float(ID_CTRL_HIP_COMPENSATION) > 0.5;
LABVIEW_HIP_KP = mb_io_get_float(ID_CTRL_HIP_KP); // hip pd controller p gain
LABVIEW_HIP_KD = mb_io_get_float(ID_CTRL_HIP_KD); // hip pd controller d gain
LABVIEW_ANK_PUSH_KP = mb_io_get_float(ID_CTRL_ANK_PUSH_KP); // push-off ankle p gain
LABVIEW_ANK_PUSH_KD = mb_io_get_float(ID_CTRL_ANK_PUSH_KD); // push-off ankle d gain
LABVIEW_ANK_STANCE_KP = mb_io_get_float(ID_CTRL_ANK_STANCE_KP); // ankle pd controller p gain when foot on ground.
LABVIEW_ANK_STANCE_KD = mb_io_get_float(ID_CTRL_ANK_STANCE_KD); // ankle pd controller d gain when foot on ground.
LABVIEW_ANK_SWING_KP = mb_io_get_float(ID_CTRL_ANK_SWING_KP); // ankle pd controller p gain when foot in air.
LABVIEW_ANK_SWING_KD = mb_io_get_float(ID_CTRL_ANK_SWING_KD); // ankle pd controller d gain when foot in air.
LABVIEW_WALK_ANK_PUSH = mb_io_get_float(ID_CTRL_WALK_ANK_PUSH); // magnitude of the push-off during walking normalized to be on the range 0 to 1
LABVIEW_WALK_CRIT_STEP_LENGTH = mb_io_get_float(ID_CTRL_WALK_CRIT_STEP_LENGTH); // the critical ankle joint separation when push-off should occur
LABVIEW_WALK_SCISSOR_GAIN = mb_io_get_float(ID_CTRL_WALK_SCISSOR_GAIN);
LABVIEW_WALK_SCISSOR_OFFSET = mb_io_get_float(ID_CTRL_WALK_SCISSOR_OFFSET);
LABVIEW_GAIT_USE_CTRL_DATA = mb_io_get_float(ID_CTRL_WALK_USE_CTRL_DATA) > 0.5; // True if walking controller should use off-line generated gait data.
FSM_LED_FLAG = mb_io_get_float(ID_CTRL_FSM_LED) > 0.5; // True if FSM LEDs should be turned on
}
/*Simple function to return the robot time in seconds */
float getTime() {
return 0.001 * mb_io_get_float(ID_TIMESTAMP);
}
/* --20-- Lets the user directly control the inputs to the steering
* motor controller at a low level */
void debug_steeringMotors(void) {
mb_io_set_float(ID_MCSI_COMMAND_CURRENT, mb_io_get_float(ID_CTRL_TEST_R0));
mb_io_set_float(ID_MCSI_STIFFNESS, mb_io_get_float(ID_CTRL_TEST_R1));
mb_io_set_float(ID_MCSI_DAMPNESS, mb_io_get_float(ID_CTRL_TEST_R2));
}
float get_io_float(unsigned short data_id)
{
return mb_io_get_float(data_id);
}
/* Accumulate (integrate) current signals to the motors for push-off
* exit conditions. Implemented using Euler's method. */
void accumulatePushOffCurrent(void) {
WalkFsm_ankOutPushInt += CLOCK_CYCLE_DURATION * Abs(mb_io_get_float(ID_MCFO_MOTOR_CURRENT));
WalkFsm_ankInnPushInt += CLOCK_CYCLE_DURATION * Abs(mb_io_get_float(ID_MCFI_MOTOR_CURRENT));
mb_io_set_float(ID_EST_OUT_PUSH_ACCUMULATED, WalkFsm_ankOutPushInt);
mb_io_set_float(ID_EST_INN_PUSH_ACCUMULATED, WalkFsm_ankInnPushInt);
}
