//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;
};
}