//衝撃検知関数
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 */
}
