/*
-----------------------------------------------------------------------------
函数名称：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);
}
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);
}
}
