//衝撃検知関数 int shock(int target){ int result=0; //電圧降下の最小値を更新 if(min_vol>ecrobot_get_battery_voltage()) min_vol=ecrobot_get_battery_voltage(); //ジャイロ及び電圧降下から衝撃検知 if(target <= battery_value-min_vol) { result = 1; } else result = 0; return result; }
//衝撃検知 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; } } }
/** * バランスコントロール */ 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); }
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; }
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; }
//***************************************************************************** // 関数名 : line_follow // 引数 : speed, turn 走行速度、旋回速度 // 引数 : gyro_sensor ジャイロセンサー値 // 返り値 : 無し // 概要 : ライントレース //***************************************************************************** void line_follow(int speed, int turn, int gyro_sensor) { signed char pwm_L, pwm_R; // 左右モータPWM出力 balance_control( (float)speed, /* 前後進命令(+:前進, -:後進) */ (float)turn, /* 旋回命令(+:右旋回, -:左旋回) */ (float)gyro_sensor, /* ジャイロセンサ値 */ (float)GYRO_OFFSET, /* ジャイロセンサオフセット値 */ (float)nxt_motor_get_count(NXT_PORT_C), /* 左モータ回転角度[deg] */ (float)nxt_motor_get_count(NXT_PORT_B), /* 右モータ回転角度[deg] */ (float)ecrobot_get_battery_voltage(), /* バッテリ電圧[mV] */ &pwm_L, /* 左モータPWM出力値 */ &pwm_R /* 右モータPWM出力値 */ ); nxt_motor_set_speed(NXT_PORT_C, pwm_L, 1); /* 左モータPWM出力セット(-100〜100) */ nxt_motor_set_speed(NXT_PORT_B, pwm_R, 1); /* 右モータPWM出力セット(-100〜100) */ }
/*!********************************************************* * @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 BalanceControl::calculation(char forward, char turn, float gyro, float gyro_offset, long motor_ang_l, long motor_ang_r){ if(! isInitialized) { // 倒立振子制御初期化 balance_init(); // 初期化済にする isInitialized = TRUE; } balance_control( (float)forward, (float)turn, (float)gyro, (float)gyro_offset, (float)motor_ang_l, (float)motor_ang_r, (float)ecrobot_get_battery_voltage(), (signed char*)&pwm_l, (signed char*)&pwm_r ); }
//走行体状態設定関数 void RN_modesetting(){ switch (runner_mode){ //走行体初期状態 case (RN_MODE_INIT): cmd_forward = 0; cmd_turn = 0; ecrobot_set_motor_rev(NXT_PORT_B,0); ecrobot_set_motor_rev(NXT_PORT_C,0); nxt_motor_set_speed(NXT_PORT_C, 0, 1); nxt_motor_set_speed(NXT_PORT_B, 0, 1); 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 batteryUpdate() { batterynow = ecrobot_get_battery_voltage(); }
//キャリブレーション関数 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_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 */ }