scar.c

基于Robo-PICA robot kit的PIC 16F877a的单片机编程 Innovative Experiment Co.,Ltd.

				break;
			case 'N':
				direction = 'S';
				break;
		}
		count = 0;
		return ARRIVE;
	}

	strcpy( buf, "stop\0" );
	if( strncmp( dir, buf, 4) == 0 )
	{
		return STOP;
	}
}

signed int
parse_cmd( char *input ){
	char    *t;

	strcpy( buf, "|SERVER" );
	t = strstr( input, buf );
	*t = '\0';
	input += 11;

	strcpy( buf, "|\0" );
	t = strtok( input, buf );  //client_id

	if( strcmp(t, Car_Id) )
		return -2;

	t = strtok( 0, buf );   //ur_path
	strcpy( ur_path, t );

	t = strtok( 0, buf );//dir
	return get_cmd( t );

}


signed int 
parse_input(char *input){
	char *input_cpy;
	input_cpy = input;

	strcpy( buf, "SERVER" );
	if( strncmp( input_cpy, buf, 6 ) != 0 ){
		return -1;
	}

	input_cpy += 7;

	strcpy( buf, "CMD" );
	if( strncmp( input_cpy, buf, 3 ) == 0 ){
		return parse_cmd( input );
	}

	return 0;
} 

	signed int
recv(char *buf)
{
	int16 timeout=0;
	i = 0;
	do
	{       
		while(!kbhit() && (++timeout<5000))   // 1/2 second
		{
			delay_us(100);
		}
		if(kbhit())
		{buf[i] = getc();
			i++;
		}
		else
		{
			output_high(LED0);
			return -1;
		}
	}while(buf[i-1] != ' ');

	buf[i-1] = '\0';
	return 0;
}



int do_cmd( int16 cmd )
{
	switch(cmd)
	{
		case TURN_LEFT: 
			TurnAngle(LEFT,80);
			brake();
			delay_ms(100);
			break;
		case TURN_RIGHT:
			TurnAngle(RIGHT,75);
			brake();
			delay_ms(100);
			break;
		case GO:
			Gostraight();
			delay_ms(200);
			break;
		case REVERSE:
			ReverseCar();
			brake();
			delay_ms(200);
			break;
		case STOP:
			brake();
			while(1);
			break;
		case ARRIVE:
			GoStraight();
			delay_ms(380);
			brake();
			ReverseCar();
			//BackBySensor();
			brake();
			//	while(1);
			delay_ms(300);
			starting = destination;
			return 1;
		default:        
			output_high(LED0);
			break;
	}
	return 0;
}

inline int16 read_sensor()
{
	int16 value=0x00;  
	int16 value0,value1,value2,value3;	
	value0 = read_IrSensor(0);
	value1 = read_IrSensor(1);
	value2 = read_IrSensor(2);
	value3 = read_IrSensor(3);
	value = ((value0<200 ? 0x08:0) | value);   //sensor0   1   2    3
	value = ((value1<200 ? 0x04:0) | value);   //      白  黑  黑   白 
	value = ((value2<200 ? 0x02:0) | value);   //      0   1   1    0
	value = ((value3<200 ? 0x01:0) | value);   //

	return value;
}

/*
   typedef struct _urgency_path
   {
   char path[MAX_CROSS];
   } urgency_path;
   */
/*char ur_path[7][6]=
  {
  "ES#\0",
  "S#",
  "ESS#",
  "SS#",
  "WSS#",
  "SESS#",
  "SWSS#"
  };*/
/*=
  {
  {
  {"E","S","arrive",0}
  },
  {
  {"S","arrive",0}
  },
  {
  {"E","S","S","arrive",0}
  },
  {
  {"S","S","arrive",0}
  },
  {
  {"W","S","S","arrive",0}
  },
  {
  {"S","E","S","S","arrive",0}
  },
  {	if( -1 == recv(buf) )
  urgency();
  {"S","W","S","S","arrive",0}
  },
  0   
  };*/
	void
cross_handle()
{
	Gostraight();
	delay_ms(70);
	brake();
	IrSensorValue=read_sensor();              

	if(IrSensorValue == 0x07)  
	{
		TurnAngle(RIGHT,80);
		brake();
		switch( direction )
		{
			case 'E':
				direction = 'S';
				break;
			case 'S':
				direction = 'W';
				break;
			case 'W':
				direction = 'N';
				break;
			case 'N':
				direction = 'E';
				break;
		}
		delay_ms(50);
	}
	if(IrSensorValue == 0x0e)
	{
		TurnAngle(LEFT,80);
		switch( direction )
		{
			case 'E':
				direction = 'N';
				break;
			case 'S':
				direction = 'E';
				break;
			case 'W':
				direction = 'S';
				break;
			case 'N':
				direction = 'W';
				break;
		}
		brake();
		delay_ms(50);
	}
}
	void 
