/*
 * State constants
 */
#define NORTH			1
#define EAST			2
#define SOUTH			3
#define WEST			4

#define WHITE			1
#define BLACK			2

/*
 * Actuators
 */
#define LEFT_MOTOR		0
#define RIGHT_MOTOR		1
#define THUMPER_MOTOR		3
#define RAMP_MOTOR		4
#define GATE_MOTOR		5

#define LEFT_SERVO		0
#define RIGHT_SERVO		1
#define SORTER_SERVO		2

/*
 * Actuator constants
 */
#define LEFT_SERVO_CENTER	1490
#define LEFT_SERVO_45		2340
#define LEFT_SERVO_90		3560
#define RIGHT_SERVO_CENTER	2420
#define RIGHT_SERVO_45		1540
#define RIGHT_SERVO_90		320
#define SORTER_CENTER		1840
#define SORTER_LEFT		3220
#define SORTER_RIGHT		400

#define LEFT_TURN_COUNT		230
#define LEFT_BACKUP_COUNT	270
#define RIGHT_TURN_COUNT	230
#define RIGHT_BACKUP_COUNT	270

/*
 * Sensors
 */
#define RAMP_BUTTON		3
#define LEFT_DISTANCE		4
#define RIGHT_DISTANCE		5
#define LEFT_MOTOR_ENCODER	7
#define RIGHT_MOTOR_ENCODER	8
#define LEFT_FRONT_LIGHT	16
#define RIGHT_FRONT_LIGHT	17
#define LEFT_REAR_LIGHT		18
#define RIGHT_REAR_LIGHT	19
#define LEFT_FRONT_BUMPER	20
#define RIGHT_FRONT_BUMPER	21
#define LEFT_REAR_BUMPER	22
#define RIGHT_REAR_BUMPER	23
#define LEFT_FRONT_SIDE		24
#define RIGHT_FRONT_SIDE	25
#define LEFT_REAR_SIDE		26
#define RIGHT_REAR_SIDE		27
#define BALL_LIGHT		29
#define RAMP_TOP_SENSOR		30
#define START_SENSOR		31

/*
 * Block positions
 */
#define UNKNOWN 0
#define SOUTH_EAST 1
#define SOUTH_WEST 2
#define NORTH_EAST 3
#define NORTH_WEST 4

/*
 * State variables
 */
int orientation;
int team;

int THRESHOLD_1;
int THRESHOLD_2;
int THRESHOLD_3;
int THRESHOLD_4;
int THRESHOLD_BALL;

int white_balls;
int black_balls;

int white_accounted;
int black_accounted;

int ramp_up;

int block_spot;

int TIMEOUT_FLAG;
int TIMEOUT_PID;
int GRABBING_BALL;