void show(int distance)
{
display_goto_xy(0, 0);
display_string("DISTANCE :");
display_goto_xy(0, 1);
display_unsigned(distance,10);
display_update();
}
void display_bios_status(int data, int max_data)
{
static U8 first_flag = 1;
if (first_flag)
{
display_clear(0);
display_goto_xy(0, 0);
display_string(VERSION);
display_goto_xy(0, 1);
display_string("================");
}
display_goto_xy(0, 2);
display_string("BATT:");
display_unsigned(battery_voltage()/100, 0);
if (first_flag)
{
display_goto_xy(0, 3);
display_string("UPLOAD: READY ");
first_flag = 0;
}
else
{
if (data >= UPLOAD_IN_PROGRESS)
{
display_goto_xy(0, 3);
display_string("UPLOAD: PROGRESS");
display_progress_bar(4, data, max_data);
}
else if (data == UPLOAD_FAILED)
{
display_goto_xy(0, 3);
display_string("UPLOAD: FAILED ");
}
else if (data == UPLOAD_FINISHED)
{
display_goto_xy(0, 3);
display_string("UPLOAD: FINISHED");
/* clear progress bar */
display_progress_bar(4, 0, 100);
}
}
display_update();
}
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 */
}
