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