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);
}
Exemplo n.º 2
0
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();
	}
}