APU::APU(float sampleRate)
: time(0), sampleRate(sampleRate), timeStep(1.0/sampleRate),
frameCounterMode(0),
pulses(std::vector<PulseWave>(2, PulseWave(sampleRate))),
triangle(TriangleWave(sampleRate))
{
}
/* --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));
}
static int32_t ChannelAmplitude(BitBinChannel *channel)
{
if(channel->currentvolume==0) return 0;
if(channel->mastervolume==0) return 0;
int32_t sample=0;
switch(channel->waveform)
{
case 0: sample=SquareWave1_8(channel); break;
case 1: sample=SquareWave1_4(channel); break;
case 2: sample=SquareWave1_2(channel); break;
case 3: sample=SquareWave3_4(channel); break;
case 4: sample=TriangleWave(channel); break;
case 5: sample=Noise(channel); break;
}
return sample*channel->currentvolume*channel->mastervolume;
}
/* --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;
}
