//後始末処理 void ecrobot_device_terminate(void) { tail_mode = RN_TAILUP; ecrobot_set_motor_speed(NXT_PORT_A, 0); ecrobot_set_motor_speed(NXT_PORT_B, 0); ecrobot_set_motor_speed(NXT_PORT_C, 0); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); ecrobot_term_sonar_sensor(NXT_PORT_S2); ecrobot_term_bt_connection(); }
//初期処理 void ecrobot_device_initialize(void) { ecrobot_set_light_sensor_active(NXT_PORT_S3); ecrobot_init_bt_slave("LEJOS-OSEK"); ecrobot_set_motor_rev(NXT_PORT_A,0); ecrobot_set_motor_rev(NXT_PORT_B,0); ecrobot_set_motor_rev(NXT_PORT_C,0); ecrobot_set_motor_speed(NXT_PORT_A,0); ecrobot_set_motor_speed(NXT_PORT_B,0); ecrobot_set_motor_speed(NXT_PORT_C,0); }
//初期処理関数(プログラムの最初に呼び出し) void ecrobot_device_initialize(void){ ecrobot_set_light_sensor_active(NXT_PORT_S3); //光センサ起動 ecrobot_init_bt_slave("LEJOS-OSEK"); //Bluetooth起動 ecrobot_init_sonar_sensor(NXT_PORT_S2); //超音波センサ起動 //モータリセット ecrobot_set_motor_rev(NXT_PORT_A,0); ecrobot_set_motor_rev(NXT_PORT_B,0); ecrobot_set_motor_rev(NXT_PORT_C,0); ecrobot_set_motor_speed(NXT_PORT_A,0); ecrobot_set_motor_speed(NXT_PORT_B,0); ecrobot_set_motor_speed(NXT_PORT_C,0); }
//後始末処理関数(プログラム終了時呼び出し) void ecrobot_device_terminate(void) { ecrobot_set_light_sensor_inactive(NXT_PORT_S3); //光センサ終了 ecrobot_term_bt_connection(); //Bluetooth終了 //モータリセット ecrobot_set_motor_rev(NXT_PORT_A,0); ecrobot_set_motor_rev(NXT_PORT_B,0); ecrobot_set_motor_rev(NXT_PORT_C,0); ecrobot_set_motor_speed(NXT_PORT_A, 0); ecrobot_set_motor_speed(NXT_PORT_B, 0); ecrobot_set_motor_speed(NXT_PORT_C, 0); }
//後始末処理関数(プログラム終了時呼び出し) void ecrobot_device_terminate(void){ tail_mode = RN_TAILUP; //尻尾を上げる(意味あるのか?) ecrobot_set_light_sensor_inactive(NXT_PORT_S3); //光センサ終了 ecrobot_term_sonar_sensor(NXT_PORT_S2); //超音波センサ終了 ecrobot_term_bt_connection(); //Bluetooth終了 //モータリセット ecrobot_set_motor_rev(NXT_PORT_A,0); ecrobot_set_motor_rev(NXT_PORT_B,0); ecrobot_set_motor_rev(NXT_PORT_C,0); ecrobot_set_motor_speed(NXT_PORT_A, 0); ecrobot_set_motor_speed(NXT_PORT_B, 0); ecrobot_set_motor_speed(NXT_PORT_C, 0); }
//尻尾角度維持 void taildown(){ static const float t_Kp = 0.7; static float t_hensa = 0; static float t_speed = 0; switch(tail_mode){ case(RN_TAILDOWN): t_hensa = ANGLEOFDOWN - ecrobot_get_motor_rev(NXT_PORT_A); break; case(RN_TAILUP): t_hensa = ANGLEOFUP - ecrobot_get_motor_rev(NXT_PORT_A); break; case(RN_TAILPUSH): t_hensa = ANGLEOFPUSH - ecrobot_get_motor_rev(NXT_PORT_A); break; default: break; } t_speed = t_Kp*t_hensa; if (t_speed < -100) t_speed = -100; else if (t_speed > 100) t_speed = 100; ecrobot_set_motor_speed(NXT_PORT_A, t_speed); }
//尻尾角度調節関数 void taildown(){ //X-ecrobot_get_motor_rev(NXT_PORT_S4) のX = 目標値 //目標値を1ずつ調節(現行目標値t_angleそのものを最終目標値t_valueへ近づけていく) //t_angleの初期値は現在の角度 static float t_hensa = 0; //尻尾角度の目標値との差 static float t_ihensa = 0; //I制御用偏差 static float t_speed = 0; //尻尾モータに送る速度 t_count++; //速度制御用カウンタ switch(tail_mode){ case(RN_TAILDOWN): //尻尾を下げる if(t_angle <= t_value){ //現在の角度が目標値以下かどうか t_hensa = t_angle - ecrobot_get_motor_rev(NXT_PORT_A); if(t_count >= t_count_limit){ //カウンタで尻尾を下げる速度を調整 t_angle+=t_up; //角度を上げる t_count = 0; } } else{ t_angle = t_value; } break; case(RN_TAILUP): //尻尾を上げる if(t_angle >= t_value){ //現在の角度が目標値以上かどうか t_hensa = t_angle - ecrobot_get_motor_rev(NXT_PORT_A); if(t_count >= t_count_limit){ //カウンタで尻尾を上げる速度を調整 t_angle-=t_up; //角度を下げる t_count = 0; } } else{ t_angle = t_value; } break; default: break; } t_ihensa = t_ihensa+(t_hensa*0.0004); //I制御用偏差 t_speed = (t_Kp*t_hensa + t_Ki*t_ihensa); //偏差を元に速度調節 if (t_speed < -100) t_speed = -100; else if (t_speed > 100) t_speed = 100; ecrobot_set_motor_speed(NXT_PORT_A, t_speed); //モータに速度を送る }
//尻尾角度コントロール関数 void tailcontrol(){ static const float t_Kp = 3.2; // static const float t_Ki = 3.3; // static const float t_Kd = 1.5; static float t_hensa = 0; /* static float t_i_hensa = 0; static float t_d_hensa = 0; static float t_bf_hensa = 0; */ static float t_speed = 0; switch(tail_mode){ case(RN_TAILDOWN): t_hensa = ANGLEOFDOWN - ecrobot_get_motor_rev(NXT_PORT_A); //尻尾を下げる break; case(RN_TAILUP): t_hensa = ANGLEOFUP - ecrobot_get_motor_rev(NXT_PORT_A); //尻尾を上げる break; case(RN_TAILPUSH): t_hensa = ANGLEOFPUSH - ecrobot_get_motor_rev(NXT_PORT_A); //尻尾を使って走行体を跳ね上げる break; case(RN_TAILSTAND): t_hensa = ANGLEOFSTAND - ecrobot_get_motor_rev(NXT_PORT_A); break; default: break; } /* t_i_hensa = t_i_hensa+(t_hensa*0.004); t_d_hensa = (t_hensa - t_bf_hensa)/0.004; t_bf_hensa = t_hensa; */ t_speed = t_Kp*t_hensa; if (t_speed < -100) t_speed = -100; else if (t_speed > 100) t_speed = 100; ecrobot_set_motor_speed(NXT_PORT_A, t_speed); }
/* *後始末処理 */ void ecrobot_device_terminate(void) { ecrobot_set_motor_speed(NXT_PORT_B, 0); ecrobot_set_motor_speed(NXT_PORT_C, 0); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); }
void ecrobot_device_terminate(){ ecrobot_set_motor_speed(NXT_PORT_A, 0); ecrobot_set_motor_speed(NXT_PORT_C, 0); }
//走行設定関数 void RN_setting() { static int step_count = 0; static int time_count = 0; //距離計測用変数 int distance_before_step = 0; int distance_step_brake = 0; int distance_step_stop = 0; int distance_gyro_up = 0; int distance_over_forty = 0; int distance_turn_clear = 0; int distance_turn_after = 0; static unsigned int angle_l_now = 0; static unsigned int angle_r_now = 0; switch (setting_mode){ case (TYREAL) : do_tyreal(&Kp,&Ki,&Kd); if(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) { systick_wait_ms(500); setting_mode = RN_SETTINGMODE_START; } break; //走行開始前 case (RN_SETTINGMODE_START): RN_calibrate(); //キャリブレーション ecrobot_set_motor_speed(NXT_PORT_A,0); ecrobot_set_motor_speed(NXT_PORT_B,0); ecrobot_set_motor_speed(NXT_PORT_C,0); break; case (RN_RUN): RA_linetrace_PID(15); setting_mode = RN_STEP_TURN_START; break; case (RN_STEP_TURN_START): time_count++; RA_linetrace_PID(15); if(lightnow < RIGHT_ANGLE_LIGHT_VALUE && time_count > 300) { ecrobot_sound_tone(880, 512, 30); setting_mode = RN_STEP_TURN_LEFT; time_count = 0; } break; //直角カーブ case (RN_STEP_TURN_LEFT): cmd_forward = 0; cmd_turn = 0; if(time_count == 0) { angle_l_now = ecrobot_get_motor_rev(NXT_PORT_B); angle_r_now = ecrobot_get_motor_rev(NXT_PORT_C); } time_count++; if(ecrobot_get_motor_rev(NXT_PORT_B) - angle_l_now <= RIGHT_ANGLE_AIM) { /* 回転する */ cmd_turn = -100; } else { /* 止まる */ time_count = 0; setting_mode = RN_STEP_TURN_FORWARD; } break; case (RN_STEP_TURN_FORWARD): RA_linetrace_PID(20); if(GYRO_OFFSET_INIT - 50 > gyronow || GYRO_OFFSET_INIT + 50 < gyronow && time_count > 200) { //GYRO_OFFSET_INIT += 7; setting_mode = RN_STOP; } break; //強制停止 case(RN_STOP): cmd_forward = 10; cmd_turn = 0; //nxt_motor_set_speed(NXT_PORT_C, 0, 1); //nxt_motor_set_speed(NXT_PORT_B, 0, 1); break; default: break; } }