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 } } }