// Include headers from OS

#include <joyos.h>
#include <math.h>




#include <happylib.h>

#define SHARP_M		21598
#define SHARP_C		13


#define ENCODER_TO_WHEEL_RATIO 25
#define WHEEL_CIRCUMFERENCE 25.53721
#define WHEEL_TRACK 17.0
#include <lib/geartrain.h>


#include "driveforward.h"
# define RIGHTMOTOR 1
# define LEFTMOTOR  2
# define FLAGMOTOR 3
# define vel 90
# define Darkregion  500
#  define range



#define RightEncoder 26
#define LeftEncoder 25

#define GYRO_PORT 		11
#define LSB_US_PER_DEG	1420000


// usetup is called during the calibration period. It must return before the
// period ends.

uint8_t team_number[2] = {21,0};



int usetup (void) {
	irdist_set_calibration (SHARP_M, SHARP_C);
	set_auto_halt(0);
	printf_P (PSTR("\nStabilizing..."));
	pause(500);
	gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
	return 0;
}

/*void driveforward()
{
	motor_set_vel(RIGHTMOTOR, -vel);
	motor_set_vel(LEFTMOTOR, vel);
}
*/
/*void driveback()
{
	motor_set_vel(RIGHTMOTOR, -vel);
	motor_set_vel(LEFTMOTOR, vel);
	
}*/



// this one is the original

/*
uint16_t orientation()
{   int result=0;
	int d=0; 
	while(d!=1)
	{
	
	uint16_t value1=analog_read ( 23 );
	
	uint16_t value3=analog_read ( 18 );
	uint16_t value4=analog_read ( 20 );
	printf("%5d%5d%5d \n", value1,value3, value4);
	pause(1000);
	
	//on the white board
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	pause(200);
	
	if (value4>Darkregion && value3<Darkregion && value1<Darkregion)
		{
			//A
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()< gvalue + 148.5 )
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		result= 0;
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1<Darkregion) 
		{  //D
		    float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue + 52)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		result =3;
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1>Darkregion) 
		{  //C
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue- 24)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		result= 2;
		d=1;	
		}
	else if(value4<Darkregion && value3>Darkregion && value1<Darkregion) 
		{  //B
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-110)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		
        stop();
        result= 1;	
		d=1;
		}
		
		
		
		
		////// now on the blackboard
	else if(value4>Darkregion && value3>Darkregion && value1<Darkregion)
		{   //C
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-24)
			{
				turnRight();
			}
		
		stop();
		
        result= 2;	
		d=1;
		}
	else if(value4>Darkregion && value3<Darkregion && value1>Darkregion)
		{  //B
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-110)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		result= 1;
		d=1;
		}
	else if(value4<Darkregion && value3>Darkregion && value1>Darkregion)
		{  //A
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue+148.5)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();	
		
		result= 0;
		d=1;
		}
	else if(value4>Darkregion && value3>Darkregion && value1>Darkregion)
		{    //D
		    float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue + 53)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
        result= 3;
        d=1; 	
		}
		
	
	
	
	}
	
	return result;
}


*/

void orientation()
{   int result=0;
	int d=0; 
	while(d!=1)
	{
	
	uint16_t value1=analog_read ( 23 );
	
	uint16_t value3=analog_read ( 18 );
	uint16_t value4=analog_read ( 20 );
	printf("%5d%5d%5d \n", value1,value3, value4);
	pause(1000);
	
	//on the white board
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	pause(200);
	
	if (value4>Darkregion && value3<Darkregion && value1<Darkregion)
		{
			//A
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()< gvalue + 148.5 )
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1<Darkregion) 
		{  //D
		    float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue + 52)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1>Darkregion) 
		{  //C
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue- 24)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		d=1;	
		}
	else if(value4<Darkregion && value3>Darkregion && value1<Darkregion) 
		{  //B
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-110)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		
        stop();
        
		d=1;
		}
		
		
		
		
		////// now on the blackboard
	else if(value4>Darkregion && value3>Darkregion && value1<Darkregion)
		{   //C
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-24)
			{
				turnRight();
			}
		
		stop();
		
        
		d=1;
		}
	else if(value4>Darkregion && value3<Darkregion && value1>Darkregion)
		{  //B
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-110)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		
		d=1;
		}
	else if(value4<Darkregion && value3>Darkregion && value1>Darkregion)
		{  //A
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue+148.5)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();	
		
		
		d=1;
		}
	else if(value4>Darkregion && value3>Darkregion && value1>Darkregion)
		{    //D
		    float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue + 53)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
        
        d=1; 	
		}
		
	
	
	
	}
	
	
}




