Will Backhair

// Created on Mon April 6 2015
//Blessed by the KaiserBeckerChan~ Himself!!!

//Touch Sensors
#define button 15
//Motor
#define RIGHTWHEEL 2
#define LEFTWHEEL 0
//Servo
#define SORTER 3
//Channel
#define RED 0
#define GREEN 1

const int SORTER_NEUTRAL_SERVO = 850;
const int SORTER_RED_SERVO = 1550;
const int SORTER_GREEN_SERVO = 230;
const int left_speed = 100;
const int right_speed = 100;
const int left_rotation_speed = 800;
const int right_rotation_speed = 200;

void setup();
void movearound(int speed, int millis);
void stop();
void servo_sort();
void leaf_green();
void fire_red();
void green_sort(int pos);
void red_sort(int pos);
void rotation(int left_rotation_speed, int right_rotation_speed);
void camera_four();
void button_change(int speed);
void drive_direct(int left_speed, int right_speed);
void movement();

int main()
{
	printf("WundurBar Herr KaiserBeckerChan~ ~Please Bless Us~\n");
	setup();
	thread life;
	life = thread_create(movement);
	thread_start(life);
	for(;;){
		movearound(100, 3000);
	}
	for(;;){
		printf("Test2\n");
		servo_sort();
	}

	return 0;
}

void setup()
{
	printf("Time to have fun\n");
	enable_servo(SORTER);
	printf("Test3\n");
	clear_motor_position_counter(RIGHTWHEEL);
	clear_motor_position_counter(LEFTWHEEL);
	camera_open_at_res(LOW_RES);
	printf("Test4\n");
}

void servo_sort(){
	set_servo_position(SORTER, SORTER_NEUTRAL_SERVO);
	int counter = 0;
	for(;;)
	{

		if(counter == 3){
				//tap_funnel();
				//counter = 0;
		}
		camera_four();
		printf("Test15\n");
		if(get_object_area(RED,0) > get_object_area(GREEN,0) && get_object_area(RED,0) > 100){
			red_sort(SORTER_RED_SERVO);
			msleep(250);
		}

		if(get_object_area(GREEN,0) > get_object_area(RED,0) && get_object_area(GREEN,0) > 100){
			green_sort(SORTER_GREEN_SERVO);
			msleep(250);
		}

		if(get_object_area(RED,0) > 100 && get_object_area(GREEN,0) >1000){
			//clear_sort_v1();
			//msleep(250);
		}

		if(get_object_area(RED,0) && (get_object_area(GREEN,0) == 0)){

			//tap_funnel();
		}
			msleep(10);
	}
}

void red_sort(int pos)
{
	set_servo_position(3, pos);
	msleep(250);
	set_servo_position(3, SORTER_NEUTRAL_SERVO);
}

void green_sort(int pos)
{
	set_servo_position(3, pos);
	msleep(250);
	set_servo_position(3, SORTER_NEUTRAL_SERVO);
}

void camera_four()
{
	 camera_update();
     camera_update();
     camera_update();
     camera_update();
}

void stop(){
	drive_direct(0,0);
}

void drive_direct(int left_speed, int right_speed)
	{
			//stablizing the movement of the robot(potenally fixed w/ software)
            motor(RIGHTWHEEL, right_speed);
            motor(LEFTWHEEL, left_speed);
    }

void movearound(int speed, int millis){
	//driving
	printf("Test5\n");
	drive_direct(speed, speed);
	printf("Test6\n");
	msleep(millis);
	printf("Test7\n");
	rotation(left_rotation_speed, right_rotation_speed);
	printf("Test8\n");
	msleep(millis);
}

void rotation(int left_rotation_speed, int right_rotation_speed){
	printf("Test9\n");
	drive_direct(left_rotation_speed, right_rotation_speed);
	printf("Test10\n");
	msleep(100);
	}

void movement(){
	movearound(100, 3000);
}