void StateEstimatorKinematic::makeMeasurement(double left_fz, double right_fz)
{
static double fz_hist[2] = {750, 750};
left_fz = low_pass_filter(fz_hist[0], left_fz, 0.99);
right_fz = low_pass_filter(fz_hist[1], right_fz, 0.99);
//_beta = left_fz / (left_fz + right_fz);
if (!(_previous_contact_state == _contact_state_full))
{
_previous_contact_state = _contact_state_full;
contact_change_flag = true;
// set_hack_footpos();
}
if (sqrt(left_fz*left_fz+right_fz*right_fz) < 50) // Total z force small
{
contact_change_flag = true; // Just update foot position until the robot drops from the air
// set_hack_footpos();
}
/*
if (_toe_flag > _previous_toe_flag)// toe off
{
contact_change_flag = true;
set_hack_footpos();
}
*/
_y = _beta*foot_registered.block(0,0,6,1) + (1.0 - _beta)*foot_registered.block(6,0,6,1);
// Set measured velocity to zero
_y.block(3,0,3,1).setZero();
}
QVector<int> USBManager::getDataVector()
{
QVector<int> rResult;
if (mDeviceState)
{
const int n = 54;
char *lBuffer = new char[n];
usb_control_msg(mDeviceHandle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN, 0x33, 0, 0, lBuffer, n, 100);
// qDebug() << QString().fromLatin1(lBuffer, n);
QStringList lMessage = QString(lBuffer).split("\n");
QString lBarometerInfo = QString(lMessage.at(0)).remove("P = ");
QString lTemperatureInfo = QString(lMessage.at(1)).remove("t = ");
QString lHigh = QString(lMessage.at(2)).remove("h = ");
// qDebug() << lBarometerInfo;
// qDebug() << lTemperatureInfo;
// qDebug() << lHigh;
// qDebug() << lMessage;
for (int i = 1; i < SAMPLE_COUNT; i++)
{
mSampleArrayGyroX[i] = mSampleArrayGyroX[i-1];
mSampleArrayGyroY[i] = mSampleArrayGyroY[i-1];
mSampleArrayGyroZ[i] = mSampleArrayGyroZ[i-1];
}
mSampleArrayGyroX[0] = (int) ((char) lBuffer[35] & 0xFF) | ((char)(lBuffer[36] & 0xFF) << 8);
mSampleArrayGyroY[0] = (int) ((char) lBuffer[37] & 0xFF) | ((char)(lBuffer[38] & 0xFF) << 8);
mSampleArrayGyroZ[0] = (int) ((char) lBuffer[39] & 0xFF) | ((char)(lBuffer[40] & 0xFF) << 8);
// qDebug() << mSampleArrayGyroX[0];
if (mFilterType == FilterParams::LPF)
{
low_pass_filter(mSampleArrayGyroX, SAPMLE_RATE, CUT_OFF_LOW, mGyroXX, mGyroXY);
low_pass_filter(mSampleArrayGyroY, SAPMLE_RATE, CUT_OFF_LOW, mGyroYX, mGyroYY);
low_pass_filter(mSampleArrayGyroZ, SAPMLE_RATE, CUT_OFF_LOW, mGyroZX, mGyroZY);
}
else if (mFilterType == FilterParams::BPF)
{
band_pass_filter(mSampleArrayGyroX, SAPMLE_RATE, CUT_OFF_HIGH, CUT_OFF_LOW, mGyroXX, mGyroXY);
band_pass_filter(mSampleArrayGyroY, SAPMLE_RATE, CUT_OFF_HIGH, CUT_OFF_LOW, mGyroYX, mGyroYY);
band_pass_filter(mSampleArrayGyroZ, SAPMLE_RATE, CUT_OFF_HIGH, CUT_OFF_LOW, mGyroZX, mGyroZY);
}
rResult.append(mSampleArrayGyroX[0]);
rResult.append(mSampleArrayGyroY[0]);
rResult.append(mSampleArrayGyroZ[0]);
rResult.append(lBuffer[41] | (lBuffer[42] << 8));
rResult.append(lBuffer[43] | (lBuffer[44] << 8));
rResult.append(lBuffer[45] | (lBuffer[46] << 8));
rResult.append(lBuffer[47] | (lBuffer[48] << 8));
rResult.append(lBuffer[49] | (lBuffer[50] << 8));
rResult.append(lBuffer[51] | (lBuffer[52] << 8));
delete [] lBuffer;
}
return rResult;
}
void encoder_thread(void)
{
bcm2835_gpio_fsel(ENC_PIN, BCM2835_GPIO_FSEL_OUTP);
bcm2835_gpio_fsel(RST_COUNT, BCM2835_GPIO_FSEL_OUTP); // reset count
bcm2835_gpio_write(RST_COUNT, LOW);
// setting modes of counter pins
bcm2835_gpio_fsel(OE_COUNT, BCM2835_GPIO_FSEL_OUTP);
bcm2835_gpio_set_pud(OE_COUNT, BCM2835_GPIO_PUD_UP); //pull-up for eoutput enable
bcm2835_gpio_fsel(SEL1, BCM2835_GPIO_FSEL_OUTP);
bcm2835_gpio_fsel(SEL2, BCM2835_GPIO_FSEL_OUTP);
bcm2835_gpio_fsel(D0, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D1, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D2, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D3, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D4, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D5, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D6, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_fsel(D7, BCM2835_GPIO_FSEL_INPT);
bcm2835_gpio_set_pud(D0, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D1, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D2, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D3, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D4, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D5, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D6, BCM2835_GPIO_PUD_DOWN);
bcm2835_gpio_set_pud(D7, BCM2835_GPIO_PUD_DOWN);
printf("Encoder Thread started.\n");
while(start == 0)
{}
bcm2835_gpio_write(RST_COUNT, HIGH); // now start counting
while(1)
{
if(sample_encoder)
{
//bcm2835_gpio_write(ENC_PIN, HIGH); // for the oscilloscope
// code for obtaining value from encoder IC
x = calculate_encoder(); // no. of rotations
// x = (x / 4.81) * 0.002; // distance traveled by slider in metres
// code for calculating xf and dx
// printf("enc_th: freq are : %f, %f\n", freq_diff, freq_filt);
discrete_diff(); // updating value of dx by calling practical differentiator
low_pass_filter(); // update xf, the filtered value of x
// printf("enc_th: DX in encoder_thread = %f\n", dx);
// printf("enc_th: xf in encoder_thread = %f\n", xf);
// bcm2835_gpio_write(ENC_PIN, LOW);
// reset time flag
sample_encoder = 0; //reset sampling flag
//set encoder flag high
encoder_flag = 1; // means encoder calculations done
}
}
}
