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();
}
