#include 
#include 
#include "function.h"
#include "pwm_enc.h"

int main()
{
//	int theta = 360; 
//	int left_cm, right_cm;
	int br=0,k=0,i=0,inl,inr,disl,disr;
	float alw,arw;

	FILE *fp;
	printf("input left_duty\n");
	scanf("%d",&inl);
	printf("input right_duty\n");
	scanf("%d",&inr);
        printf("input left_distance\n");
        scanf("%d",&disl);
        printf("input right_distance\n");
        scanf("%d",&disr);
//	left_cm = theta/180. *pi *diameter / 2.;
//	right_cm = left_cm;

	pwm_enco_stop();

//	pwm_cm_data(left_cm,right_cm);
//	pwm_circle_data(theta);
	pwm_both_data(inl,inr);

	pwm_enco_start(10000);

	//usleep(5000);

	if((fp=fopen("data.dat","w"))==NULL){
		printf("File not open\n");
		exit(1);
	}
	while(i==0){
                usleep(20000);
                pwm_both_data(inl,inr);
                usleep(20000);
                encoder_data2(&alw,&arw,&br);
                printf("%f %f %d \n",alw,arw,br);
		if(disl < 0){
			if(alw < disl){i=1;}}
		if(disl >= 0){
			if(alw > disl){i=1;}}
                if(disr < 0){
                        if(arw < disr){i=1;}}
                if(disr >= 0){
                        if(arw > disr){i=1;}}
		k=k+1;
		if(k==500){i=1;}
	}	
	fclose(fp);
	pwm_both_data(-1,-1 );
	usleep(1000000);
	pwm_enco_stop();
	printf("Program stopped\n");
	return 0;
}