示例#1
0
/* 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;

}
示例#2
0
/* 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);
}
示例#3
0
文件: unit_test.c 项目: mws262/Ranger
/* --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));
}
示例#4
0
文件: unit_test.c 项目: mws262/Ranger
/* --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);
}
示例#5
0
文件: unit_test.c 项目: mws262/Ranger
/* --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);
 }
示例#6
0
文件: unit_test.c 项目: mws262/Ranger
/* --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();
}
示例#7
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);
}
示例#8
0
/* 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);
}
示例#9
0
/* 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;

}
示例#10
0
/* 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;
}
示例#11
0
文件: unit_test.c 项目: mws262/Ranger
/* --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);
}
示例#12
0
/* 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);

}
示例#13
0
/* 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);


}
示例#14
0
文件: unit_test.c 项目: mws262/Ranger
/* --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));
}
示例#15
0
// 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());
}
示例#16
0
void set_io_float(short unsigned int data_id, float value)
{
  mb_io_set_float(data_id, value);
}
示例#17
0
/* 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);
}