示例#1
0
/*
-----------------------------------------------------------------------------
	函数名称:void Steer_control
	函数功能:控制舵机转向
	输入形参:无
   	返回值:无
-----------------------------------------------------------------------------
*/
void  Steer_control(void)
{

  	u8 kp;
  	s16 steererr  = 0;
  	Err=((BlackLineData[11]+BlackLineData[12]+BlackLineData[13]+BlackLineData[14]+BlackLineData[15])/5)-80;
  	kp=8;//比例
     
  	steererr  = kp*Err;
	// 500+ 或- 该向左转的时候 大于500 ,该向右转的时候小于500
  	Steerangle  = STRM-steererr;   		
	LED_1(ON);
	LED_2(ON);
	if(Steerangle>STRR)
	{
		Steerangle=700;	
		LED_1(ON);
		LED_2(OFF);
	}
	else
	if(Steerangle<STRL)
	{
		Steerangle=330;//1 ms高电平
		LED_1(OFF);
		LED_2(ON);
		
	}
	Change_Direction(Steerangle); 
}
void set_LED (unsigned char led, unsigned char on)
{
	volatile unsigned char	*latch;
	unsigned char		mask;
	switch(led) {
	case 1:
		latch = &LED_1(LAT);
		mask = LED_1_MASK;
		break;
	case 2:
		latch = &LED_2(LAT);
		mask = LED_2_MASK;
		break;
	case 3:
		latch = &LED_3(LAT);
		mask = LED_3_MASK;
		break;
	case 4:
		latch = &LED_4(LAT);
		mask = LED_4_MASK;
		break;
	default:
		return;
	}
	if (on)
		*latch |= mask;
	else
		*latch &= ~mask;

	eventlog_track(EVENTLOG_LED + (led-1), on);
}
示例#3
0
void main(void)
{
	int cnt = 0;
	unsigned char cmd_send1[] = {0x40,0x00};
	unsigned char cmd_send2[] = {0x00};
	volatile 	int send_f = 0;
	volatile	int	s_f =0 ;
	volatile	int 	sci_f = 0;
	volatile 	int f = 0;
	char moji[50] = {0}; 	
	char moji_null[] = {0};
	uint16_t count[] = {0};
	uint16_t count_null[] = {0};
	unsigned char format = 0;
	
	unsigned char joy_x_axis = 0;
	unsigned char joy_y_axis = 0;
	int accel_x_axis = 0;
	int accel_y_axis = 0;
	int accel_z_axis = 0;
	unsigned char z_button = 0;
	unsigned char c_button = 0;
	char buffer[] = {0};
	
	Init_Sci();
	Init_Led();
	R_PG_Clock_Set();
	R_PG_I2C_Set_C1();
	
	while(1){
		
		LED_0(ON);
		
		R_PG_I2C_MasterSend_C1(
		format,//スレーブアドレスフォーマット
		SLAVE_ADDRESS_W,//スレーブアドレス
		cmd_send1, //送信データの格納先アドレス
		2//送信データ数
		);
		
		for(f = 0;f < 10000; f++);
	
		R_PG_I2C_MasterSend_C1(
		format,//スレーブアドレスフォーマット
		SLAVE_ADDRESS_W,//スレーブアドレス
		cmd_send2, //送信データの格納先アドレス
		1//送信データ数
		);
		
		for(f = 0;f < 10000; f++);
		
		R_PG_I2C_MasterReceive_C1(
		format,			//スレーブアドレスフォーマット
		SLAVE_ADDRESS_R,		//スレーブアドレス
		buffer,	//受信データの格納先アドレス
		7//受信データ数
		);
		
		for(cnt = 0;cnt < 8;cnt++){
			buffer[cnt] = nunchuk_decode_byte(buffer[cnt]) ;
		}
		
		LED_1(ON);
		
		joy_x_axis = buffer[0];
		joy_y_axis = buffer[1];
		accel_x_axis = (buffer[2]) << 2;
		accel_y_axis = (buffer[3]) << 2;
		accel_z_axis = (buffer[4]) << 2;
		
		if ((buffer[5] & 0x01) != 0){
			z_button = 1; 
		}else{
			z_button = 0; 
		}
		if ((buffer[5] & 0x02) != 0){
			c_button = 1; 
		}else{
			c_button = 0; 
		}
		accel_x_axis += ((buffer[5]) >> 2) & 0x03;
		accel_y_axis += ((buffer[5]) >> 4) & 0x03;
		accel_z_axis += ((buffer[5]) >> 6) & 0x03;
		
	
		
		
		
		//シリアル通信
		#if MODE_SCIDATA_BOX != OFF
		//座標データ
		g_sci1 = joy_x_axis;
		g_sci2 = joy_y_axis;
		g_sci3 = accel_x_axis;
		g_sci4 = accel_y_axis;
		g_sci5 = accel_z_axis;
		g_sci6 = c_button;
		g_sci7 = z_button;
		//PCにデータ送信
		sci_transformer();
		#endif
		LED_2(ON);
					
		}
}