#include <joyos.h>
#include "driving.h"
#include "turning.h"
#include "sensing.h"
#include "collecting.h"

uint8_t team_number[2] = {11,0};

int usetup () {
	printf_P (PSTR("\nPlace robot,    press go."));
	go_click ();
	printf_P (PSTR("\nStabilizing..."));
	pause (500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
	irdist_set_calibration(13000,6);
	raise_arms();
	return 0;
}

int umain (void){
	
	pause(1000);
				
	close_basket();
	
		orient_gameboard();
	
	turn_out_of_Starting_Square();
	
		drive_straight(BACKWARD,2000,DEADRECKONING,260);
	
	drive_straight(FORWARD,4000,FORWARD,300);
	
		turn_degrees_on_axis(Right,85.0);
	
	drive_straight(BACKWARD,2000,DEADRECKONING,260);
	
		lower_arms();
	
	raise_balls_and_flag();
	
		drive_straight(FORWARD,10000,ACROSS,705);
	
	raise_arms();
	
		turn_degrees_off_axis(Right,BACKWARD,84.0);
	
	raise_arms();
	
		drive_straight(FORWARD,4000,GOAL,340);                      // FIX ME
	
	drive_straight(FORWARD,1300,GOAL,70);
	
		turn_degrees_off_axis(Left,BACKWARD,73.0);
	
	drive_straight(FORWARD,1600,GOAL,200);
	
		deposit_balls();
	
	turn_degrees_off_axis(Right,BACKWARD,84.0);
	
		turn_degrees_off_axis(Right,FORWARD,172.0);
	
	stop_balls_and_flag();
	
	drive_straight(BACKWARD,1300,FLAGFORWARD,200);
	
	drive_straight(FORWARD,9000,FLAGFORWARD,825);   // FIX ME
	
		turn_degrees_on_axis(Left,84.0);
	
	drive_straight(FORWARD,4000,FLAGACROSS,220); // FIX ME
	
		turn_degrees_on_axis(Left,86.0);
	
	drive_straight(FORWARD,2000,FLAGBOX, 260);   // FIX ME
	
		drive_straight(BACKWARD,500,DEADRECKONING, 200);
	
	turn_degrees_on_axis(Left,180.0);
	
		drive_straight(BACKWARD,2000,FLAGREVERSE, 150);
	
	raise_balls_and_flag();

	pause(3000);
	
	uint16_t counter = 1;
	while(1){
		
		stop_balls_and_flag();
		drive_straight(FORWARD,800,FLAGREPEAT,100);
		drive_straight(BACKWARD,800,FLAGREPEAT,90);
		raise_balls_and_flag();
		pause(counter*2500);
		counter++;
	}
return 0;
}