#include<iostream>
#include<string>
#include<cmath>
//~ #include"eyebot++.h"
#include"/home/pi/eyebot/include/eyebot++.h"
using namespace std;

int main()
{
	
	LCDMenu("","","","END");
	int num;
	//~ int pwm1, pwm2;
	do
	{
		cin>>num;
		if (num == 1)
		{
			ENCODERReset(1);
			ENCODERReset(2);
			do 
			{
				MOTORDriveRaw(1,21);
				MOTORDriveRaw(2,-19);
			} while (abs(ENCODERRead(2)) < 217);
			MOTORDriveRaw(1,0);
			MOTORDriveRaw(2,0);
		}
		
		else if (num == 2)
		{
			ENCODERReset(1);
			ENCODERReset(2);
			do 
			{
				MOTORDriveRaw(1,21);
				MOTORDriveRaw(2,-19);
			} while (abs(ENCODERRead(2)) < 500);
			MOTORDriveRaw(1,0);
			MOTORDriveRaw(2,0);
		}
		
		else if (num == 3)
		{
			ENCODERReset(1);
			ENCODERReset(2);
			do 
			{
				MOTORDriveRaw(1,-21);
				MOTORDriveRaw(2,19);
			} while (abs(ENCODERRead(2)) < 242);
			MOTORDriveRaw(1,0);
			MOTORDriveRaw(2,0);
		}
		
		else if (num == 4)
		{
			ENCODERReset(1);
			ENCODERReset(2);
			do 
			{
				MOTORDriveRaw(1,-21);
			} while (abs(ENCODERRead(1)) < 285);
			MOTORDriveRaw(1,0);
			do 
			{
				MOTORDriveRaw(2,19);
			} while (abs(ENCODERRead(2)) < 285);
			MOTORDriveRaw(2,0);
		}
		
		else 
		{
			cout << " num error " << endl;
		}
		
	} while (KEYRead() != KEY4);
	
}