/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static void seesaw_stand(
F32 foward,
F32 turn,
F32 gyro_offset
)
{
S8 pwm_l;
S8 pwm_r;
/* 倒立制御 */
balance_control(
(F32)foward,
(F32)turn,
(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);
return;
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_stand(
F32 foward,
F32 turn,
F32 gyro_offset
)
{
S8 pwm_l;
S8 pwm_r;
U32 ret = GAGAGE_RET_FORE;
/* 倒立制御 */
balance_control(
(F32)foward,
(F32)turn,
(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);
if (pwm_l < 0 && pwm_r < 0) {
ret = GAGAGE_RET_REAR;
} else { /* nothing */ }
return ret;
}
//キャリブレーション関数
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;
}
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_linetrace(
void
)
{
S8 pwm_l;
S8 pwm_r;
F32 forward;
F32 turn;
F32 u_turn;
U16 lightness;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
lt_calc_pid(lightness, &turn);
if (turn > 100) {
turn = 100;
} else if (turn < -100) {
turn = -100;
}
/* 絶対値をとる */
if (turn > 0) {
u_turn = turn;
} else {
u_turn = (-1) * turn;
}
/* 旋回量から前進量を調整する */
if (u_turn < 30) {
forward = 85;
} else if (u_turn < 50) {
forward = 85;
} else {
forward = 85;
}
//if (u_turn < 30) {
// forward = 100;
//} else if (u_turn < 50) {
// forward = 90;
//} else {
// forward = 85;
//}
balance_control(
(F32)forward,
turn,
(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);
return;
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_statnd(
void
)
{
S8 pwm_l;
S8 pwm_r;
int tail;
/* 倒立制御 */
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);
/* 尻尾制御 */
tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
if (tail < -40) {
gkRobotStat.robotStat = ROBOT_STAT_TRACE;
//gkRobotStat.robotStat = ROBOT_STAT_GARAGE;
} else {
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)-50, 1);
}
}
//走行体状態設定関数
void RN_modesetting(){
switch (runner_mode){
//走行体初期状態
case (RN_MODE_INIT):
cmd_forward = 0;
cmd_turn = 0;
break;
//バランサー
case (RN_MODE_BALANCE):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
//バランサー無し
case (RN_MODE_BALANCEOFF):
break;
default:
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
}
}
//走行方法設定
void RN_modesetting()
{
switch (runner_mode){
case (RN_MODE_INIT):
cmd_forward = 0;
cmd_turn = 0;
break;
case (RN_MODE_CONTROL):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
case (RN_MODE_CONTROL_2):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
case (RN_MODE_STOP):
break;
default:
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
}
}
/**
* バランスコントロール
*/
void BLNU_balance_control(void) {
S8 pwm_l, pwm_r;
/* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
balance_control((F32) BLNU_cmd_forward, (F32) BLNU_cmd_turn,
(F32) ecrobot_get_gyro_sensor(BLNU_PORT), (F32) BLNU_gyro_offset,
(F32) nxt_motor_get_count(L_MOTOR_PORT), (F32) nxt_motor_get_count(
R_MOTOR_PORT), (F32) ecrobot_get_battery_voltage(), &pwm_l,
&pwm_r);
nxt_motor_set_speed(L_MOTOR_PORT, pwm_l, 1);
nxt_motor_set_speed(R_MOTOR_PORT, pwm_r, 1);
}
//衝撃検知
void shock(void){
if(min_vol>ecrobot_get_battery_voltage())
min_vol=ecrobot_get_battery_voltage();
if((ecrobot_get_gyro_sensor(NXT_PORT_S1) <= gyro_offset-PM_GYRO ||
ecrobot_get_gyro_sensor(NXT_PORT_S1) >= gyro_offset+PM_GYRO) &&
min_vol <= battery_value-DOWN_BATTERY)
{
ecrobot_sound_tone(880,512,30);
if(runner_mode == RN_MODE_CONTROL){
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
setting_mode = RN_SLOW_RUN;
runner_mode = RN_MODE_CONTROL_2;
gyro_offset += 10;
min_vol = battery_value;
}
}
}
PWMValues calcBalancePWMValue(PWMValues outputvalues)
{
balance_control(
(F32)getCmdForward(),
(F32)getCmdTurn(),
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)getGyroOffset(),
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&outputvalues.pwmL,
&outputvalues.pwmR);
return outputvalues;
}
void RN_set_gyro()
{
/* ジャイロセンサの設定をする */
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
avg_cnt++;
/* 1秒経過したら、ジャイロセンサのオフセット値の平均値を計算し、次の状態に遷移する。 */
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U);
setting_mode = RN_SETTINGMODE_GYRO_END;
}
}
PWMValues PWMValGenerator_calBalancePWM(PWMValGenerator * this_PWMValGenerator,float forward,float turn) {
PWMValues pValues;
balance_control(
(F32)forward,
(F32)turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)this_PWMValGenerator ->gyroOffset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pValues.pwmL,
&pValues.pwmR);
return pValues;
}
/*!*********************************************************
* @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 */ }
}
//走行状態設定関数
void RN_setting()
{
int forward_speed;
PWMValues pValues;
switch (setting_mode){
//キャリブレーション状態
case (RN_SETTINGMODE_START):
if(Calibration_doCalibrate(&mCalibration) == 1)
setting_mode = RN_RUN;
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):
//wait_count++;
//RA_linetrace_PID_balanceoff(100);
pValues = PWMValGenerator_calTailRunPWM(&mPWMValGenerator,50,PIDCalcDebug_PIDLinetrace(&mPIDCalcDebug));
nxt_motor_set_speed(NXT_PORT_C, pValues.pwmL, 1);
nxt_motor_set_speed(NXT_PORT_B, pValues.pwmR, 1);
/*
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500)
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE_START;
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
wait_count = 0;
}
*/
break;
case (RN_SLOPE_START):
RA_linetrace_PID_balanceoff(70);
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_now_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if(distance_now_slope - distance_before_slope > 30)
{
setting_mode = RN_SLOPE_DOWN;
}
break;
case (RN_SLOPE_DOWN):
RA_linetrace_PID_balanceoff(55);
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1))
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE_AFTER;
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_peak_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
}
break;
case (RN_SLOPE_AFTER):
RA_linetrace_PID_balanceoff(55);
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_after_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if(distance_after_slope - distance_peak_slope > 30)
{
setting_mode = RN_SLOPE_END;
}
break;
case (RN_SLOPE_END):
RA_linetrace_PID_balanceoff(65);
break;
default:
break;
}
}
/* ______________アウトコース走行区間検出_______________________ */
void setSection_out(){
/* アウトコース走行区間 */
static int wait_count = 0;
wait_count++;
float def_x = getXCoo() - buf_x;
float def_y = getYCoo() - buf_y;
float def_l = getDistance() - buf_l;
float def_th = getTheta( ) - buf_th;
switch(crt_sect){
case (START): //スタート→坂道
if(getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500){
ecrobot_sound_tone(220, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = UP_SLOPE;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (UP_SLOPE): //坂道始点→頂点
if(def_l >= 30 && getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)){
ecrobot_sound_tone(233, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = DOWN_SLOPE;
trgt_speed = trgt_speed -10;
}
trgt_R = 0.0;
break;
case (DOWN_SLOPE): //頂点→坂道終点
//if(getDistance() >= 390){
if(def_l >= 90){
ecrobot_sound_tone(246, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_CORNER;
trgt_speed = trgt_speed - 10;
}
trgt_R = 0.0;
break;
case (FST_CORNER): //坂道終点→第一カーブ
if(def_x >= 74 && def_y >= 70 && def_l >= 110 && def_th >= 90){
ecrobot_sound_tone(261, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_STRAIGHT;
trgt_speed = trgt_speed +20;
}
trgt_R = 67.59;
break;
case (FST_STRAIGHT): //第一カーブ終点→第一ストレート
if(def_l >= 115){
ecrobot_sound_tone(277, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_CORNER;
trgt_speed = trgt_speed +10;
}
trgt_R = 0.0;
break;
case (SND_CORNER): //第一ストレート終点→第二カーブ
if(def_x <= -95 && def_y <= -5 && def_l >= 245 && def_th >= 240){
ecrobot_sound_tone(293, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_STRAIGHT;
trgt_speed = trgt_speed;
}
trgt_R = 56.59;
break;
case (SND_STRAIGHT): //第二カーブ終点→第二ストレート
if(def_x >= 40 && def_y <= -15 && def_l >= 40){
ecrobot_sound_tone(311, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_CORNER;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (TRD_CORNER): //第二ストレート終点→第三カーブ
if(def_x <= -80 && def_y <= -80 && def_l >= 235 && def_th <= -210){
ecrobot_sound_tone(329, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_STRAIGHT;
trgt_speed = trgt_speed;
}
trgt_R = -64.02;
break;
case (TRD_STRAIGHT): //第三カーブ終点→第三ストレート
if(def_x <= -50 && def_y >= 105 && def_l >= 115){
ecrobot_sound_tone(349, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FIN_APPROACH;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (FIN_APPROACH): //第三ストレート終点→マーカー
if(1){
ecrobot_sound_tone(369, 100, 50);
trgt_speed = trgt_speed;
}
trgt_R = 51.80;
break;
case (STEPS): //階段
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (TURN90D):
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
default:
trgt_speed = 0;
trgt_R = 0.0;
break;
}
}
//走行状態設定関数
void RN_setting()
{
static int timecounter = 0;
float weight = 0.6;
static int markerflag = 0;
switch (setting_mode){
//キャリブレーション状態
case (RN_SETTINGMODE_START):
if(RN_calibrate() == 1)
{
setting_mode = RN_RUN;
PWMGeneratorModeChange(RN_MODE_TAIL);
TailAngleChange(ANGLEOFDOWN);
}
break;
//通常走行状態
case (RN_RUN):
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
setSection_in();
if(getInitGyroOffset() - 50 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && timecounter > 1000)
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE;
timecounter = 0;
}
break;
case (RN_SLOPE):
setSection_in();
if(runningSlope() == 1)
setting_mode = RN_RUN_SECOND;
break;
case (RN_RUN_SECOND):
setSection_in();
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(markerDetector() == 1)
markerflag+=1;
if(markerflag == 2)
setting_mode = RN_LOOKUPGATE;
break;
case (RN_LOOKUPGATE):
if(runningLookUpGate() == 1)
setting_mode = RN_RUN_THIRD;
break;
case (RN_RUN_THIRD):
setCmdForward(RA_speed(60));
setCmdTurn(RA_linetrace_PID(getCmdForward()));
break;
/*
case (RN_DRIFTTURN):
break;
*/
default:
break;
}
timecounter++;
}
//キャリブレーション
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;
}
}
//キャリブレーション終了
}
//*****************************************************************************
// 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 */
}
/* インコース走行区間検出 */
void setSection_out(){
static SECT_OUT crt_sect = START;
static float buf_x = 0.0, buf_y = 0.0, buf_l = 0.0, buf_th = 0.0;
float def_x = x_r - buf_x;
float def_y = y_r - buf_y;
float def_l = dist - buf_l;
float def_th = theta - buf_th;
switch(crt_sect){
case (START): //スタート→坂道
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500){
ecrobot_sound_tone(220, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = UP_SLOPE;
}
trgt_R = 0.0;
break;
case (UP_SLOPE): //坂道始点→頂点
if(def_l >= 30 && GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)){
ecrobot_sound_tone(233, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = DOWN_SLOPE;
}
trgt_R = 0.0;
break;
case (DOWN_SLOPE): //頂点→坂道終点
//if(dist >= 390){
if(def_l >= 90){
ecrobot_sound_tone(246, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_CORNER;
}
SPEED_COUNT = 50;
trgt_R = 0.0;
break;
case (FST_CORNER): //坂道終点→第一カーブ
if(def_x >= 74 && def_y >= 70 && def_l >= 110 && def_th >= 90){
ecrobot_sound_tone(261, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_STRAIGHT;
}
SPEED_COUNT = 60;
trgt_R = 67.59;
break;
case (FST_STRAIGHT): //第一カーブ終点→第一ストレート
if(def_l >= 115){
ecrobot_sound_tone(277, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_CORNER;
}
trgt_R = 0.0;
break;
case (SND_CORNER): //第一ストレート終点→第二カーブ
if(def_x <= -95 && def_y <= -5 && def_l >= 245 && def_th >= 240){
ecrobot_sound_tone(293, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_STRAIGHT;
}
trgt_R = 56.59;
break;
case (SND_STRAIGHT): //第二カーブ終点→第二ストレート
if(def_x >= 40 && def_y <= -15 && def_l >= 40){
ecrobot_sound_tone(311, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_CORNER;
}
trgt_R = 0.0;
break;
case (TRD_CORNER): //第二ストレート終点→第三カーブ
if(def_x <= -80 && def_y <= -80 && def_l >= 235 && def_th <= -210){
ecrobot_sound_tone(329, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_STRAIGHT;
}
trgt_R = -64.02;
break;
case (TRD_STRAIGHT): //第三カーブ終点→第三ストレート
if(def_x <= -50 && def_y >= 105 && def_l >= 115){
ecrobot_sound_tone(349, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FIN_APPROACH;
}
trgt_R = 0.0;
break;
case (FIN_APPROACH): //第三ストレート終点→マーカー
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (STEPS): //階段
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (90DTURN):
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
default:
break;
}
}
/**
* ジャイロセンサオフセット値の設定
*/
void BLNU_set_gyro_offset(void) {
BLNU_gyro_offset = (U32) ecrobot_get_gyro_sensor(BLNU_PORT);
}
//キャリブレーション関数
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_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;
}
}
}
int runningSlope()
{
static int timecounter = 0;
static int distanceslopestart = 0;
static int distanceslopeup = 0;
static int distanceslopetop = 0;
static int distanceslopedown = 0;
int slopeendflag = 0;
int weight = 1.0;
switch(runningslope){
case (SLOPE_START):
if(timecounter == 0)
distanceslopestart = getNowDistance();
distanceslopeup = getNowDistance();
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(distanceslopeup - distanceslopestart > 10 && timecounter > 300)
{
ecrobot_sound_tone(800, 512, 20);
runningslope = SLOPE_TOP;
timecounter = 0;
}
break;
case (SLOPE_TOP):
setCmdForward(RA_speed(50));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(getInitGyroOffset() + 50 < (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)/* && timecounter > 300*/)
{
ecrobot_sound_tone(820, 512, 20);
runningslope = SLOPE_DOWN;
timecounter = 0;
}
break;
case (SLOPE_DOWN):
if(timecounter == 0)
distanceslopetop = getNowDistance();
distanceslopedown = getNowDistance();
setCmdForward(RA_speed(50));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(distanceslopedown - distanceslopetop > 30/* && timecounter > 300*/)
{
runningslope = SLOPE_END;
timecounter = 0;
ecrobot_sound_tone(840, 512, 20);
}
break;
case (SLOPE_END):
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)/* && timecounter > 300*/)
{
ecrobot_sound_tone(900, 512, 20);
slopeendflag = 1;
timecounter = 0;
}
break;
default:
break;
}
timecounter++;
return slopeendflag;
}
void gyroUpdate()
{
gyronow = ecrobot_get_gyro_sensor(NXT_PORT_S1);
}
