/* All constants */

/* !! Persistent Global variables !! */
persistent int servoFWD	= 2432;
persistent int servoBK	= 2481;
persistent int frontrightIRthresh = 100;
persistent int backleftIRthresh = 100;
persistent int backrightIRthresh = 100;


/* Initialization Routines */
#define frontrightIRport 18
#define backleftIRport 20
#define backrightIRport 22

/* MAIN */
#define true 1
#define false 0
#define leftmotorA 0
#define leftmotorB 4
#define rightmotorA 1
#define rightmotorB 5
#define intakemotorA 2
#define intakemotorB 3
#define servoport 2
#define batt_port 6


/* Main Globals */
int teamisblack		= true; 
int rf_our_team		= 0;
int initial_rf_vote_winner = 0; /* in case of tie */
int our_color		= 0;
int initial_orient	= 0;		/* can be n, s, e, w (0,1,2,3) */
int using_RF		= true;	/* change to compete! */

/* servo */
int servoTURN			= 720;
int servo_delta			= 15;   /* amount to change servo for steering */
int max_delta_servo		= 250; /* max amount servo can be changed */
int servo_learning_rate = 0;
int last_was_straight	= false;

/*-- Motor Variables -- (global) */
int Mot_StopMot			= true;
int Mot_READY			= true;
int Mot_Desired_Speed	= 100;
int MotL_clicks_to_go	= 0;
int MotR_clicks_to_go	= 0;
int MotL_direction		= 1;  /* +1 or -1 */
int MotR_direction		= 1;  /* +1 or -1 */
float pi				= 3.14159;
float Mot_clicks_to_go	= 0.0;
float se_cps			= 0.0;

/* Strategy */ 
int start_x = 0;
int start_y = 0;
int goal_x	= 0;
int goal_y	= 0;
int coord_factor = 1;	/* 1 for white, -1 if black*/
float heading_const = 0.0;	/* 0 for black, 180 for white*/
float unitsperinch	= 100.0/12.0;
float clicksperinch = 1.23*100.0/12.0;
float clicksperfoot = 123.0;
float clicksperunit = (clicksperfoot*(1.0/100.0));

/* McGruff Globals */
int op_x;
int op_y;
int my_x;
int my_y;
int MG_1_visited		= false;
int MG_2_visited		= false;
int MG_3_visited		= false;
int MG_4_visited		= false;
int MG_5_visited		= false;
int MG_6_visited		= false;
int MG_1_present		= false;
int MG_2_present		= false;
int MG_3_present		= false;
int MG_4_present		= false;
int MG_5_present		= false;
int MG_6_present		= false;
int MG_nearREDvote		= false;
int MG_nearGREENvote	= false;
int MG_my_last_x		= 0;
int MG_my_last_y		= 0;
int MG_op_last_x		= 0;
int MG_op_last_y		= 0;
float MG_my_rf_heading	= 0.0;
float MG_op_rf_heading	= 0.0;
float MG_my_rf_speed	= 0.0;
float MG_op_rf_speed	= 0.0;
float MG_last_rf_update = 0.0;
int MG_Collision_Avoid	= false;
int MG_StopMG			= true;
int MG_op_near			= false;
float se_last_sec		= 0.0;
float se_last_last_sec	= 0.0;
int AL_StopAL			= true;

 /* Gyro */
int gyro_type	= 300;			/* 150 for ADXRS150, 300 for ADXRS300 */
float scale_factor = 1.0145;	/* adjust this for your particular gyro! */
float lsb_ms_per_deg = 256.0*scale_factor;      /* nominal scale factor depending on gyro type */
float theta;					/* current angle, units explained in update_angle() */
float offset;					/* gyro offset */
int gyro_port	= 3;			/* gyro analog input port number */
int num_avgs	= 2000;			/* number of averages for calibration */
long time_ms;					/* system time the last gyro reading was taken */
float angle		= 0.0;			/* angle in degrees */