//キャリブレーション関数
void RN_calibrate()
{
/*バランサーON用*/
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(932, 512, 10);
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
systick_wait_ms(500);
break;
}
}
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(982,512,10);
setting_mode = RN_RUN;
runner_mode = RN_MODE_BALANCE;
systick_wait_ms(500);
break;
}
}
}
//走行状態設定関数
void RN_setting(){
static unsigned char buf = 0;
if (ecrobot_get_touch_sensor(NXT_PORT_S4)){
buf = 1;
}
if (!ecrobot_get_touch_sensor(NXT_PORT_S4) && buf){
runner_mode = RN_MODE_INIT;
setting_mode = RN_SETTINGMODE_START;
}
switch (setting_mode){
//キャリブレーション状態
case (RN_SETTINGMODE_START):
RN_calibrate();
LV_buf = ecrobot_get_light_sensor(NXT_PORT_S3);
break;
case (RN_SPEEDZERO):
cmd_forward = 0;
cmd_turn = RA_wheels(cmd_turn);
wait_count++;
if(wait_count >= 200){
setting_mode = RN_RUN;
wait_count = 0;
}
break;
//通常走行状態
case (RN_RUN):
RA_linetrace_PID_balanceoff(SPEED_COUNT);
cmd_turn += RA_curvatureCtrl_PID(trgt_R);
wait_count++;
break;
// 曲率PID
case (RN_CVRUN):
cmd_forward = SPEED_COUNT;
cmd_turn = RA_curvatureCtrl_PID(trgt_R);
wait_count++;
break;
case (RN_LOT90):
cmd_turn = 0;
cmd_turn = RA_directionCtrl_PID(90);
wait_count++;
break;
default:
break;
}
}
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet...
static void
decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors )
{
U8 temp_port;
output_packet->port = input_packet->port;
output_packet->operation = input_packet->operation;
switch (input_packet->operation) {
case LIGHT_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_light_sensor(temp_port);
break;
case TOUCH_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_touch_sensor(temp_port);
break;
case SOUND_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sound_sensor(temp_port);
break;
case RADAR_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port);
break;
case TACHO_COUNT:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) nxt_motor_get_count(temp_port);
break;
case SET_POWER:
decode_bro_port (input_packet->port, &temp_port);
switch (temp_port) {
case NXT_PORT_A:
motors->first.speed_control_type = RAW_POWER;
motors->first.powers[0] = input_packet->data;
output_packet->data = (float) nxt_motor_get_count(NXT_PORT_A);
output_packet->timestamp = (float) systick_get_ms();
break;
case NXT_PORT_B:
motors->second.speed_control_type = RAW_POWER;
motors->second.powers[0] = input_packet->data;
break;
case NXT_PORT_C:
motors->third.speed_control_type = RAW_POWER;
motors->third.powers[0] = input_packet->data;
break;
};
break;
default:
//Nothing HERE
break;
};
}
/*
//急加速用関数
int rapid_speed_up(int target_gyro){
static int rapid_speed_up_counter = 0;
if(rapid_speed_up_counter >= 0)
{
if(GYRO_OFFSET_INIT > (target_gyro + GYRO_OFFSET_INIT))
GYRO_OFFSET_INIT--;
else
GYRO_OFFSET_INIT++;
rapid_speed_up_counter = 0;
}
rapid_speed_up_counter++;
if((target_gyro + GYRO_OFFSET_INIT) == GYRO_OFFSET_INIT)
return 1;
else
return 0;
}
*/
static int RN_rapid_speed_up_signal_recevie(void)
{
int i;
unsigned int rx_len;
unsigned char start = 0;
for (i=0; i<BT_MAX_RX_BUF_SIZE; i++)
{
rx_buf[i] = 0; /* 受信バッファをクリア */
}
rx_len = ecrobot_read_bt(rx_buf, 0, BT_MAX_RX_BUF_SIZE);
if (rx_len > 0)
{
/* 受信データあり */
if (rx_buf[0] == rapid_SPEED_UP_SIGNAL)
{
start = 1;
stepflag = 1;
}
ecrobot_send_bt(rx_buf, 0, rx_len); /* 受信データをエコーバック */
}
else if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
start = 1;
return start;
}
void RN_set_gyro_end()
{
/* バンパを離すと次の状態に遷移する */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE) {
setting_mode = RN_SETTINGMODE_OK;
}
}
void RN_set_ok_end()
{
/* バンパを離すと次の状態に遷移する(設定モードを終了する）*/
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE) {
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SETTINGMODE_END;
}
}
void RN_set_ok()
{
/* スタート位置にロボットを置き、バンパを押すと次の状態に遷移する。 */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) {
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SETTINGMODE_OK_END;
}
}
/**
* 荷物積載催促中
*/
static void AC_carry_press(void) {
DU_showString("4thPhase");
while(!ecrobot_get_touch_sensor(NXT_PORT_S2)){
LDC_stop();
HU_confirSound();
}
AC_changeStatus(AC_REVERSAL);
}
//*****************************************************************************
// 関数名 : calibration
// 引数 : *black (黒、最小値)，*white（白、最大値）
// 返り値 : 無し
// 概要 : 光センサの手動キャリブレーション
// 黒白の順でタッチする。
//*****************************************************************************
void calibration(int *black,int *white,int angle)
{
while(1) {
tail_control(angle);
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) {
ecrobot_sound_tone(440, 170, 100);
*black = ecrobot_get_light_sensor(NXT_PORT_S3);
display_clear(0); /* 画面表示 */
display_goto_xy(0, 1);
display_string("BLACK:");
display_int(*black, 4);
break;
}//if
systick_wait_ms(100); /* 10msecウェイト */
}//while
display_update();
while(ecrobot_get_touch_sensor(NXT_PORT_S4));
ecrobot_sound_tone(880, 170, 100);
while(1) {
tail_control(angle);
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) {
ecrobot_sound_tone(880, 170, 100);
*white = ecrobot_get_light_sensor(NXT_PORT_S3);
//display_clear(0); /* 画面表示 */
display_goto_xy(0, 2);
display_string("WHITE:");
display_int(*white, 4);
break;
}//if
systick_wait_ms(100); /* 10msecウェイト */
}//while
//display_clear(0); /* 画面表示 */
display_goto_xy(0,4);
display_string("TH:");
display_int(TH(*black,*white), 3);
display_update();
while(ecrobot_get_touch_sensor(NXT_PORT_S4));
ecrobot_sound_tone(440, 170, 100);
}
void RN_set_gyro_start()
{
/* ジャイロセンサの設定を始める */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) {
ecrobot_sound_tone(880, 512, 30);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
setting_mode = RN_SETTINGMODE_GYRO;
}
}
/**
* 荷物降ろし催促中
*/
static void AC_lift_press(void) {
DU_showString("Hurry lift off");
while(ecrobot_get_touch_sensor(NXT_PORT_S2)){
LDC_stop();
HU_confirSound();
}
AC_changeStatus(AC_FINISH);
}
/**
* 荷崩れ中
*/
static void AC_crumble(void) {
DU_showString("crumble");
while(!ecrobot_get_touch_sensor(NXT_PORT_S2)){
LDC_stop();
HU_confirSound();
}
AC_changeStatus(AC_RETURN);
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_pol_triger(
void
)
{
U8 touch;
U32 bt = 0;
char buf[128];
U32 size;
S8 pwm_l;
S8 pwm_r;
/* タッチセンサ */
touch = ecrobot_get_touch_sensor(PORT_TOUCH);
/* Bluetooth */
size = ecrobot_read_bt(buf, 0, 128);
if (size > 0) {
if (buf[0] == 's') {
ecrobot_send_bt("start", 0, 5);
bt = 1;
} else {
ecrobot_send_bt("unknown", 0, 7);
}
} else { /* nothing */ }
if (touch == 1 || bt == 1) {
/* 倒立制御 */
balance_control(
(F32)0,
(F32)0,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
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);
nxt_motor_set_count(PORT_MOTOR_TAIL, (int)0);
gkRobotStat.robotStat = ROBOT_STAT_STAND;
} else { /* NOTHING */ }
}
inline std_return RTE_StopSensor_GetSensorValue_OSPort_In(uint8_t* value)
{
static uint8_t IIC_Initialized = 0;
#ifdef RTE_StopSensor_GetSensorValue_OSPort_In_BUTTON
/* als Abbruchbedingung ist ein Button definiert */
uint8_t currentStatus = ecrobot_get_touch_sensor(RTE_StopSensor_GetSensorValue_OSPort_In_PORT);
static uint8_t driverButtonLastState = 0;
if ( currentStatus != driverButtonLastState && currentStatus != 0)
{
/* Button is gedrückt */
*value = 1;
}
else
{
/* Button nicht gedrückt */
*value = 0;
}
driverButtonLastState = currentStatus;
#endif
#ifdef RTE_StopSensor_GetSensorValue_OSPort_In_IIC
/* als Abbruchbedingung ist der IIC definiert */
static uint8_t currentStatus_StopSensor = 0xFF;
// if(!IIC_Initialized)
// {
// IIC_Initialized = 1;
// i2c_enable(RTE_StopSensor_GetSensorValue_OSPort_In_PORT);
// }
ecrobot_send_i2c(RTE_StopSensor_GetSensorValue_OSPort_In_PORT, 0x20, currentStatus, ¤tStatus, 0);
while(i2c_busy(RTE_StopSensor_GetSensorValue_OSPort_In_PORT) != 0);
ecrobot_read_i2c(RTE_StopSensor_GetSensorValue_OSPort_In_PORT, 0x20, 0xF0, ¤tStatus, 1);
if((currentStatus == 0xD0) || (currentStatus == 0xE0))
{
*value = 1;
}
else
{
*value = 0;
}
#endif
return 0;
}
/**
* 荷物降ろし待機
*/
static void AC_lift_wait(void) {
DU_showString("LIFT WAIT");
if(!TU_isStart()){
TU_start(3000);
}
while(ecrobot_get_touch_sensor(NXT_PORT_S2)){
LDC_stop();
if(TU_isTimeout()){
HU_confirSound();
TU_finish();
AC_changeStatus(AC_LIFTPRESS);
return;
}
}
AC_changeStatus(AC_FINISH);
}
/**
* 荷物積載待機
*/
static void AC_carry_wait(void) {
DU_showString("3rdPhase");
if(!TU_isStart()){
TU_start(3000);
}
while(!ecrobot_get_touch_sensor(NXT_PORT_S2)){
LDC_stop();
if(TU_isTimeout()){
HU_confirSound();
TU_finish();
AC_changeStatus(AC_CARRYPRESS);
return;
}
}
reverse_state =0;
AC_changeStatus(AC_REVERSAL);
}
/**
* 帰還中
*/
static void AC_return(void) {
DU_showString("Return!!");
LDC_pre();
while(!BU_isTouched()){
LDC_linetrace2();
if(!ecrobot_get_touch_sensor(NXT_PORT_S2)){
AC_changeStatus(AC_CRUMBLE);
return;
}
}
if(!TU_isStart()){
TU_start(100);
}
if(TU_isTimeout()){
HU_confirSound();
TU_finish();
AC_changeStatus(AC_LIFTWAIT);
}
}
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet...
static void
decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors)
{
U8 temp_port;
output_packet->port = input_packet->port;
output_packet->operation = input_packet->operation;
switch (input_packet->operation) {
case LIGHT_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_light_sensor(temp_port);
break;
case TOUCH_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_touch_sensor(temp_port);
break;
case SOUND_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sound_sensor(temp_port);
break;
case RADAR_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port);
break;
case TACHO_COUNT:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) nxt_motor_get_count(temp_port);
break;
case SET_POWER:
decode_bro_port (input_packet->port, &temp_port);
switch (temp_port) {
case NXT_PORT_A:
motors->first.speed_control_type = RAW_POWER;
motors->first.powers[0] = input_packet->data;
output_packet->data = (float) nxt_motor_get_count(temp_port);
output_packet->time = (long) ecrobot_get_systick_ms();
break;
case NXT_PORT_B:
motors->second.speed_control_type = RAW_POWER;
motors->second.powers[0] = input_packet->data;
output_packet->data = (float) nxt_motor_get_count(temp_port);
output_packet->time = (long) ecrobot_get_systick_ms();
break;
case NXT_PORT_C:
motors->third.speed_control_type = RAW_POWER;
motors->third.powers[0] = input_packet->data;
output_packet->data = (float) nxt_motor_get_count(temp_port);
output_packet->time = (long) ecrobot_get_systick_ms();
break;
};
break;
case SET_SPEED:
motors->first.speed_control_type = PID_CONTROLLED;
motors->second.speed_control_type = PID_CONTROLLED;
motors->first.speed_ref[setSpeedCounter] = input_packet->data;
motors->second.speed_ref[setSpeedCounter] = input_packet->data;
if(setSpeedCounter == 0){
motors->first.distance_ref[setSpeedCounter] = input_packet->data2;
motors->second.distance_ref[setSpeedCounter] = input_packet->data2;
}else{
motors->first.distance_ref[setSpeedCounter] = motors->first.distance_ref[setSpeedCounter -1] + input_packet->data2;
motors->second.distance_ref[setSpeedCounter] = motors->second.distance_ref[setSpeedCounter -1] + input_packet->data2;
}
setSpeedCounter++;
break;
default:
break;
};
}
//キャリブレーション関数
void RN_calibrate()
{
//黒値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(880, 512, 10);
BLACK_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//白値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(906, 512, 10);
WHITE_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//灰色値計算
GRAY_VALUE=(BLACK_VALUE+WHITE_VALUE)/2;
//ジャイロオフセット及びバッテリ電圧値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(932, 512, 10);
GYRO_OFFSET_INIT += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
GYRO_OFFSET_INIT = GYRO_OFFSET_INIT;
gyroValue = GYRO_OFFSET_INIT;
battery_value = ecrobot_get_battery_voltage();
min_vol = battery_value;
systick_wait_ms(500);
break;
}
}
//走行開始合図
while(1){
//リモートスタート
if(remote_start()==1)
{
ecrobot_sound_tone(982,512,10);
tail_mode = RN_TAILUP;
setting_mode = RN_RUN;
runner_mode = RN_MODE_BALANCE;
break;
}
//タッチセンサスタート
else if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(982,512,10);
while(1){
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE)
{
setting_mode = RN_RUN;
runner_mode = RN_MODE_BALANCE;
tail_mode = RN_TAILUP;
break;
}
}
break;
}
}
}
//走行状態設定関数
void RN_setting()
{
static int wait_count = 0;
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])/2; /* 前進量(そのままでは早すぎるので値を半分） */
cmd_turn = ((S8)bt_receive_buf[1]); /* 旋回量 */
//ターボチェック
if(boost() == 1)
{
setting_mode = RN_BOOST;
ecrobot_sound_tone(980,512,100);
}
else /* ターボ無し、スティック操作 */
{
nxt_motor_set_speed(NXT_PORT_C, cmd_forward + cmd_turn/2, 1);
nxt_motor_set_speed(NXT_PORT_B, cmd_forward - cmd_turn/2, 1);
}
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) /* ヒットチェック */
{
ecrobot_sound_tone(980,512,100);
nxt_motor_set_speed(NXT_PORT_C,100,1);
nxt_motor_set_speed(NXT_PORT_B,-100,1);
wait_count = 0;
setting_mode = RN_PUSHBUTTON;
}
break;
//ターボ動作(ver 2.0新機能）
case (RN_BOOST):
wait_count++;
/* 両車輪にモータ操作量の最大値を送信（スティック操作不可） */
nxt_motor_set_speed(NXT_PORT_C,127,1);
nxt_motor_set_speed(NXT_PORT_B,127,1);
/* 1秒後に通常モードに復帰 */
if(wait_count > 125)
{
setting_mode = RN_RUN;
wait_count = 0;
}
break;
//敗北動作
case (RN_PUSHBUTTON):
wait_count++;
if(wait_count >= 150)
{
TailAngleChange(ANGLEOFZERO);
nxt_motor_set_speed(NXT_PORT_C,0,1);
nxt_motor_set_speed(NXT_PORT_B,0,1);
}
if(wait_count == 1000)
setting_mode = RN_START;
break;
default:
break;
}
}
//キャリブレーション
void RN_calibrate()
{
//黒値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(880, 512, 30);
BLACK_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//白値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(906, 512, 30);
WHITE_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//灰色値計算
GRAY_VALUE=(BLACK_VALUE+WHITE_VALUE)/2;
//ジャイロオフセット及びバッテリ電圧値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(932, 512, 30);
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
battery_value = ecrobot_get_battery_voltage();
min_vol = battery_value;
systick_wait_ms(500);
break;
}
}
//走行開始合図
while(1){
if(remote_start()==1)
{
ecrobot_sound_tone(982,512,30);
tail_mode = RN_TAILUP;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
break;
}
else if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(982,512,30);
while(1){
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE)
{
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
break;
}
}
break;
}
}
//キャリブレーション終了
}
//キャリブレーション関数
void RN_calibrate(){
tail_mode_change(0,ANGLEOFDOWN,1,2);
//黒値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(880, 512, 10);
BLACK_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//白値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(906, 512, 10);
WHITE_VALUE=ecrobot_get_light_sensor(NXT_PORT_S3);
systick_wait_ms(500);
break;
}
}
//灰色値計算
GRAY_VALUE=(BLACK_VALUE+WHITE_VALUE)/2;
//ジャイロオフセット及びバッテリ電圧値
while(1){
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(932, 512, 10);
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
systick_wait_ms(500);
break;
}
}
//走行開始合図
while(1){
//リモートスタート
if(remote_start()==1){
ecrobot_sound_tone(982,512,30);
tail_mode_change(1,ANGLEOFUP,0,2);
setting_mode = RN_SPEEDZERO;
runner_mode = RN_MODE_BALANCE;
break;
}
//タッチセンサスタート
else if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE){
ecrobot_sound_tone(982,512,10);
while(1){
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE){
setting_mode = RN_SPEEDZERO;
runner_mode_change(1);
tail_mode_change(1,ANGLEOFUP,0,2);
break;
}
}
break;
}
}
}
//走行設定関数
void RN_setting()
{
static int step_count = 0;
static int time_count = 0;
//距離計測用変数
int distance_before_step = 0;
int distance_step_brake = 0;
int distance_step_stop = 0;
int distance_gyro_up = 0;
int distance_over_forty = 0;
int distance_turn_clear = 0;
int distance_turn_after = 0;
static unsigned int angle_l_now = 0;
static unsigned int angle_r_now = 0;
switch (setting_mode){
case (TYREAL) :
do_tyreal(&Kp,&Ki,&Kd);
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
systick_wait_ms(500);
setting_mode = RN_SETTINGMODE_START;
}
break;
//走行開始前
case (RN_SETTINGMODE_START):
RN_calibrate(); //キャリブレーション
ecrobot_set_motor_speed(NXT_PORT_A,0);
ecrobot_set_motor_speed(NXT_PORT_B,0);
ecrobot_set_motor_speed(NXT_PORT_C,0);
break;
case (RN_RUN):
RA_linetrace_PID(15);
setting_mode = RN_STEP_TURN_START;
break;
case (RN_STEP_TURN_START):
time_count++;
RA_linetrace_PID(15);
if(lightnow < RIGHT_ANGLE_LIGHT_VALUE && time_count > 300)
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_STEP_TURN_LEFT;
time_count = 0;
}
break;
//直角カーブ
case (RN_STEP_TURN_LEFT):
cmd_forward = 0;
cmd_turn = 0;
if(time_count == 0)
{
angle_l_now = ecrobot_get_motor_rev(NXT_PORT_B);
angle_r_now = ecrobot_get_motor_rev(NXT_PORT_C);
}
time_count++;
if(ecrobot_get_motor_rev(NXT_PORT_B) - angle_l_now <= RIGHT_ANGLE_AIM)
{
/* 回転する */
cmd_turn = -100;
}
else
{
/* 止まる */
time_count = 0;
setting_mode = RN_STEP_TURN_FORWARD;
}
break;
case (RN_STEP_TURN_FORWARD):
RA_linetrace_PID(20);
if(GYRO_OFFSET_INIT - 50 > gyronow || GYRO_OFFSET_INIT + 50 < gyronow && time_count > 200)
{
//GYRO_OFFSET_INIT += 7;
setting_mode = RN_STOP;
}
break;
//強制停止
case(RN_STOP):
cmd_forward = 10;
cmd_turn = 0;
//nxt_motor_set_speed(NXT_PORT_C, 0, 1);
//nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
default:
break;
}
}
