//PID制御ライントレース関数 void RA_linetrace_PID(int forward_speed) { RA_speed(forward_speed,1); //速度を段階的に変化 if(forward_speed > 0) hensa = (float)GRAY_VALUE - (float)ecrobot_get_light_sensor(NXT_PORT_S3); else hensa = (float)ecrobot_get_light_sensor(NXT_PORT_S3) - (float)GRAY_VALUE; i_hensa = i_hensa+(hensa*0.0005); d_hensa = (hensa - bf_hensa)/0.0005; bf_hensa = hensa; cmd_turn = -(Kp * hensa + Ki * i_hensa + Kd * d_hensa); if (-100 > cmd_turn) { cmd_turn = -100; } else if (100 < cmd_turn) { cmd_turn = 100; } /*倒立制御OFF時*/ //nxt_motor_set_speed(NXT_PORT_C, forward_speed + cmd_turn/2, 1); //nxt_motor_set_speed(NXT_PORT_B, forward_speed - cmd_turn/2, 1); }
float PIDControl_PIDLineTrace(PIDControl *this_PIDControl,int forwardSpeed){ //PID制御用偏差値 static float hensa; //P制御用 static float i_hensa = 0; //I制御用 static float d_hensa = 0; //D制御用 static float bf_hensa = 0; ControlVals controlVals; if(forwardSpeed > 0) hensa = (float)Calibration_getGrayValue(&mCalibration) - (float)ecrobot_get_light_sensor(NXT_PORT_S3); else hensa = (float)ecrobot_get_light_sensor(NXT_PORT_S3) - (float)Calibration_getGrayValue(&mCalibration); i_hensa = i_hensa+(hensa*EXECUTE_CYCLE); d_hensa = (hensa - bf_hensa)/EXECUTE_CYCLE; bf_hensa = hensa; controlVals.turn_val = -( this_PIDControl->Kp * hensa + this_PIDControl->Ki * i_hensa + this_PIDControl->Kd * d_hensa); if (-100 > controlVals.turn_val) { controlVals.turn_val = -100; } else if (100 < controlVals.turn_val) { controlVals.turn_val = 100; } return controlVals.turn_val; }
/* 光センサ値の急激な上昇下降を検知するやつ */ signed char LVTrigger(){ signed int LV_def = ecrobot_get_light_sensor(NXT_PORT_S3) - LV_buf; LV_buf = ecrobot_get_light_sensor(NXT_PORT_S3); if(LV_def >= 5){ return 1; } else if(LV_def <= -5){ return -1; } else{ return 0; } }
/*!********************************************************* * @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; }
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet... static void decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors ) { U8 temp_port; output_packet->port = input_packet->port; output_packet->operation = input_packet->operation; switch (input_packet->operation) { case LIGHT_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_light_sensor(temp_port); break; case TOUCH_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_touch_sensor(temp_port); break; case SOUND_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sound_sensor(temp_port); break; case RADAR_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port); break; case TACHO_COUNT: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) nxt_motor_get_count(temp_port); break; case SET_POWER: decode_bro_port (input_packet->port, &temp_port); switch (temp_port) { case NXT_PORT_A: motors->first.speed_control_type = RAW_POWER; motors->first.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(NXT_PORT_A); output_packet->timestamp = (float) systick_get_ms(); break; case NXT_PORT_B: motors->second.speed_control_type = RAW_POWER; motors->second.powers[0] = input_packet->data; break; case NXT_PORT_C: motors->third.speed_control_type = RAW_POWER; motors->third.powers[0] = input_packet->data; break; }; break; default: //Nothing HERE break; }; }
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4, S32 datas32, S32 datas32_2) { static U8 data_log_buffer[32]; //左辺は結局data_log_bufferの中身を表している。要するにただの代入 //32ビット分アクセスするためにdata_log_bufferの[1]から[3]へはアクセスしてない。 *((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms(); *(( S8 *)(&data_log_buffer[4])) = (S8)data1; *(( S8 *)(&data_log_buffer[5])) = (S8)data2; *((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_get_light_sensor(NXT_PORT_S3); //light sensor value *((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0); *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1); *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2); *((S16 *)(&data_log_buffer[20])) = (S16)adc1; *((S16 *)(&data_log_buffer[22])) = (S16)adc2; *((S16 *)(&data_log_buffer[24])) = (S16)adc3; *((S16 *)(&data_log_buffer[26])) = (S16)adc4; *((S32 *)(&data_log_buffer[28])) = (U32)datas32_2; ecrobot_send_bt_packet(data_log_buffer, 32); }
/*!********************************************************* * @brief * * @param * * @retval * * @return * **********************************************************/ static U32 garage_trace( void ) { U32 ret = RET_REMAIN; F32 forward; F32 turn; F32 u_turn; U16 lightness; static U32 cnt = 0; lightness = ecrobot_get_light_sensor(PORT_LIGHT); garage_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; } cnt++; if (cnt > 250) { /* 旋回量から前進量を調整する */ if (u_turn < 30) { forward = 100; } else if (u_turn < 50) { forward = 90; } else { forward = 85; } /* 倒立振子制御 */ garage_stand(forward, turn, GYRO_OFFSET); /* 灰色検知 */ if (lightness > 610) { nxt_motor_set_count(PORT_MOTOR_TAIL, 0); nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)10, 1); gkGarageInfo.motor_r = nxt_motor_get_count(PORT_MOTOR_R); gkGarageInfo.motor_l = nxt_motor_get_count(PORT_MOTOR_L); ret = RET_FINISH; } else { /* nothing */ } } else { forward = 50; /* 倒立振子制御 */ garage_stand(forward, turn, GYRO_OFFSET); } return ret; }
//***************************************************************************** // 関数名 : calibration // 引数 : *black (黒、最小値),*white(白、最大値) // 返り値 : 無し // 概要 : 光センサの手動キャリブレーション // 黒白の順でタッチする。 //***************************************************************************** void calibration(int *black,int *white,int angle) { while(1) { tail_control(angle); if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) { ecrobot_sound_tone(440, 170, 100); *black = ecrobot_get_light_sensor(NXT_PORT_S3); display_clear(0); /* 画面表示 */ display_goto_xy(0, 1); display_string("BLACK:"); display_int(*black, 4); break; }//if systick_wait_ms(100); /* 10msecウェイト */ }//while display_update(); while(ecrobot_get_touch_sensor(NXT_PORT_S4)); ecrobot_sound_tone(880, 170, 100); while(1) { tail_control(angle); if (ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) { ecrobot_sound_tone(880, 170, 100); *white = ecrobot_get_light_sensor(NXT_PORT_S3); //display_clear(0); /* 画面表示 */ display_goto_xy(0, 2); display_string("WHITE:"); display_int(*white, 4); break; }//if systick_wait_ms(100); /* 10msecウェイト */ }//while //display_clear(0); /* 画面表示 */ display_goto_xy(0,4); display_string("TH:"); display_int(TH(*black,*white), 3); display_update(); while(ecrobot_get_touch_sensor(NXT_PORT_S4)); ecrobot_sound_tone(440, 170, 100); }
//ON-OFF制御用ライン判定関数 int online(void) { int light_value; light_value = ecrobot_get_light_sensor(NXT_PORT_S3); //現在の輝度値 if (GRAY_VALUE > light_value) //輝度値が目標値より大きいか判断 return FALSE; //ライン外 else return TRUE; //ライン内 }
//P制御ライントレース関数 void RA_linetrace_P(int forward_speed){ cmd_forward = forward_speed; hensa = (float)GRAY_VALUE - (float)ecrobot_get_light_sensor(NXT_PORT_S3); cmd_turn = -(1.4 * hensa); if (-100 > cmd_turn) { cmd_turn = -100; } else if (100 < cmd_turn) { cmd_turn = 100; } }
//走行状態設定関数 void RN_setting(){ static unsigned char buf = 0; if (ecrobot_get_touch_sensor(NXT_PORT_S4)){ buf = 1; } if (!ecrobot_get_touch_sensor(NXT_PORT_S4) && buf){ runner_mode = RN_MODE_INIT; setting_mode = RN_SETTINGMODE_START; } switch (setting_mode){ //キャリブレーション状態 case (RN_SETTINGMODE_START): RN_calibrate(); LV_buf = ecrobot_get_light_sensor(NXT_PORT_S3); break; case (RN_SPEEDZERO): cmd_forward = 0; cmd_turn = RA_wheels(cmd_turn); wait_count++; if(wait_count >= 200){ setting_mode = RN_RUN; wait_count = 0; } break; //通常走行状態 case (RN_RUN): RA_linetrace_PID_balanceoff(SPEED_COUNT); cmd_turn += RA_curvatureCtrl_PID(trgt_R); wait_count++; break; // 曲率PID case (RN_CVRUN): cmd_forward = SPEED_COUNT; cmd_turn = RA_curvatureCtrl_PID(trgt_R); wait_count++; break; case (RN_LOT90): cmd_turn = 0; cmd_turn = RA_directionCtrl_PID(90); wait_count++; break; default: break; } }
//ON-OFF制御用ライン判定関数 int online(void) { int light_value; light_value = ecrobot_get_light_sensor(NXT_PORT_S3); if (GRAY_VALUE > light_value) { if ((GRAY_VALUE) > light_value) { return FALSE; } else { return TRUE; } } return TRUE; }
//***************************************************************************** // 関数名 : line_follow2 // 引数 : Black (黒のセンサ値) // 引数 : white (白のセンサ値) // 返り値 : 無し // 概要 : バランサーを使用しないライントレース //***************************************************************************** static void line_follow2(int speed, int black, int white) { int pwm_L, pwm_R, turn2; turn2 = KP * (ecrobot_get_light_sensor(NXT_PORT_S3) - TH2(black, white)); if (turn2 > 50) turn = 50; if (turn2 < -50) turn = -50; pwm_L = speed - turn2; pwm_R = speed + turn2; if (pwm_L > 100) pwm_L = 100; if (pwm_L < -100) pwm_L = -100; if (pwm_R > 100) pwm_R = 100; if (pwm_R < -100) pwm_R = -100; 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) */ //xsprintf(tx_buf,"lf2:turn2=%d,%d\n",turn2,ecrobot_get_light_sensor(NXT_PORT_S3)); //ecrobot_send_bt(tx_buf,0,strlen(tx_buf)); }
//bluetoothログ送信関数 void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4){ U8 data_log_buffer[32]; *((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms(); *(( S8 *)(&data_log_buffer[4])) = (S8)data1; *(( S8 *)(&data_log_buffer[5])) = (S8)data2; *((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_get_light_sensor(NXT_PORT_S3); *((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0); *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1); *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2); *((S16 *)(&data_log_buffer[20])) = (S16)adc1; *((S16 *)(&data_log_buffer[22])) = (S16)adc2; *((S16 *)(&data_log_buffer[24])) = (S16)adc3; *((S16 *)(&data_log_buffer[26])) = (S16)adc4; *((S32 *)(&data_log_buffer[28])) = (S32)ecrobot_get_sonar_sensor(NXT_PORT_S2); ecrobot_send_bt_packet(data_log_buffer, 32); }
//走行状態設定関数 void RN_setting(){ switch (setting_mode){ //キャリブレーション状態 case (RN_SETTINGMODE_START): RN_calibrate(); LV_buf = ecrobot_get_light_sensor(NXT_PORT_S3); break; case (RN_SPEEDZERO): cmd_forward = 0; cmd_turn = RA_wheels(cmd_turn); wait_count++; if(wait_count >= 200){ setting_mode = RN_RUN; wait_count = 0; } break; //通常走行状態 case (RN_RUN): RA_linetrace_PID(SPEED_COUNT); wait_count++; break; // 曲率PID case (RN_CVRUN): cmd_forward = SPEED_COUNT; cmd_turn = RA_curvatureCtrl_PID(trgt_R); wait_count++; break; case (RN_LOT90): cmd_turn = 0; cmd_turn = RA_directionCtrl_PID(90); wait_count++; break; default: break; } }
//***************************************************************************** // 関数名 : line_follow3 // 引数 : Black (黒のセンサ値) // 引数 : white (白のセンサ値) // 返り値 : 無し // 概要 : バランサーを使用しないライントレース //***************************************************************************** static void line_follow3(int speed, int black, int white) { int pwm_L, pwm_R; int turn2 = 10; if(ecrobot_get_light_sensor(NXT_PORT_S3) > TH2(black, white)){ pwm_R = speed + turn2; pwm_L = speed; }else{ pwm_R = speed; pwm_L = speed + turn2; } if (pwm_L > 100) pwm_L = 100; if (pwm_L < -100) pwm_L = -100; if (pwm_R > 100) pwm_R = 100; if (pwm_R < -100) pwm_R = -100; 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) */ //xsprintf(tx_buf,"lf2:turn2=%d,%d\n",turn2,ecrobot_get_light_sensor(NXT_PORT_S3)); //ecrobot_send_bt(tx_buf,0,strlen(tx_buf)); }
/*!********************************************************* * @brief * * @param * * @retval * * @return * *********************************************************/ static U32 seesaw_forward( void ) { U32 ret = RET_REMAIN; U16 lightness; F32 turn; static int motor_r = 0; static int motor_l = 0; int cur_motor_r; int cur_motor_l; lightness = ecrobot_get_light_sensor(PORT_LIGHT); turn = (0.2) * (530 - lightness); seesaw_stand((F32)15, (F32)turn, GYRO_OFFSET + 2); //seesaw_stand((F32)1, (F32)0, GYRO_OFFSET); if (motor_r == 0) { motor_r = nxt_motor_get_count(PORT_MOTOR_R); motor_l = nxt_motor_get_count(PORT_MOTOR_L); nxt_motor_set_count(PORT_MOTOR_TAIL, 0); } else { ///* 尻尾制御 */ //tail = nxt_motor_get_count(PORT_MOTOR_TAIL); //if (tail < SEESAW_TAIL_ANGLE_BLAKE) { // nxt_motor_set_speed(PORT_MOTOR_TAIL, 20, 1); //} else { // nxt_motor_set_speed(PORT_MOTOR_TAIL, 0, 1); //} cur_motor_r = nxt_motor_get_count(PORT_MOTOR_R); cur_motor_l = nxt_motor_get_count(PORT_MOTOR_L); if (cur_motor_r - motor_r > 900 && cur_motor_l - motor_l > 900) { ret = RET_FINISH; } else { /* nothing */ } } return ret; }
// ‹P“x‚𓾂é U16 LightSensor::getBrightness(){ return ecrobot_get_light_sensor(inputPort); }
void monitorVariation() { static int counter; ++counter; if(counter==240/4){//約20ms calLightSensorVarieation(); switch (detectLineState) { case OnLine : //ラインアウトしたら if(lightSensorVarieation < DETECTLINEOUT_THRESHOLD ) { setDetectLineState(OutOfLine); ecrobot_sound_tone(880, 512, 30); systick_wait_ms(500); } else if(lightSensorVarieation > DETECTLINE_IN_THRESHOLD) { setDetectLineState(OnLine); } //OnLine状態なのに、光センサの値が白色付近だったら。 else if((ecrobot_get_light_sensor(NXT_PORT_S3) > getWhiteValue() -20 && ecrobot_get_light_sensor(NXT_PORT_S3) < getWhiteValue() +20 )) { setDetectLineState(OnLine); ecrobot_sound_tone(440, 512, 30); systick_wait_ms(500); } break; case OutOfLine : //ラインに乗ったら if(lightSensorVarieation > DETECTLINE_IN_THRESHOLD) { setDetectLineState(OnLine); ecrobot_sound_tone(440, 512, 30); systick_wait_ms(500); } else if(lightSensorVarieation < DETECTLINEOUT_THRESHOLD ) { setDetectLineState(OutOfLine); } //OutOfLine状態なのに値が目標値付近だったら else if((ecrobot_get_light_sensor(NXT_PORT_S3) > getLineMokuhyouValue() -20 && ecrobot_get_light_sensor(NXT_PORT_S3) < getLineMokuhyouValue() +20 )) { setDetectLineState(OnLine); ecrobot_sound_tone(440, 512, 30); systick_wait_ms(500); } break; default : //none break; } /* //ラインアウトしたら if(lightSensorVarieation < DETECTLINEOUT_THRESHOLD ) { switch (detectLineState) { case OnLine : setDetectLineState(OutOfLine); ecrobot_sound_tone(880, 512, 30); systick_wait_ms(500); break; case OutOfLine : setDetectLineState(OutOfLine); break; default : //none break; } } //ラインを検知したら else if(lightSensorVarieation > DETECTLINE_IN_THRESHOLD ) { switch (detectLineState) { case OnLine : setDetectLineState(OnLine); break; case OutOfLine : setDetectLineState(OnLine); ecrobot_sound_tone(440, 512, 30); systick_wait_ms(500); break; default : //none break; } } else if (ecrobot_get_light_sensor(NXT_PORT_S3) -20 > mokuhyouti && ecrobot_get_light_sensor(NXT_PORT_S3)+20 < mokuhyouti) { switch (detectLineState) { case OnLine : setDetectLineState(OnLine); break; case OutOfLine : setDetectLineState(OnLine); ecrobot_sound_tone(440, 512, 30); systick_wait_ms(500); break; default : //none break; } } */ counter = 0; } }
//キャリブレーション関数 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(){ tail_mode_change(0,ANGLEOFDOWN,1,2); //黒値 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 += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1); systick_wait_ms(500); break; } } //走行開始合図 while(1){ //リモートスタート if(remote_start()==1){ ecrobot_sound_tone(982,512,30); tail_mode_change(1,ANGLEOFUP,0,2); setting_mode = RN_SPEEDZERO; 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_SPEEDZERO; runner_mode_change(1); tail_mode_change(1,ANGLEOFUP,0,2); break; } } break; } } }
void lightUpdate() { lightnow = ecrobot_get_light_sensor(NXT_PORT_S3); }
void calLightSensorVarieation() { lightSensorVarieation = (float)ecrobot_get_light_sensor(NXT_PORT_S3) - BufLightSensorVal; //変化量の計算 BufLightSensorVal = (float)ecrobot_get_light_sensor(NXT_PORT_S3); //一回前のライトセンサの変化量の保存 }
//キャリブレーション 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; } } //キャリブレーション終了 }
void Light_sensor::input() { brightness=ecrobot_get_light_sensor(port); }
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet... static void decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors) { U8 temp_port; output_packet->port = input_packet->port; output_packet->operation = input_packet->operation; switch (input_packet->operation) { case LIGHT_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_light_sensor(temp_port); break; case TOUCH_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_touch_sensor(temp_port); break; case SOUND_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sound_sensor(temp_port); break; case RADAR_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port); break; case TACHO_COUNT: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) nxt_motor_get_count(temp_port); break; case SET_POWER: decode_bro_port (input_packet->port, &temp_port); switch (temp_port) { case NXT_PORT_A: motors->first.speed_control_type = RAW_POWER; motors->first.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; case NXT_PORT_B: motors->second.speed_control_type = RAW_POWER; motors->second.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; case NXT_PORT_C: motors->third.speed_control_type = RAW_POWER; motors->third.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; }; break; case SET_SPEED: motors->first.speed_control_type = PID_CONTROLLED; motors->second.speed_control_type = PID_CONTROLLED; motors->first.speed_ref[setSpeedCounter] = input_packet->data; motors->second.speed_ref[setSpeedCounter] = input_packet->data; if(setSpeedCounter == 0){ motors->first.distance_ref[setSpeedCounter] = input_packet->data2; motors->second.distance_ref[setSpeedCounter] = input_packet->data2; }else{ motors->first.distance_ref[setSpeedCounter] = motors->first.distance_ref[setSpeedCounter -1] + input_packet->data2; motors->second.distance_ref[setSpeedCounter] = motors->second.distance_ref[setSpeedCounter -1] + input_packet->data2; } setSpeedCounter++; break; default: break; }; }