void loop() { // read raw accel/gyro measurements from device accelgyro.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]); CalculatedT(); ComplementaryFilter(accData, gyrData, &final_pitch, &final_roll); printf("pitch = %6f roll %6f dt = %lu\n", final_pitch, final_roll, elapsed); }
void start_scheduler(void) { while (1) { //timer interrupt if (sampling_flag == 1) { timer_rst(); if (curr_channel == 1) { // used to indicate scheduler cycle in GPIO pin RA5 update_status(SYSTEM_START); PORTAbits.RA5 = !PORTAbits.RA5; } else if (curr_channel == 100) { update_status(SYSTEM_GYRO); done = gyro_task(); } else if (curr_channel == 200) { update_status(SYSTEM_ACCELERO); done = get_acceleration(); } else if (curr_channel == 300) { update_status(SYSTEM_COMPASS); done = get_compass(); // //done = Compass_test_single(); // } else if (curr_channel == 250) { } else if (curr_channel == 400) { update_status(SYSTEM_FUSION); ComplementaryFilter(); fusion_final(); } else if (curr_channel == 700) { update_status(SYSTEM_UART); //done = uart_task(); } } else { //update status if a task terminate if (done && (check_status() == SYSTEM_UART) && TXSTAbits.TRMT) { update_status(SYSTEM_IDLE); done = 0; } else if (done && ((check_status() == SYSTEM_GYRO) || check_status() == SYSTEM_ACCELERO || check_status() == SYSTEM_COMPASS || check_status() == SYSTEM_FUSION )) { update_status(SYSTEM_IDLE); done = 0; } } } }
virtual void run() { int16_t accData[3]; int16_t gyrData[3]; CTimer tm(TIMER0); tm.second(DT); tm.enable(); myPID.SetMode(AUTOMATIC); myPID.SetOutputLimits(-100.0, 100.0); myPID.SetSampleTime(PID_SAMPLE_RATE); m_roll = 0.0f; m_pitch = 0.0f; double output; #if USE_AUTO_TUNING double sp_input, sp_output, sp_setpoint, lastRoll; PID speedPID(&sp_input, &sp_output, &sp_setpoint, 35, 1, 2, DIRECT); speedPID.SetMode(AUTOMATIC); speedPID.SetOutputLimits(-config.roll_offset, config.roll_offset); speedPID.SetSampleTime(PID_SAMPLE_RATE); sp_input = 0; sp_setpoint = 0; lastRoll = 0; #endif // // loop // while (isAlive()) { // // wait timer interrupt (DT) // if (tm.wait()) { // // read sensors // m_mxMPU.lock(); m_mpu->getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]); m_mxMPU.unlock(); // // filter // ComplementaryFilter(accData, gyrData, &m_pitch, &m_roll); #if USE_AUTO_TUNING sp_input = (lastRoll - m_roll) / PID_SAMPLE_RATE; // roll speed lastRoll = m_roll; speedPID.Compute(); m_setpoint = -sp_output; // tune output angle #else m_roll -= config.roll_offset; #endif m_input = m_roll; myPID.Compute(); if ( m_output > config.skip_interval ) { LEDs[1] = LED_ON; LEDs[2] = LED_OFF; output = map(m_output, config.skip_interval, 100, config.pwm_min, config.pwm_max); m_left->direct(DIR_FORWARD); m_right->direct(DIR_FORWARD); } else if ( m_output<-config.skip_interval ) { LEDs[1] = LED_OFF; LEDs[2] = LED_ON; output = map(m_output, -config.skip_interval, -100, config.pwm_min, config.pwm_max); m_left->direct(DIR_REVERSE); m_right->direct(DIR_REVERSE); } else { LEDs[1] = LED_OFF; LEDs[2] = LED_OFF; output = 0; m_left->direct(DIR_STOP); m_right->direct(DIR_STOP); } // // auto power off when fell. // if ( abs(m_roll)>65 ) { output = 0; } // // output // m_left->dutyCycle(output * config.left_power); m_right->dutyCycle(output * config.right_power); } }
PROCESS_THREAD(acc_process, ev, data) { PROCESS_BEGIN(); // just wait shortly to be sure sensor is available etimer_set(&timer, CLOCK_SECOND * 0.05); PROCESS_YIELD(); // get pointer to sensor static const struct sensors_sensor *acc_sensor; acc_sensor = sensors_find("Acc"); { // activate and check status uint8_t status = SENSORS_ACTIVATE(*acc_sensor); if (status == 0) { printf("Error: Failed to init accelerometer, aborting...\n"); PROCESS_EXIT(); } // configure acc_sensor->configure(ACC_CONF_SENSITIVITY, ACC_2G); acc_sensor->configure(ACC_CONF_DATA_RATE, ACC_100HZ); } static const struct sensors_sensor *gyro_sensor; gyro_sensor = sensors_find("Gyro"); { // get pointer to sensor // activate and check status uint8_t status = SENSORS_ACTIVATE(*gyro_sensor); if (status == 0) { printf("Error: Failed to init gyroscope, aborting...\n"); PROCESS_EXIT(); } // configure gyro_sensor->configure(GYRO_CONF_SENSITIVITY, GYRO_250DPS); gyro_sensor->configure(GYRO_CONF_DATA_RATE, GYRO_100HZ); } SENSORS_ACTIVATE(button_sensor); etimer_set(&timer, CLOCK_SECOND * 0.01); static int on; static short accData[3], gyroData[3]; static double pitch = 0, roll = 0; while (1) { if(ev == sensors_event && data == &button_sensor) { on = !on; } else if(ev == PROCESS_EVENT_TIMER) { accData[0] = acc_sensor->value(ACC_X); accData[1] = acc_sensor->value(ACC_Y); accData[2] = acc_sensor->value(ACC_Z); gyroData[0] = gyro_sensor->value(GYRO_X); gyroData[1] = gyro_sensor->value(GYRO_Y); gyroData[2] = gyro_sensor->value(GYRO_Z); ComplementaryFilter(accData, gyroData, &pitch, &roll); // printf("%d %d\n", (int) (pitch), (int) (accData[1])); // read and output values // printf("%d %d %d %d %d %d %d\n", // acc_sensor->value(ACC_X), // acc_sensor->value(ACC_Y), // acc_sensor->value(ACC_Z), // gyro_sensor->value(GYRO_X), // gyro_sensor->value(GYRO_Y), // gyro_sensor->value(GYRO_Z), // on); etimer_reset(&timer); } PROCESS_YIELD(); // PROCESS_PAUSE(); } // deactivate (never reached here) SENSORS_DEACTIVATE(*acc_sensor); SENSORS_DEACTIVATE(*gyro_sensor); SENSORS_DEACTIVATE(button_sensor); PROCESS_END(); }