/* This function accumulates the total energy used by each board */
void updateEnergyUsage(void) {
static float t = 0; // time at last data point
static float pHip = 0; // power used by the hip
static float p0 = 0; // power used by the outer feet
static float p1 = 0; // power used by the inner feet
static float pCpu = 0; // power used by the overheads
static float eHip = 0; // energy used by the hip
static float e0 = 0; // energy used by the outer feet
static float e1 = 0; // energy used by the inner feet
static float eCpu = 0; // energy used by the overheads
float dt = 0; // time step between current and previous data
// Approximate integral by trapezoid method
dt = STATE_t - t;
eHip = eHip + 0.5 * dt * (pHip + POWER_MCH);
e0 = e0 + 0.5 * dt * (p0 + POWER_MCFO);
e1 = e1 + 0.5 * dt * (p1 + POWER_MCFI);
eCpu = eCpu + 0.5 * dt * (pCpu + POWER_OVERHEAD);
// Send estimates:
mb_io_set_float(ID_EST_ENERGY_MCH, eHip);
mb_io_set_float(ID_EST_ENERGY_MCFO, e0);
mb_io_set_float(ID_EST_ENERGY_MCFI, e1);
mb_io_set_float(ID_EST_ENERGY_OVERHEAD, eCpu);
// Update static power variables:
t = STATE_t;
pHip = POWER_MCH;
p0 = POWER_MCFO;
p1 = POWER_MCFI;
pCpu = POWER_OVERHEAD;
}
/* Update the robot orientation by integral of the rate gyro */
void updateRobotOrientation(void) {
STATE_th0_gyro = STATE_th0_gyro + getIntegralRateGyro();
mb_io_set_float(ID_EST_STATE_TH0_GYRO, STATE_th0_gyro);
// For now, just set the robot state to be the imu internal sensor fusion:
STATE_th0 = 1.0 * STATE_th0_imu + 0.0 * STATE_th0_gyro; ////HACK//// Only use the IMU's internal estimate for now.
mb_io_set_float(ID_EST_STATE_TH0, STATE_th0);
}
/* --1-- Check that the wave functions in Ranger math are performing correctly.
* Creates a sine wave, triangle wave, and a square wave, and then sends
* them over the CAN bus to LabVIEW, on channels:
* ID_CTRL_TEST_W0, *_W1, *_W2 */
void test_waveFunctions() {
float period = 2.0;
float min = -0.5;
float max = 2.2;
float time = getTime();
mb_io_set_float(ID_CTRL_TEST_W0, SquareWave(time, period, min, max));
mb_io_set_float(ID_CTRL_TEST_W1, TriangleWave(time, period, min, max));
mb_io_set_float(ID_CTRL_TEST_W2, SineWave(time, period, min, max));
mb_io_set_float(ID_CTRL_TEST_W3, SawToothWave(time, period, min, max));
}
/* --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);
}
/* --21-- Tests the variable-grid linear interpolation on Ranger by
* sending a periodic waveform to be plotted in labview. W0 plots a
* saw-tooth wave and w1 plots the test function. The result should be
* a sine wave */
void test_linInterpVar(void){
static float T[] = {0.0, 0.1, 0.3, 0.4, 0.45, 0.5, 0.9, 0.98, 1.0}; // Time vector
static float Y[] = {0.0, 0.59, 0.95, 0.59, 0.31, 0.0, -0.59, -0.13, 0.0}; // Function
static int nGrid = 9; // <-- BE VERY CAREFUL WITH THIS - don't want to get a segfault...
float period = 2.0;
float min = 0.0;
float max = 1.0;
float time = getTime();
float timeWrap = SawToothWave(time, period, min, max);
float func = LinInterpVar(timeWrap,T,Y,nGrid);
mb_io_set_float(ID_CTRL_TEST_W0, timeWrap);
mb_io_set_float(ID_CTRL_TEST_W1, func);
}
/* --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();
}
/* 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);
}
/* Computes the horizontal component of the robot's position and velocity,
and then updates the parameters in both labview and the estimator. */
void updateComKinematics(void) {
float pos, vel;
switch (STATE_contactMode) {
case CONTACT_S0:
pos = getComPos(STATE_th0, STATE_th1);
vel = getComVel(STATE_th0, STATE_th1, STATE_dth0, STATE_dth1);
break;
case CONTACT_S1:
pos = getComPos(STATE_th1, STATE_th0);
vel = getComVel(STATE_th1, STATE_th0, STATE_dth1, STATE_dth0);
break;
default:
pos = 0.0;
vel = 0.0;
break;
}
// Send updates to the estimator and also to LabVIEW
STATE_posCom = pos;
STATE_velCom = vel;
mb_io_set_float(ID_EST_STATE_POSCOM, pos);
mb_io_set_float(ID_EST_STATE_VELCOM, vel);
}
/* Runs the butterworth filters for the robot */
void runAllFilters(void) {
float y; // Store the estimate
// outer leg absolute orientation
y = runFilter(&FC_FAST, &FD_OUTER_LEG_ANGLE);
y = y - GYRO_ROLL_BIAS; // correct for bias term in the gyro
mb_io_set_float(ID_EST_STATE_TH0_IMU, y);
STATE_th0_imu = y; // Send robot orientation to the control and estimation code
// Outer leg absolute orientation rate
y = runFilter(&FC_FAST, &FD_UI_ANG_RATE_X);
y = y - GYRO_RATE_BIAS; // correct for gyro bias
mb_io_set_float(ID_EST_STATE_DTH0, y);
STATE_dth0 = y; // Send robot orientation rate to the control and estimation code
// Hip Angular Rate:
y = runFilter(&FC_FAST, &FD_MCH_ANG_RATE);
mb_io_set_float(ID_E_MCH_ANG_RATE, y);
STATE_dqh = y;
// Outer Ankle Rate:
y = runFilter(&FC_FAST, &FD_MCFO_RIGHT_ANKLE_RATE);
mb_io_set_float(ID_E_MCFO_RIGHT_ANKLE_RATE, y);
STATE_dq0 = y;
// Inner Ankle Rate:
y = runFilter(&FC_FAST, &FD_MCFI_ANKLE_RATE);
mb_io_set_float(ID_E_MCFI_ANKLE_RATE, y);
STATE_dq1 = y;
// Outer feet contact filter:
y = runFilter(&FC_SLOW, &FD_MCFO_RIGHT_HEEL_SENSE);
y = y + runFilter(&FC_SLOW, &FD_MCFO_LEFT_HEEL_SENSE);
mb_io_set_float(ID_EST_CONTACT_OUTER, y);
STATE_c0 = y > CONTACT_VALUE_THRESHOLD;
// Inner feet contact filter
y = runFilter(&FC_SLOW, &FD_MCFI_RIGHT_HEEL_SENSE);
y = y + runFilter(&FC_SLOW, &FD_MCFI_LEFT_HEEL_SENSE);
mb_io_set_float(ID_EST_CONTACT_INNER, y);
STATE_c1 = y > CONTACT_VALUE_THRESHOLD;
// Steering angle
y = runFilter(&FC_VERY_SLOW, &FD_MCSI_STEER_ANGLE);
mb_io_set_float(ID_EST_STATE_PSI, y);
STATE_psi = y;
}
/* 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;
}
/* --5-- Robot is standing in single stance on the outer feet.
* The inner legs should track a slow sine curve, with
* minimal stead-state error thanks to gravity and spring
* compensation. The target hip angle is on ID_CTRL_TEST_W0. */
void test_hipCompensation_outer() {
float kp_hip = 14.0;
float kd_hip = 2.0;
float kp_ank = 7.0;
float kd_ank = 1.0;
float min = -0.2;
float max = 0.2;
float period = 5.0;
float qTarget;
float time = getTime();
qTarget = SineWave(time, period, min, max);
trackAbs_ankOut(0.0, kp_ank, kd_ank);
trackRel_ankInn(0.2, kp_ank, kd_ank);
trackRel_hip(qTarget, kp_hip, kd_hip);
mb_io_set_float(ID_CTRL_TEST_W0, qTarget);
}
/* 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);
}
/* 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);
}
/* --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));
}
// Wrapper function to call the execution time function
void mb_update_execution_time(void)
{
mb_io_set_float(ID_MB_EXECUTION_TIME, mb_clock_get_execution_time());
}
void set_io_float(short unsigned int data_id, float value)
{
mb_io_set_float(data_id, value);
}
/* 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);
}
