#include "FRC_Comp_Include.c"

void Initialization()
{
	bMotorReflected[port2] = true;  //Reflect Motor
	frcDigitalIODirection[pio3] = dirInput;  //IR Output 1 on Digital IO 3
	frcDigitalIODirection[pio4] = dirInput;  //IR Output 2 on Digital IO 4
	frcDigitalIODirection[pio5] = dirInput;  //IR Output 3 on Digital IO 5
	frcDigitalIODirection[pio6] = dirInput;  //IR Output 4 on Digital IO 6
}

task Autonomous()
{
	int IRswitch1;
	int IRswitch2;
	int IRswitch3;
	int IRswitch4;

	while(true)
	{
	   IRswitch1 = frcDigitalIOValue[pio3];  //Update Variables
	   IRswitch2 = frcDigitalIOValue[pio4];  //So we can see status in the Debugger
	   IRswitch3 = frcDigitalIOValue[pio5];
	   IRswitch4 = frcDigitalIOValue[pio6];

	   if(IRswitch1 == 1)  //If Switch 1 is pressed (it pulses, doesn't remain on)
	   {
	     motor[port1] = 50;
	     motor[port2] = 50;
	   }

	   if(IRswitch2 == 1)  //If Switch 2 is pressed
	   {
	     motor[port1] = 0;
	     motor[port2] = 0;
	   }

	   if(IRswitch3 == 1)  //If Switch 3 is pressed
	   {
	     motor[port1] = -50;
	     motor[port2] = 50;
	   }

	   if(IRswitch4 == 1)  //If Switch 4 is pressed
	   {
	     motor[port1] = 50;
	     motor[port2] = -50;
	   }
	}
}

task Human_Control()
{
	while(true)
	{
		motor[port1] = frcRF[p1_y];
		motor[port2] = frcRF[p2_y];
	}
}