void updateAccelerometer(float * pitch, float * roll) {
float Accelerometer_Data [3];
readAccelerometer(Accelerometer_Data);
for(int i = 0; i < 3; i++)
moving_filter(&buffer_list[i], &Accelerometer_Data[i]);
calculate_Pitch(Accelerometer_Data, pitch);
calculate_Roll(Accelerometer_Data, roll);
moving_filter(&buffer_list[3], pitch);
moving_filter(&buffer_list[4], roll);
}
void moving_state() {
printf("\nMoving state");
left_encoder_base = encoder_read(LEFT_ENCODER);
right_encoder_base = encoder_read(RIGHT_ENCODER);
while(state == MOVING)
{
float input = gyro_get_degrees();
float output = update_pid_input(&controller, input);
motor_set_vel(RIGHT_MOTOR, FORWARD_SPEED + (int)output + OFFSET_ESTIMATE);
motor_set_vel(LEFT_MOTOR, FORWARD_SPEED - (int)output - OFFSET_ESTIMATE);
pause(50);
moving_filter();
}
}