void testGeneral(int samples)
{
	uint16_t i = 0;
	float dists[samples];
	
	while(1)
	{
    
		int sum=0;
		float ave=0;
		dists[i%samples] = irdist_read (12)/2.54;
	
		if (i >= samples)
		{
	
	
			//summing over the array
			for (uint16_t j = 0; j < samples; j++)
			{
				sum = sum + dists[j];
				//printf("%f\n", sum); 
			}
	
				ave = sum/(float) samples;
				//printf("%f\n",ave);

			if ( ave>29.0 && ave< 35.0)
			{
				stop();					
				servo_set_pos ( 5,411 );
				break;				
			}
	
		}
		
		i++;
	  
	}
}

void testRight()
{	
	
	turnRight();
	testGeneral(1);
	stop();
	
	driveforward(193);
}


void testLeft()
{	
	
	turnLeft();
	testGeneral(5);	
	stop();
	
	driveforward(193);
}





void testRight1()
{	
	float d1=0;
	int s=0;
	
	turnRight();
	while(s!=1)
	{

	
	int sum=0;
	float ave=0;
	float dist=0;
	float dists[5];
	
	
	uint16_t i = 0;
	
	dists[i%5] = irdist_read (12)/2.54;
	i++;
	
	
	if (i >= 5)
	{
	
	for (uint16_t j = 0; j < 5; j++)
	{
		sum = sum + dists[j];
		
	}
		ave = sum/5.0;
	   

	  	
	
	  if ( ave>25.0 &&ave< 36.0)

		{
			stop();
						
			servo_set_pos ( 5,411 );
			d1=ave;
			s=1;
			
		
		}
	  
	  }
	  
	}
	
	stop();
	
	driveforward(130);
	stop();
	pause(1000);
	servo_set_pos ( 5,411 );
	
	

}

void testLeft1()
{	
	float d1=0;
	int s=0;
	
	turnLeft();
	while(s!=1)
	{

	
	int sum=0;
	float ave=0;
	float dist=0;
	
	
	
	   for (uint16_t i = 0; i < 10; i++)
		{	
			
			dist = irdist_read (12)/2.54;
	
			sum=sum+dist;
		
			pause(3);
		
		}
		
		
	
	  ave=sum/10.0;
	  	  
	
	  if ( ave>28.0 &&ave< 37.0)

		{
			stop();
						
			servo_set_pos ( 5,411 );
			d1=ave;
			s=1;
			
		
		}
	  
	  
	}
	
	stop();
	
	

}


/*
void orientation()
{
	int d=0; 
	while(d!=1)
	{
	
	uint16_t value1=analog_read ( 23 );
	
	uint16_t value3=analog_read ( 18 );
	uint16_t value4=analog_read ( 20 );
	
	printf("%5d%5d%5d \n", value1,value3, value4);
	pause(500);
	//on the white board
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	pause(500);
	
	if (value4>Darkregion && value3<Darkregion && value1<Darkregion)
		{
			//A
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()< gvalue + 90.0 )
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		
		testLeft();
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1<Darkregion) 
		{  //D
		    testLeft();
			d=1;
		}
	else if(value4<Darkregion && value3<Darkregion && value1>Darkregion) 
		{  //C
			testRight();
		    d=1;	
		}
	else if(value4<Darkregion && value3>Darkregion && value1<Darkregion) 
		{  //B
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-80)
			{
				turnRight();
			}
		
		
        stop();	
		
		testRight();
		d=1;
		}
		
		
		
		
		////// now on the blackboard
	else if(value4>Darkregion && value3>Darkregion && value1<Darkregion)
		{   //A
			testRight();
		d=1;	
		}
	else if(value4>Darkregion && value3<Darkregion && value1>Darkregion)
		{  //D
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()>gvalue-90)
			{
				turnRight();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		testRight();
		d=1;
		}
	else if(value4<Darkregion && value3>Darkregion && value1>Darkregion)
		{  //C
			float gvalue = gyro_get_degrees();
			while(gyro_get_degrees()<gvalue+90)
			{
				turnLeft();
			}
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		testLeft();
		d=1;
		}
	else if(value4>Darkregion && value3>Darkregion && value1>Darkregion)
		{  {  //B
		    testLeft();
		d=1;	
		}
		}
	
	
	
	}
	
	stop();
}

	
*/

