//走行体状態設定関数
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;
}
}
/*!*********************************************************
* @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);
}
}
/*!*********************************************************
* @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;
}
/*!*********************************************************
* @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
*
**********************************************************/
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;
}
//走行方法設定
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);
}
/**
* バランサの値を更新する
* @param angle 角速度
* @param rwEnc 右車輪エンコーダ値
* @param lwEnc 左車輪エンコーダ値
* @param battety バッテリ電圧値
*/
void Balancer::update(int angle, int rwEnc, int lwEnc, int battery) {
// 倒立振子制御APIを呼び出し、倒立走行するための
// 左右モータ出力値を得る
balance_control(
static_cast<float>(mForward),
static_cast<float>(mTurn),
static_cast<float>(angle),
static_cast<float>(mOffset),
static_cast<float>(cancelBacklash(mLeftPwm, lwEnc)),
static_cast<float>(cancelBacklash(mRightPwm, rwEnc)),
static_cast<float>(battery),
&mLeftPwm,
&mRightPwm);
}
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 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){ //べーす３００
// 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 setSoukouTouritsu( Command* cmd , ROBOT_STATUS* status )
{
signed char left;
signed char right;
signed char tail;
f4 f4t_yp;
f4 f4t_yd;
f4 f4t_turn;
f4 f4t_forward = 0;
int s4t_distance;
int s4t_disdiff;
unsigned short u2t_light;
f4 f4t_min;
tail = 0;
/* user logic start */
u1s_2msflipcount = 1 - u1s_2msflipcount;
if ( u1s_2msflipcount == 1 )
{
f4t_yp = (f4)(GRAY_WHITE - status->lightval );
f4t_yd = (f4t_yp - f4s_last_yp) / 0.004;
f4s_last_yp = f4t_yp;
f4t_turn = (f4)(TRACE_P * f4t_yp) + (f4)(TRACE_D * f4t_yd);
s4t_distance = ( status->motorangle_L + status->motorangle_R ) >> 1;
f4t_forward = f4s_forward;
// f4t_forward += 0.1;
if ( f4t_forward < FORWARD_MIN )
{
f4t_min = FORWARD_MIN_SHORT;
}
else
{
f4t_min = FORWARD_MIN;
}
if ( s4t_distance < 2500 ) //2000
{
f4t_forward += 0.1;
}
else
{
if ( s4t_distance < 4000)
{
f4t_forward -= 0.1;
}
else
{
if ( ( s4t_distance > 7300 )
&& ( u1s_judge == 0 ) )
{
s4s_distance = s4t_distance;
u1s_judge = 1;
}
else
{
if ( u1s_judge == 2 )
{
f4t_forward += 0.15;
}
else
{
f4t_forward += 0.1;
}
}
}
}
if ( u1s_judge == 1 )
{
f4t_forward -= 0.2;
f4t_min = FORWARD_MIN_SHORT;
f4t_turn = 32;
s4t_disdiff = (int)(s4t_distance - s4s_distance);
u2t_light = status->lightval;
if( s4t_disdiff > 700)
{
f4t_turn = 5;
}
if( ( s4t_disdiff > 750 )
&& ( u2t_light > ( GRAY_WHITE + 10 ) ) )
{
u1s_judge = 2;
}
}
if( s4t_distance > 13000 )
{
f4t_forward -= 0.3;
f4t_min = 10;
}
if ( f4t_forward > FORWARD_MAX )
{
f4t_forward = FORWARD_MAX;
}
if ( f4t_forward < f4t_min )
{
f4t_forward = f4t_min;
}
f4s_forward = f4t_forward;
balance_control(
(f4)f4t_forward, /* 前後進命令(+:前進, -:後進) */
(f4)-f4t_turn, /* 旋回命令(+:右旋回, -:左旋回) */
(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出力値 */
);
tail = tailControl( 30 , status->motorangle_T );
setMotorVal( left , right , tail );
}
//*****************************************************************************
// 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 */
}
