#pragma config(Sensor, S1,     FrontSensor,    sensorEV3_Color)
#pragma config(Sensor, S2,     Gyroscope,      sensorEV3_Gyro)
#pragma config(Motor,  motorA,          ArmMotor,      tmotorEV3_Medium, PIDControl, encoder)
#pragma config(Motor,  motorB,          LeftMotor,     tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motorD,          RightMotor,    tmotorEV3_Large, PIDControl, driveRight, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void Wander();
void PushEggIntoNest();
long nest = 1;
task main()
{
	//resetGyro(Gyroscope);
	nest = getGyroDegrees(Gyroscope);

	short currentColour;
	Wander();
	//PushEggIntoNest();
}
void PushEggIntoNest() {
	//turn until towards nest
	repeatUntil(fabs( getGyroDegrees(Gyroscope)-nest) <10){
		//turn counter clockwise if needed
		if (nest < getGyroDegrees(Gyroscope)) {
			setMotorSpeed(LeftMotor, 0);
			setMotorSpeed(RightMotor, 30);
		}
		//turn clockwise if needed
		else {
				setMotorSpeed(LeftMotor, 30);
			setMotorSpeed(RightMotor, 0);
		}
		displayTextLine(3, "%d",  getGyroDegrees(Gyroscope));
	}

	displayTextLine(7, "facing nest");
	//look for black tape
	while (SensorValue[FrontSensor] > 20){
			setMotorSpeed(LeftMotor, 20);
			setMotorSpeed(RightMotor, 20);
			displayTextLine(5, "%d", SensorValue[FrontSensor]);
	}
	//found nest/black tape
	//dip in to nest
			displayTextLine(6, "found nest");
			setMotorSpeed(LeftMotor, 50);
			setMotorSpeed(RightMotor, 50);
			sleep(200);
			//back out
			setMotorSpeed(LeftMotor, -30);
			setMotorSpeed(RightMotor, -30);
			sleep(750);
			//stop
			setMotorSpeed(LeftMotor, 0);
			setMotorSpeed(RightMotor, 0);
}
void Wander() {
	while (true)
	{
		setMotorSpeed(LeftMotor, 30);		//Set the leftMotor (motor1) to half power (50)
		setMotorSpeed(RightMotor, 30);  //Set the rightMotor (motor6) to half power (50)
		sleep(2000);
		if (random[2]==1) {
			setMotorSpeed(LeftMotor, 0);			//Set the leftMotor (motor1) to Off
			setMotorSpeed(RightMotor, 100);  	//Set the rightMotor (motor6) to full power forward (100)
	} else {
				setMotorSpeed(LeftMotor, 100);		//Set the leftMotor (motor1) to full power forward (100)
			setMotorSpeed(RightMotor, 0);  		//Set the rightMotor (motor6) to full power reverse (-100)
	}
		sleep(800);
	}
}