void liftdown()
{
	servo_set_pos( 3,511 );
}



void clawopen()
{
	servo_set_pos ( 5,330 );
}


void clawclose()
{
	servo_set_pos ( 5,0 );
}

void liftup()
{
	servo_set_pos ( 3,0 );
}

void stop()
{
	motor_brake(RIGHTMOTOR);
	motor_brake(LEFTMOTOR);
	pause(100);
}

void turnLeft()
{
	motor_set_vel(RIGHTMOTOR, -95);
	motor_set_vel(LEFTMOTOR, -95);
//	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
}



void turnRight()
{
	motor_set_vel(RIGHTMOTOR, 95);
	motor_set_vel(LEFTMOTOR, 95);
//	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());	
}









//moves forward and catches a ball. play with distance and gap to make it work.

 void movetoCatch1(uint16_t distance)
 {
 clawopen();
 ResetEncoders();
 driveforward(distance);
 
 stop();
 
 pause(400);
 clawclose();
 pause(400);
 }

void movetoCatch2(uint16_t distance)
 {
 ResetEncoders();
 driveforward(distance);
 stop();
 pause(600);
 clawopen();
 pause(1000);
 driveforward(15);
 clawclose();
 stop();
 
 
 }

void turnRighttoAngle(float angle)
{   ResetEncoders();
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	float newvalue = gyro_get_degrees();
	float newvalue1 = newvalue - angle;
	printf_P (PSTR("\ntheta = %.2f"), newvalue1);
	go_click();
	turnRight();
	while(1)
	{
	pause(3);
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	}
	
	stop();
}





void movetodeposit(uint16_t distance)
{	
	liftup();
	pause(3000);
	ResetEncoders();
	driveforward(distance);
	clawopen();
	stop();
	
	
}


uint16_t implement(uint16_t dist1,uint16_t dist2,uint16_t dist3,uint16_t ang1,uint16_t ang2)
{
    movetoCatch1(dist1);
	turnRighttoAngle(ang1);
	movetoCatch2(dist2);
	turnRighttoAngle(ang2);
	movetodeposit(dist3);                                             
	
}




