/* --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));
}
/* --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();
}
/* --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();
}
int CVICALLBACK Aquire_Display (int panel, int control, int event,
void *callbackData, int eventData1, int eventData2)
{
double phase = 0.0;
double datapoints[256];
int i = 0;
int index = 0;
switch (event)
{
case EVENT_COMMIT:
GetCtrlIndex (MYPANEL, MYPANEL_MYRING, &index);
switch (index)
{
case 0:
SineWave (256, 1.0, 7.8125e-3, &phase, datapoints);
break;
case 1:
SquareWave (256, 1.0, 7.8125e-3, &phase, 50.0, datapoints);
break;
case 2:
TriangleWave (256, 1.0, 7.8125e-3, &phase, datapoints);
break;
case 3:
for (i = 0; i < 256; i++)
datapoints[i] = rand() /32767.0;
break;
}
DeleteGraphPlot (MYPANEL, MYPANEL_MYGRAPH, -1, VAL_IMMEDIATE_DRAW);
PlotY (MYPANEL, MYPANEL_MYGRAPH, datapoints, 256, VAL_DOUBLE,
VAL_THIN_LINE, VAL_EMPTY_SQUARE, VAL_SOLID, 1, VAL_RED);
break;
}
return 0;
}
/* --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);
}
