#include "conio.h"
#include "dmotor.h"
#include "dsensor.h"

const int SPEED = 255;
const float ROT_DIVISOR = 3.75;
const int DIST_MULTIPLE = 3;

void rt(int deg)
{
	ds_rotation_set(&SENSOR_1, 0);
	ds_rotation_set(&SENSOR_2, 0);
	ds_rotation_set(&SENSOR_3, 0);

	motor_a_dir(rev);
	motor_b_dir(rev);
	motor_c_dir(rev);
	motor_a_speed(SPEED);
	motor_b_speed(SPEED);
	motor_c_speed(SPEED);

	while(ROTATION_2 > (-deg / ROT_DIVISOR));

	motor_a_dir(brake);
	motor_b_dir(brake);
	motor_c_dir(brake);
	msleep(10);
	motor_a_dir(off);
	motor_b_dir(off);
	motor_c_dir(off);
}

void lt(int deg)
{
	ds_rotation_set(&SENSOR_1, 0);
	ds_rotation_set(&SENSOR_2, 0);
	ds_rotation_set(&SENSOR_3, 0);

	motor_a_dir(fwd);
	motor_b_dir(fwd);
	motor_c_dir(fwd);
	motor_a_speed(SPEED);
	motor_b_speed(SPEED);
	motor_c_speed(SPEED);

	while(ROTATION_2 < (deg / ROT_DIVISOR));

	motor_a_dir(brake);
	motor_b_dir(brake);
	motor_c_dir(brake);
	msleep(10);
	motor_a_dir(off);
	motor_b_dir(off);
	motor_c_dir(off);
}

void fd(int dist)
{
	ds_rotation_set(&SENSOR_1, 0);
	ds_rotation_set(&SENSOR_2, 0);
	ds_rotation_set(&SENSOR_3, 0);

	motor_a_dir(brake);
	motor_b_dir(rev);
	motor_c_dir(fwd);
	motor_b_speed(SPEED);
	motor_c_speed(SPEED);

	while(ROTATION_3 < (dist * DIST_MULTIPLE));

	motor_b_dir(brake);
	motor_c_dir(brake);
	msleep(10);
	motor_a_dir(off);
	motor_b_dir(off);
	motor_c_dir(off);
}

void bk(int dist)
{
	ds_rotation_set(&SENSOR_1, 0);
	ds_rotation_set(&SENSOR_2, 0);
	ds_rotation_set(&SENSOR_3, 0);

	motor_a_dir(brake);
	motor_b_dir(fwd);
	motor_c_dir(rev);
	motor_b_speed(SPEED);
	motor_c_speed(SPEED);

	while(ROTATION_2 < (dist * DIST_MULTIPLE));

	motor_b_dir(brake);
	motor_c_dir(brake);
	msleep(10);
	motor_a_dir(off);
	motor_b_dir(off);
	motor_c_dir(off);
}

int main(int argc, char** argv)
{
	ds_active(&SENSOR_1);
	ds_active(&SENSOR_2);
	ds_active(&SENSOR_3);
	ds_rotation_on(&SENSOR_1);
	ds_rotation_on(&SENSOR_2);
	ds_rotation_on(&SENSOR_3);

        // GENERATED CODE GOES HERE

	return 0;
}