#pragma config(StandardModel, "RVW Anemobot")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
Project Title: Ruines of atlantis
Name: Mikko Taitague
Date:


Task Description:


Pseudocode:


*/
void forward(float distance, int speed);
void backward(float distance, int speed);
void turn(float angle, int speed);
void gotowall(float distance, int speed);




task main()
{                       //Program begins, insert code within curly braces}
	backward(0.56,45);
	forward(1.2,70);
	turn(90,35);
	forward(0.33,45);
	backward(0.33,45);
	turn(-90,35);
	forward(0.38,70);
	turn(-90,35);
	gotowall(0.30,80);
	turn(117,40);
	forward(2.23,90);
	turn(55,50);
	backward(0.51,45);
	forward(1.63,80);
	turn(-90,55);
	forward(1.09,75);
	turn(90,55);
	forward(0.95,80);
	turn(90,65);
	forward(0.45,60);
	backward(0.45,60);
	turn(-90,60);
	forward(0.7,75);
	turn(90,75);
	forward(1.75,90);



}
void forward(float distance, int speed)
{
	SensorValue[leftEncoder]=0;
	while(SensorValue[leftEncoder]<distance*1000)
	{
		motor[leftMotor] = speed;
		motor[rightMotor] = speed;
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;

}
void backward(float distance, int speed)
{
	SensorValue[leftEncoder]=0;
	while(SensorValue[leftEncoder]>-distance*1000)
	{
		motor[leftMotor] = -speed;
		motor[rightMotor] = -speed;
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;

}
void turn(float angle, int speed)
{
	float target = SensorValue[gyro] + angle*10;
	if(angle>0)//turn right gyro increases
	{
		while(SensorValue[gyro]<target)
		{
			motor[leftMotor] = speed;
			motor[rightMotor] = -speed;
		}
	}else  //turn left gyro decreases
	{
			while(SensorValue[gyro]>target)
		{
			motor[leftMotor] = -speed;
			motor[rightMotor] = speed;
		}
	}	motor[leftMotor] = 0;
		motor[rightMotor] = 0;
}
void gotowall(float distance, int speed)
{
		while(SensorValue[sonarSensor]>distance*100)
	{
		motor[leftMotor] = speed;
		motor[rightMotor] = speed;
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
}