//走行状態設定関数 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; } }
void seesaw_ride( Command* cmd , ROBOT_STATUS* status ) { static s1 tail_seesaw = 0; static signed char left = 0; static signed char right = 0; f4 f4t_yp; f4 f4t_yd; static u2 u2_count = 0; static u2 u2_gyro_temp=0; static s4 s4t_distance = 0; static s4 s4t_motor_L1 = 0; static s4 s4t_motor_R1 = 0; static s4 s4t_motor_L2 = 0; static s4 s4t_motor_R2 = 0; static s4 s4t_motor_L3 = 0; static s4 s4t_motor_R3 = 0; static u1 u1s_trace = 0; static u1 u1s_touritu = 1; s4t_motor_L1 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R1 = nxt_motor_get_count(PORT_MOTOR_R); static u4 u4_recieve = 0; u1 receive_data[90] = {0}; /* user logic start */ u1s_2msflipcount_see = 1 - u1s_2msflipcount_see; display_clear(0); display_goto_xy(0, 0); display_string("stage"); display_unsigned(u1g_seesaw_ride_mode,3); display_goto_xy(0, 1); display_string("LEFT"); display_unsigned(left,3); display_goto_xy(0, 2); display_string("RIGHT"); display_unsigned(right,3); display_goto_xy(0, 3); display_string("count"); display_unsigned(f4g_forward,3); display_update(); if ( u1s_2msflipcount_see == 1 ) { switch(u1g_seesaw_ride_mode) /* シーソーシーケンス開始 */ { case 0 : /* 倒立開始 */ // u1s_trace = 1; /* トレースOFF */ u1s_touritu = 1; /* トレースON */ left = 0; right = 0; f4g_forward = 0; // balance_init(); /* balance APIの初期化 */ // nxt_motor_set_count(PORT_MOTOR_L, 0); /* 左モータ・カウント値[°]を0リセット */ // nxt_motor_set_count(PORT_MOTOR_R, 0); /* 右モータ・カウント値[°]を0リセット */ s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u2_count= 0; // u1g_seesaw_mode = 1; tail_seesaw = tailControl( 30, status->motorangle_T); if(status->motorangle_T > 10){ u1g_seesaw_ride_mode = 1; tail_seesaw = 0; } break; case 1 : s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 30){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 20; chokushin2(); }else if(s4t_distance < 200){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; }else if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode++; } break; case 2 : /* 段差検知まで */ s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 200){ u1s_trace = 1; /*トレースの設定*/ f4g_forward = 40; }else{ f4g_forward = 20; if(f4g_turn < 3 && f4g_turn > -3){ u2_count++; }else{ u2_count = 0; } } if(u2_count > 20 && f4g_turn < 2 && f4g_turn > -2){ u1g_seesaw_ride_mode++; u2_count = 0; f4g_turn = 0; } break; case 3: u1s_trace = 0; f4g_forward = 30; chokushin2(); if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1g_seesaw_ride_mode++; f4g_forward = 100; s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R); u2_count = 0; u1s_trace = 0; u2_gyro_temp = robot.robotSensor.GYRO.gyroOffsetValue - 5; } break; case 4 : /* 検知後ある一定以上上る */ s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2; if(s4t_distance < 50){ //ベース100 robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp; f4g_forward = 80; chokushin2(); }else if(s4t_distance < 250){ //べーす300 // robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp; f4g_forward = 20; chokushin2(); }else{ u2_count = 0; f4g_forward = 0; chokushin2(); u1g_seesaw_ride_mode = 6; } break; case 5 : /* Bluetoothが来るまで待機 */ f4g_forward = 0; chokushin2(); u4_recieve = ecrobot_read_bt_packet( receive_data, 90 ); if ( u4_recieve > 0 ) { ecrobot_sound_tone(100, 100, 50); u2_count = 0; f4g_forward = 0; chokushin2(); u1g_seesaw_ride_mode++; } break; case 6 : /* Bluetoothが来たから上る */ if(u2_count < 250){ f4g_forward = 40; chokushin2(); u2_count++; }else{ if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){ u1g_seesaw_ride_mode++; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u2_count = 0; }else{ f4g_forward = 40; chokushin2(); } } break; case 7 : /* 段差検知 */ s4t_distance = s4t_motor_L1 - s4t_motor_L3 + s4t_motor_R1 - s4t_motor_R3; if(s4t_distance < 100){ f4g_forward = 60; chokushin2(); }else{ f4g_forward = 0; chokushin2(); u2_count++; if(u2_count > 500){ s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode++; u2_count = 0; } } break; case 8 : /* ライン復帰 */ if(s4t_motor_L1 - s4t_motor_L3 < 250){ if(status->lightval > GRAY_WHITE){ f4g_forward = 20; f4g_turn = f4g_forward; u1g_seesaw_ride_mode = 11; ecrobot_sound_tone(100, 100, 50); }else{ f4g_forward = 20; f4g_turn = f4g_forward; } }else{ f4g_turn = 0; u1g_seesaw_ride_mode = 9; } break; case 9 : /* 右側に居る */ if( s4t_motor_L1 - s4t_motor_L3 > -100){ f4g_forward = -20; f4g_turn = f4g_forward; }else{ // f4g_turn = 0; // f4g_forward = 0; u1g_seesaw_ride_mode = 10; } break; case 10: f4g_forward = 30; chokushin2(); if(status->lightval > GRAY_WHITE){ f4g_turn = 0; f4g_forward = 0; u1s_trace = 1; f4g_forward = 20; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); u1g_seesaw_ride_mode = 13; } break; case 11 : /* ライン復帰 */ if((status->lightval > GRAY_WHITE - 30) && (u2_count == 0)){ f4g_turn = f4g_forward; //ecrobot_sound_tone(1000, 1000, 50); }else{ f4g_forward = 0; f4g_turn = -40; u2_count = 1; if((s4t_motor_L1 - s4t_motor_L3) - (s4t_motor_R1 - s4t_motor_R3) < -100 ){ f4g_forward = 0; //f4g_turn = 0; u1g_seesaw_ride_mode = 12; } } break; case 12: f4g_forward = 30; chokushin2(); if(status->lightval > GRAY_WHITE){ f4g_turn = 0; f4g_forward = 0; u1s_trace = 1; f4g_forward = 20; u1g_seesaw_ride_mode = 13; s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L); s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R); } break; case 13 : u1s_trace = 1; f4g_forward = 20; s4t_distance = s4t_motor_L1 - s4t_motor_L3 + s4t_motor_R1 - s4t_motor_R3; if(s4t_distance > 300){ u1g_seesaw_ride_mode = 14; } break; case 14 : u1s_trace = 1; f4g_forward = 20; break; } if (u1s_trace == 1) { f4t_yp = (f4)(GRAY_WHITE - status->lightval ); f4t_yd = (f4t_yp - f4s_last_yp) / 0.004; f4s_last_yp = f4t_yp; f4g_turn = (f4)((f4)TRACE_P * f4t_yp) + (f4)((f4)TRACE_D * f4t_yd); // f4g_turn = -f4g_turn; /* 旋回方向決定 */ if(f4g_turn > 50){ f4g_turn = 50; }else if(f4g_turn < -50){ f4g_turn = -50; }else{ f4g_turn = f4g_turn; } f4g_turn = -f4g_turn; } if (u1s_touritu == 1) { balance_control( (f4)f4g_forward, /* 前後進命令(+:前進, -:後進) */ (f4)f4g_turn, /* 旋回命令(+:右旋回, -:左旋回) */ // (f4)-6, /* 旋回命令(+:右旋回, -:左旋回) */ (f4)status->gyroval, /* ジャイロセンサ値 */ (f4)status->gyro_offset, /* ジャイロセンサオフセット値 */ (f4)status->motorangle_L, /* 左モータ回転角度[deg] */ (f4)status->motorangle_R, /* 右モータ回転角度[deg] */ (f4)status->batteryval, /* バッテリ電圧[mV] */ &left, /* 左モータPWM出力値 */ &right /* 右モータPWM出力値 */ ); } setMotorVal( left , right , tail_seesaw ); } /* user logic end */ }
//走行状態設定関数 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; } }
//***************************************************************************** // 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 */ }