// Entry point to contestant code.
// Create threads and return 0.
int umain (void) {
	
	/*int a;
	a=orientation();
	
	if (a==0)
	{
	implement(90,87,300,30,60);
	}
	else if (a==1)
	{
	implement(100,120,220,100,40);
	}
	
	else if (a==2)
	{
	implement(138,90,300,30,60);
	}
	else  
	{
	implement(135,60,350,20,60);
	}
	
	
	
	*/
	
/*	
	stop();
	driveback(400);
	
	stop();
	
	
	
	
	turnRighttoAngle(90.0);
	stop();
	
	*/
	
	
	
	
	orientation();
	clawopen();
	
	driveforward(155);
	clawclose();
	pause(10);
	stop();
	pause(400);
	
	
	
	pause(100);
	
	
	
	
	
	
	
	float gvalue = gyro_get_degrees();
	while(gyro_get_degrees()>gvalue-45)
	
	turnRight();
	
	{
		pause(10);
	}
	
	
	stop();
	pause(100);
	liftup();
	pause(2000);
	
	
	
	
	driveforward(555);
	clawopen();
	pause(3);
	
	stop();
	
	driveback(50);
	stop();
	pause(500);
	
	
	
	
	float value4 = gyro_get_degrees();
	while(gyro_get_degrees()<value4+135)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(200);
	
	
	
	
	
	driveback1();
	if (digital_read(0) && digital_read(1))
	{
		stop();
	}
	else if (digital_read(0) | digital_read(1))
	{
		driveback1();
	
	}
	
	pause(500);
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	
	driveforward(350);
	stop();
	pause(100);
	
	
	//turn toward the next ball
	
	float value2 = gyro_get_degrees();
	while(gyro_get_degrees()<value2+90)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(200);
	
	
	
	
	
	driveback1();
	if (digital_read(0) && digital_read(1))
	{
		stop();
	}
	else if (digital_read(0) | digital_read(1))
	{
		driveback1();
	
	}
	
	pause(500);
	liftdown();
	pause(2000);
	
	clawopen();
	pause(100);
	
	
	driveforward(32);
	clawclose();
	pause(10);
	stop();
	pause(30);
	
	
	
	
	liftup();
	pause(2000);
	
	
	
	
	//turn back
	
	float value5 = gyro_get_degrees();
	while(gyro_get_degrees()<value5+90)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(200);
	
	driveforward(55);
	
	stop();
	
	
	
	
	
	
	float value8 = gyro_get_degrees();
	while(gyro_get_degrees()<value8+45)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(100);
	 
	
	driveforward(100);
	clawopen();
	stop();
	pause(10);
	
	
	
	//now to enemy goal
	
	driveback(100);
	float value12 = gyro_get_degrees();
	while(gyro_get_degrees()<value12+135)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(100);
	
	
	
	
	driveback1();
	if (digital_read(0) && digital_read(1))
	{
		stop();
	}
	else if (digital_read(0) | digital_read(1))
	{
		driveback1();
	
	}
	
	
	driveforward(970);
	
	float value11 = gyro_get_degrees();
	while(gyro_get_degrees()<value11+90)
	
	turnLeft();
	
	{
		pause(10);
	}
	
	stop();
	pause(100);
	
	
	driveback1();
	if (digital_read(0) && digital_read(1))
	{
		stop();
	}
	else if (digital_read(0) | digital_read(1))
	{
		driveback1();
	
	}
	
	
	
	driveforward(750);
	
	stop();
	pause(10);
	
	
	
	
	
	///////End//////////// 
	
	// turn towards goal
	/*ResetEncoders();
	
	
	
	ResetEncoders();
	float newvalue = gyro_get_degrees();
	float newvalue1 = newvalue - 67;
	turnRight();
	while(gyro_get_degrees()>newvalue1)
	{
	pause(3);
	}
	
	liftup();
	(2000);
	stop();
	pause(1000);
	
	
	
	ResetEncoders();
	driveforward(450);
	stop();
	clawopen();
	pause(100);
	
	
	
	*/
	
	
	
	
	/////////////////////////////////////////////////
	/*orientation();
	stop();
	pause(10);
	servo_set_pos ( 5,24 );
	pause(1000);
	
	
	float newvalue = gyro_get_degrees();
	float newvalue1 = newvalue - 10;
	turnRight();
	while(gyro_get_degrees()>newvalue1)
	{
	pause(3);
	}
	stop();
	pause(40);
	

	testRight();
	
	stop();
	
	//servo_set_pos ( 5,24 );
	//pause(10);
	
	*/
	


	//create_thread(&stopthemotor, STACK_DEFAULT, 0, "stop");
	//create_thread(&bumpsensormonitor, STACK_DEFAULT, 0, "bumpthread");
	
	
	
	
	
	
	
	
	
	
	
	
	
// code for the black line following
	
	// left movement of gyroscope would increase the angle
	// right movement decreases the angle...possibly negative

	/*uint16_t value1=analog_read ( 23 );
	//uint16_t value2=analog_read ( 21 );
	uint16_t value3=analog_read ( 22 );
	uint16_t value4=analog_read ( 20 );
	
	printf("%5d%5d%5d \n", value1, value2, value3, value4);
	
	uint8_t a = (value1>Darkregion), b = (value2>Darkregion), c = (value3>Darkregion);
	
	
	

    
	


		if(!a && b && !c )
			{
				driveforward();
			}

		else if( !a && !b && c )
			{
				turnLeft();
				
			}
		else if (!a && b && c)
			{   uint16_t value = gyro_get_degrees();
				uint16_t value1=88+value;
				turnLeft1();
				if (!a && !b && !c)
				{
					
					while(gyro_get_degrees()<value1)
					{
						turnLeft();
				
					}
				
				}
			}
		
		


		else if(!c && ! b && a)
			{   uint16_t value = gyro_get_degrees();
				uint16_t value1=88+value;
				turnRight1();
				if (!a && !b && !c)
				{
					
					while(gyro_get_degrees()<value1)
					{
						turnRight();
				
					}
				
				}
			}
			
		else if(!c && b && a)
			{

				turnRight();
				
				
			}
			
			
		else if(a && c && !b)
		{
			driveforward();
		}

		
			
		pause(20);
		
}
*/




// program for the orientation


/*	int d=0;
	while(d!=1)
	{
	
	uint16_t value1=analog_read ( 23 );
	//uint16_t value2=analog_read ( 21 );
	uint16_t value3=analog_read ( 22 );
	uint16_t value4=analog_read ( 20 );
	printf("%5d%5d%5d \n", value1,value3, value4);
	
	//on the white board
	
	
	if (value4>Darkregion && value3<Darkregion && value1<Darkregion)
		{
			//A
			float gvalue = gyro_get_degrees();
			float gvalue1 = 120+gvalue;
			while(gyro_get_degrees()<gvalue1)
			{
				turnLeft();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1<Darkregion)
		{  //D
		    float gvalue = gyro_get_degrees();
			float gvalue1 = 35+gvalue;
			while(gyro_get_degrees()<gvalue1)
			{
				turnLeft();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		d=1;	
		}
	else if(value4<Darkregion && value3<Darkregion && value1>Darkregion)
		{  //C
			float gvalue = gyro_get_degrees();
			float gvalue1 = gvalue - 20;
			while(gyro_get_degrees()>gvalue1)
			{
				turnRight();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		d=1;	
		}
	else if(value4<Darkregion && value3>Darkregion && value1<Darkregion)
		{  //B
			float gvalue = gyro_get_degrees();
			float gvalue1 = gvalue-105;
			while(gyro_get_degrees()>gvalue1)
			{
				turnRight();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		d=1;
        stop();	
		}
		
		// now on the blackboard
	else if(value4>Darkregion && value3>Darkregion && value1<Darkregion)
		{   //A
			float gvalue = gyro_get_degrees();
			float gvalue1 = gvalue-25;
			while(gyro_get_degrees()>gvalue1)
			{
				turnRight();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		d=1;	
		}
	else if(value4>Darkregion && value3<Darkregion && value1>Darkregion)
		{  //D
			float gvalue = gyro_get_degrees();
			float gvalue1 = gvalue- 105;
			while(gyro_get_degrees()>gvalue1)
			{
				turnRight();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();
		d=1;
		}
	else if(value4<Darkregion && value3>Darkregion && value1>Darkregion)
		{  //C
			float gvalue = gyro_get_degrees();
			float gvalue1 = 105+gvalue;
			while(gyro_get_degrees()<gvalue1)
			{
				turnLeft();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		stop();	
		d=1;
		}
	else if(value4>Darkregion && value3>Darkregion && value1>Darkregion)
		{  //B
			float gvalue = gyro_get_degrees();
			float gvalue1 = 20+gvalue;
			while(gyro_get_degrees()>gvalue1)
			{
				turnLeft();
			}
		printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
		d=1;
        stop();	
		}
	
	
	
	}*/
	
	
	



	

    




	
	
	
	/*while(1)
	{
	
	if (digital_read(0) | digital_read(1))
	{
	stop();
	for (uint8_t i = 0; i < 40; i++)
	{
	printf("\nThe value is %d", i);
	driveback();
	
	}
	printf("hello");
	
	for (uint8_t j = 0; j < 12; j++)
	{
	printf("\nvalue of is %d",j);
	turnleft();
	}
	
	}
	else
	{
	driveforward();
	}
	}*/
	

// move in a square
	
	
	/*while(1)
	{

	uint16_t value = analog_read(11);
	printf("\nvalue: %d", value);
	
	pause(500);
	
	}	
	
	

	
	
	

	for (uint8_t j = 0; j < 15; j++)
	{
	printf("\nvalue of is %d",j);
	driveforward();
	
	}
	
	stop();
	
	
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	turnleft();
	uint16_t value = gyro_get_degrees();
	uint16_t value1=88+value;
	
	
	while(gyro_get_degrees()<value1)
	{
	turnleft();
	
	
	}
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	
	
	
	
	
	stop();
	
	
	
	
	
	
	for (uint8_t j = 0; j < 15; j++)
	{
	printf("\nvalue of is %d",j);
	driveforward();
	
	}
	
	stop();
	
	
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	turnleft();
	uint16_t value2 = gyro_get_degrees();
	uint16_t value3=88+value2;
	
	
	while(gyro_get_degrees()<value3)
	{
	turnleft();
	
	
	}
	printf_P (PSTR("\ntheta = %.2f, value3: %d"), gyro_get_degrees(),value3);
	stop();
	
	
	
	
	
	
	
	
	
	
	
	
	for (uint8_t j = 0; j < 15; j++)
	{
	printf("\nvalue of is %d",j);
	driveforward();
	
	}
	
	stop();
	
	
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	turnleft();
	uint16_t value4 = gyro_get_degrees();
	uint16_t value5=88+value4;
	
	
	while(gyro_get_degrees()<value5)
	{
	turnleft();
	
	
	}
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	
	
	
	
	
	stop();
	
	
	
	
	
	
   
   for (uint8_t j = 0; j < 15; j++)
	{
	printf("\nvalue of is %d",j);
	driveforward();
	
	}
	
	stop();
	
	
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	turnleft();
	uint16_t value6 = gyro_get_degrees();
	uint16_t value7=88+value6;
	
	
	while(gyro_get_degrees()<value7)
	{
	turnleft();
	
	
	}
	printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	
	
	
	
	stop();
	*/
	

	
	
	
	
	
	
	


/////////////////////////code for the ball grabbing///////////////////////////


//im here now 
/*
	orientation();

	/// open the gates
	
	
	stop();
	pause(100);
	
	
	ResetEncoders();
	
	pause(10);
	
	
	
	
	
	
	
	
	servo_set_pos ( 5,411 );
	
	
	
	//////////////////////////////////////// now drive forward
	
	ResetEncoders();
	
	driveforward(190);
	
	
	stop();
	
	
	pause(1000);
	
	///////////////////////////////////////// grab the ball
	
	
	servo_set_pos ( 5,0 );
	
	
	
	
	
	
	
	
	
	

	////////////////////////////////////////// rotate towards the next ball
	
	
    float newvalue = gyro_get_degrees();
	float newvalue1 = newvalue - 25;
	turnRight();
	while((gyro_get_degrees()>newvalue1)
	{
	pause(3);
	}
	stop();
	pause(100);
	
	
	
	ResetEncoders();
	
	driveforward(100);
	
	
	stop();
	
	pause(200);
	servo_set_pos(5,511);
	
	pause(10);
	servo_set_pos(5,0);
	
	*/
	
	
	
	
	

//////////////now we need to implement the strategy of turning towards the next ball
	
	/*
	
	double x= analog_read(12) - 8;
	double y=(12/x);
	double anglerad= atan(y);
	double angle=57.2888*anglerad
	
	
	///rotate towards the goal
	
	
	
	
	float newvalue = gyro_get_degrees();
	float newvalue1 = newvalue - angle;
	turnRight();
	while(gyro_get_degrees()>newvalue1)
	{
	pause(3);
	}
	stop();
	pause(100);


		
	
	//////open the gate
	
	
	
	//servo_set_pos ( 5,511 );
	
	
	/////move the robot forward second time
	
	/*ResetEncoders();
	
	pause(10);
	driveforward(400);
	
	stop();
	pause(100);*/
	
	
	////////////////////////////////////////////////////////// close the gate
	
	//servo_set_pos ( 5,0 );
	//pause(30);
	
	/////////////////////////////////////////////////////////// rotate again towards the goal
	
	
	
	
    
	/*
	float nvalue = gyro_get_degrees();
	while(gyro_get_degrees()>nvalue - 26)
	{
		turnRight();
		//printf_P (PSTR("\ntheta = %.2f"), gyro_get_degrees());
	}
	
	stop();*/
	
	////////////////////////////////////////////////////////raise the lift
	
	
	 // 0 is top position for this servo.
	/*servo_set_pos ( 3,0 );
    	
	
	//////////////////////////////////////////////////////drive forward and open gate
	
	ResetEncoders();
	
	
	
	stop();
	
	
	
	
	
	driveforward(600);
	servo_set_pos ( 5,511 );
	driveforward(100);
	
	stop();
	pause(20);
	
	
	//driveback(200);
	//motor_set_vel(FLAGMOTOR, 240);
	
	//////////////////////////////////////////////////////from here next move of the robot to be implemented
	
	
	/*driveback(200);
	stop();
	pause(100);
	
	float dvalue = gyro_get_degrees();
	while(gyro_get_degrees()>dvalue - 70)
	{
		turnRight();
	
	}
	
	stop();
	
	

	motor_set_vel(extramotor, 200);
	*/
	
	
	///////////////*******************************************************////new strategy/////////////////////////
	
	
	/*while(1)
	{
	uint16_t value1 = analog_read (12);
	printf("\n%d",analog_read (12));
	
	pause(100);
	}
	*/
	
	/*uint16_t value=DEG_TO_TICKS_IN_PLACE(90);
	
	encoder_reset ( 26 );
	
	turnRight();
	while (encoder_read (26 ) < value)
	{
		pause(10);
	}
	printf_P (PSTR("\nvalue = %.2f   %2f"), value,encoder_read (26 ));
	
	stop();
	pause(100);
	
	
	
	*/
	/////////////////////////////////////*******************************************implementing the new strategy//////////
	
	
	
	
	//im working here|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

	//orientation();
	//stop();
	//pause(100);
	
	
	//now to spot the ball another 40 degrees
	
	
	//float y = 1.7371*dist - 3.4669;
	//float lowervalue = 30.0;
	//float uppervalue = 34.0;

/*	
	float d1=0;
	int s=0;
	
	t1Right();
	while(s!=1)
	{

	
	int sum=0;
	float ave=0;
	float dist=0;
	
	
	
	   for (uint16_t i = 0; i < 10; i++)
		{	
			
			dist = irdist_read (12)/2.54;
	
			sum=sum+dist;
		
			pause(5);
		
		}
		
		
	
	  ave=sum/10.0;
	  	  
	
	  if ( ave>25.0 &&ave< 35.0)

		{
			stop();
						
			servo_set_pos ( 5,411 );
			d1=ave;
			s=1;
			
		
		}
	  
	  
	}
	
	stop();
	
	
	float circumference = 10.4;
	float v = (d1+3.469)/1.739;
	float rev=(v/circumference)*150;
	printf_P (PSTR("\nrev = %.2f"), rev);
	pause(1000);
	
	//driveforward(rev);

	stop();
	pause(20);
*/

/*	float lowervalue = 30.0;
	float uppervalue = 34.0;
	int sum=0;
	float ave=0;
	float dist=0;
	
	int s=0;
	float dvalue = gyro_get_degrees();
	
	while(gyro_get_degrees()>(dvalue - 35) )
	{
	
	t1Right();
	
	for (uint16_t i = 0; i < 10; i++)
	{
	
	dist = irdist_read (12)/2.54;
	
	sum=sum+dist;
	
	}
	
	
	ave = sum/10.0;
	}
	
	
	
	stop();
	pause(100);
	
	printf("%d",ave);


*/


/*
while(1)
{       float dist=0;
		float sum=0;
        float ave=0;
		//printf_P (PSTR("\ndist = %.2f in\n"), irdist_read (12)/2.54);
    	
		//pause (40);
		dist=irdist_read (12)/2.54;
		
		
	    for (uint16_t i = 0; i < 10; i++)
		{	
			pause(10);	
			dist = irdist_read (12)/2.54;
	
			sum=sum+dist;
		
		
		}
		
		
	
	  ave=sum/10;
	  
	  //printf_P (PSTR("\nave = %.2f"), ave);
	  //pause(100);
	 
	  
	
	  if (ave < 15.0)

		{
			
			servo_set_pos ( 5,511 );
			pause(2);
		
		}
	  

}


*/
	// Will never return, but the compiler complains without a return
	// statement.
	return 0;
}