urgency()
{       
	char str[2];
	delay_ms(300);
	LCD_write_string( 3, 1, ur_path );
	count = 0;
	sprintf(str,"%c\0",ur_path[count++]);
	LCD_write_string(0,1,str);
	cmd = get_cmd( str );
	do_cmd( cmd );
	while(true)
	{      
		IrSensorValue = read_sensor();
		if( IrSensorValue == 0x06 )	       //0000 0110
		{
			GoStraight();
		} 
		if( IrSensorValue == 0x02 || IrSensorValue == 0x01 || IrSensorValue == 0x03 )          //0000 0010
		{
			TurnRight();
		}
		if (IrSensorValue == 0x04 || IrSensorValue == 0x08 || IrSensorValue == 0x0c )  	        //0000 0100
		{
			TurnLeft();
		}
		if( IrSensorValue == 0x0f)         //0000 1111
		{     
			   delay_ms(500);	
			sprintf(str,"%c\0",ur_path[count++]);
			cmd = get_cmd( str );
			do_cmd( cmd ); 
		    if(cmd == ARRIVE) while(1);
		}
		if(IrSensorValue == 0x07 || IrSensorValue == 0x0e)
		{
			cross_handle();
		}

	}
}



/*************************************************************************
 *  main funtion
 * ***********************************************************************/
int main()
{
	char str[BUF_SIZE];
	int16  in;

	SET_TRIS_D( 0x00 );
	LCD_init();        //LCD init
	starting = 'A';

	strcpy(str, " GoTo: #");
	LCD_write_string( 0, 0, str );

	destination='A';                       //destination目的的 A B C D E

          /*   while(1)
	     {IrSensorValue = read_sensor();
                LCD_set_xy(0,0);
	        LCD_write_char(1,IrSensorValue+30);  
	         delay_ms(200);
	     }
*/


SET_DEST:
	in = 1;
	while(in)
	{
		if( !INPUT(PIN_E1)) destination++;
		if(destination=='F') destination='A';
		LCD_set_xy(6,0);
		LCD_write_char(1,destination);
		delay_ms(200);
		if( !INPUT(PIN_E2)) in=0;           //in==0  jump out loop
	} 

	//for(i=0;i<3;i++)
	//{printf("scar\n");delay_ms(300);}         //检测无线发射正常



	//destination = 'B';
	count = 0;
	printf("CLIENT|ASK_DIR|%s|%c|%c|%ld|%c|CLIENT",Car_Id,starting,destination,count,direction);
	count++;
	if( -1 == recv(buf) )
		urgency();

	strcpy( str, buf );
	cmd = parse_cmd( str );

	LCD_set_xy(0,0);
	LCD_write_char(1,cmd+48);

	if( 1 == do_cmd( cmd ) )
		goto SET_DEST;

	while(true)
	{      

		IrSensorValue = read_sensor();


		if( IrSensorValue == 0x06 )	       //0000 0110
		{
			GoStraight();
		} 
		if( IrSensorValue == 0x02 || IrSensorValue == 0x01 || IrSensorValue == 0x03 )          //0000 0010
		{
			TurnRight();
		}
		if (IrSensorValue == 0x04 || IrSensorValue == 0x08 || IrSensorValue == 0x0c )  	        //0000 0100
		{
			TurnLeft();
		}
		if( IrSensorValue == 0x0f)         //0000 1111
		{       brake();
			printf("CLIENT|ASK_DIR|%s|%c|%c|%ld|%c|CLIENT\n",Car_Id,starting,destination,count,direction);
			count = count+1;

			if( -1 == recv(buf) )
				urgency();

			strcpy(str,buf);
			//LCD_write_string(0,1,str);

			//output_high(LED0);
			cmd = parse_input( str ); 

			LCD_set_xy(0,0);
			LCD_write_char(1,cmd+48);

			delay_ms(800);

			if( 1 == do_cmd( cmd ) )
				goto SET_DEST;


		}
		if(IrSensorValue == 0x07 || IrSensorValue == 0x0e)
		{
			cross_handle();
		}
	}	

	return(1);
}