//走行状態設定関数
void RN_setting()
{
ecrobot_bt_data_logger(cmd_turn, cmd_forward);
switch (setting_mode){
//キャリブレーション状態
case (RN_START):
RN_calibrate();
break;
//通常走行状態
case (RN_RUN):
(void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
cmd_forward = -((S8)bt_receive_buf[0]);
cmd_turn = ((S8)bt_receive_buf[1]);
break;
default:
break;
}
}
//*****************************************************************************
// TASK : tsk0
// DESCRIPTION : 4msec periodical Task and controls NXTway-GS
// INIT_MODE
// ��
// CAL_MODE
// ��
// CONTROL_MODE
//*****************************************************************************
void tsk0(VP_INT exinf)
{
static U32 gyro_offset; /* gyro sensor offset value */
static U32 avg_cnt; /* average count to calc gyro offset */
static U32 cal_start_time; /* calibration start time */
static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */
SINT i;
S8 cmd_forward, cmd_turn;
S8 pwm_l, pwm_r;
switch(nxtway_gs_mode){
case(INIT_MODE):
gyro_offset = 0;
avg_cnt = 0;
for (i = 0; i < BT_RCV_BUF_SIZE; i++){
bt_receive_buf[i] = 0;
}
balance_init(); /* NXTway-GS C API initialize function */
nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */
nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */
cal_start_time = ecrobot_get_systick_ms();
nxtway_gs_mode = CAL_MODE;
break;
case(CAL_MODE):
gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);
avg_cnt++;
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
/* 1000msec later, start balancing */
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U); /* beep a tone */
nxtway_gs_mode = CONTROL_MODE;
}
break;
case(CONTROL_MODE):
(void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
/*
* R/C command from NXT GamePad
* buf[0]: -100(forward max.) to 100(backward max.)
* buf[1]: -100(turn left max.) to 100(turn right max.)
*/
cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */
cmd_turn = (S8)bt_receive_buf[1];
if (obstacle_flag == 1){
/* make NXTway-GS move backward to avoid obstacle */
cmd_forward = -100;
cmd_turn = 0;
}
/* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
(F32)gyro_offset,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */
break;
default:
/* unexpected mode */
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
break;
}
ext_tsk(); /* terminates this task */
}
static void AC_log(){
ecrobot_bt_data_logger(AC_currentStatus, j--);
ecrobot_status_monitor("Data Logging");
}
