/*
 *   Motor Check Program 2 (LegOS)
 *
 *   Mortor => A , Rotation Sensor => 1
 */

#include <conio.h>
#include <dmotor.h>
#include <unistd.h>
#include <dsensor.h>
#include <time.h>

int main(void) 
{
	int r[10];
	int record = 0;
	int max = 0;
	int min = 0;
	int time1 = 0;
	int time2 = 0;
	int i = 0, j = 0;

	ds_active(&SENSOR_1);
	motor_a_speed(MAX_SPEED);

	lcd_int(0);

	for(i=0;i<5;i++)
	{
		// forward drive test

		ds_rotation_set(&SENSOR_1,0);
		ds_rotation_on(&SENSOR_1);
		time1 = sys_time;
		time2 = 0;

		while (time2 < 10000){
			motor_a_dir(fwd);
			time2 = sys_time;
			time2 -= time1;
		}
		motor_a_dir(brake);
		r[i] = ROTATION_1;
		lcd_int(r[i]);
		ds_rotation_off(&SENSOR_1);
		msleep(500);

		// reverse drive test

		ds_rotation_set(&SENSOR_1,0);
		ds_rotation_on(&SENSOR_1);
		time1 = sys_time;
		time2 = 0;
		j = i + 5;

		while (time2 < 10000){
			motor_a_dir(rev);
			time2 = sys_time;
			time2 -= time1;
		}
		motor_a_dir(brake);
		r[j] = -(ROTATION_1);
		lcd_int(r[j]);
		ds_rotation_off(&SENSOR_1);
		msleep(500);

	}

	record = 0;
	max = 0;
	min = 9999;

	for(i=0;i<10;i++)
	{
		record += r[i];
		if (max<r[i]) max=r[i];
		if (min>r[i]) min=r[i];
	}
	record -= max;
	record -= min;
	record /= 8;
	lcd_int(record);

	return 0;
}