//キャリブレーション関数
void RN_calibrate()
{
/*バランサーON用*/
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(ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE)
{
ecrobot_sound_tone(982,512,10);
setting_mode = RN_RUN;
runner_mode = RN_MODE_BALANCE;
systick_wait_ms(500);
break;
}
}
}
int Calibration_calibration(Calibration *this_Calibration){
static U32 avg_cnt = 0;
static U32 cal_start_time;
//gyro_offset
while (1){
if(PushButton_detect_push_button(&pushButton) == TRUE)break;
}
ecrobot_sound_tone(880, 512, 10);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
while((ecrobot_get_systick_ms() - cal_start_time) < 1000U){
/* ジャイロセンサの設定をする */
this_Calibration->gyro += InclinationEncoder_get_inclination(&inclinationEncoder);
avg_cnt++;
}
this_Calibration->gyro /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 10);
systick_wait_ms(500);
//black
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_black(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
//white
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_white(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
InclinationEncoder_set_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
InclinationEncoder_set_initial_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
systick_wait_ms(500);
break;
}
}
//キャリブレーションが終了したら1を返す
return 1;
}
static int check_marker(int turn)
{
static int r=0,l=0;
if(turn < -20) {
r++;
} else {
if (turn > 0) {
r = 0;
}
}
if(turn > 20) {
l++;
} else {
if (turn < 0) {
l = 0;
}
}
//xsprintf(tx_buf,"%4d, %4d\n",turn,tripmeter());
//ecrobot_send_bt(tx_buf,1, 12);
if (r >= 5) {
r = 0;
ecrobot_sound_tone(440*4 , 10, 100);
return 1; //右エッジ走行のときはマーカースタート
}
if (l >= 5) {
l = 0;
//ecrobot_sound_tone(440*2 , 10, 100);
return -1;
}
return 0;
}
void RN_set_ok_end()
{
/* バンパを離すと次の状態に遷移する(設定モードを終了する）*/
if (ecrobot_get_touch_sensor(NXT_PORT_S4) != TRUE) {
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SETTINGMODE_END;
}
}
void do_tyreal(float *Kp_t,float *Ki_t,float *Kd_t){
//�ó‘Ô‘JˆÚ
if (ecrobot_is_RUN_button_pressed() == 1) {
ecrobot_sound_tone(932, 512, VOL);
systick_wait_ms(100);
ecrobot_sound_tone(466, 256, VOL);
systick_wait_ms(10);
systick_wait_ms(10);
if(adjust_param_state==ADJUST_Kp_VAL){
adjust_param_state=ADJUST_Ki_VAL;
}
else if(adjust_param_state==ADJUST_Ki_VAL){
adjust_param_state=ADJUST_Kd_VAL;
}
else if(adjust_param_state==ADJUST_Kd_VAL){
adjust_param_state=ADJUST_Kp_VAL;
}
}
switch(adjust_param_state) {
case (ADJUST_Kp_VAL): *Kp_t=change_float_param(*Kp_t);
break;
case (ADJUST_Ki_VAL): *Ki_t=change_float_param(*Ki_t);
break;
case (ADJUST_Kd_VAL): *Kd_t=change_float_param(*Kd_t);
break;
default:
break;
}
display_clear(1);
float_to_string(*Kp_t,float_string);
display_show_string("Kp",0,0);
display_show_string(float_string,0,1);
float_to_string(*Ki_t,float_string);
display_show_string("Ki",0,2);
display_show_string(float_string,0,3);
float_to_string(*Kd_t,float_string);
display_show_string("Kd",0,4);
display_show_string(float_string,0,5);
}
void RN_set_ok()
{
/* スタート位置にロボットを置き、バンパを押すと次の状態に遷移する。 */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) {
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SETTINGMODE_OK_END;
}
}
void emergency_stop(char* errMsgLine1, char* errMsgLine2) {
motor_set_speed(NXT_PORT_A,0,1);
motor_set_speed(NXT_PORT_B,0,1);
motor_set_speed(NXT_PORT_C,0,1);
print_clear_display();
print_str(0,1,errMsgLine1);
print_str(0,2,errMsgLine2);
int i;
for(i = 0; i < 3; i++) {
print_str(6,4,"ERROR");
print_update();
ecrobot_set_light_sensor_active(NXT_PORT_S1);
ecrobot_set_light_sensor_active(NXT_PORT_S2);
ecrobot_set_light_sensor_active(NXT_PORT_S3);
ecrobot_sound_tone(1000,500,15);
systick_wait_ms(750);
print_clear_line(4);
print_update();
ecrobot_set_light_sensor_inactive(NXT_PORT_S1);
ecrobot_set_light_sensor_inactive(NXT_PORT_S2);
ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
ecrobot_sound_tone(500,500,15);
systick_wait_ms(750);
}
motor_set_speed(NXT_PORT_A,0,0);
motor_set_speed(NXT_PORT_B,0,0);
motor_set_speed(NXT_PORT_C,0,0);
while(true) {
print_str(6,4,"ERROR");
print_update();
ecrobot_set_light_sensor_active(NXT_PORT_S1);
ecrobot_set_light_sensor_active(NXT_PORT_S2);
ecrobot_set_light_sensor_active(NXT_PORT_S3);
systick_wait_ms(750);
print_clear_line(4);
print_update();
ecrobot_set_light_sensor_inactive(NXT_PORT_S1);
ecrobot_set_light_sensor_inactive(NXT_PORT_S2);
ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
systick_wait_ms(750);
}
}
//*****************************************************************************
// 関数名 : 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);
}
void RN_set_gyro_start()
{
/* ジャイロセンサの設定を始める */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) {
ecrobot_sound_tone(880, 512, 30);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
setting_mode = RN_SETTINGMODE_GYRO;
}
}
int Section_clear_section(Section *self,int executed_flag)
{
if(executed_flag==1 &&
self->current_running_method_number+1 == self->number_of_running_method){
ecrobot_sound_tone(880,10,60);
return 1;
}
else {
return 0;
}
}
void RN_set_gyro()
{
/* ジャイロセンサの設定をする */
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
avg_cnt++;
/* 1秒経過したら、ジャイロセンサのオフセット値の平均値を計算し、次の状態に遷移する。 */
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U);
setting_mode = RN_SETTINGMODE_GYRO_END;
}
}
//衝撃検知
void shock(void){
if(min_vol>ecrobot_get_battery_voltage())
min_vol=ecrobot_get_battery_voltage();
if((ecrobot_get_gyro_sensor(NXT_PORT_S1) <= gyro_offset-PM_GYRO ||
ecrobot_get_gyro_sensor(NXT_PORT_S1) >= gyro_offset+PM_GYRO) &&
min_vol <= battery_value-DOWN_BATTERY)
{
ecrobot_sound_tone(880,512,30);
if(runner_mode == RN_MODE_CONTROL){
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
setting_mode = RN_SLOW_RUN;
runner_mode = RN_MODE_CONTROL_2;
gyro_offset += 10;
min_vol = battery_value;
}
}
}
//キャリブレーション関数
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;
}
}
}
static void Section_reset_encoder(Section *self){
Timer_reset(&timer);
DistanceEncoder_reset_Encoder(&distanceEncoder);
//DirectionEncoder_reset(&directionEncoder);
ecrobot_sound_tone(1000,5,100);
}
void getNextUnvisited()
{
checkIfAllVisited();
signed char x1 = currentPosition[0];
signed char y1 = currentPosition[1];
signed char cnt =0;
signed char colX=0;
signed char compare;
signed char rowY=0;
signed char foundOne=0;
//ecrobot_sound_tone(900,90,100);
if((allVisited)&&(getTokensFound()!=3))
{
addToPath(none);
return;
}
if(getTokensFound()==3)
{
getPathComplicated(x1,y1,6,6);
return;
}
signed char l,t,r,d;
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeVisited[colX][rowY]=visited(colX,rowY);
mazeDistance[colX][rowY]=0;
}
}
//now start calculatin'
mazeDistance[x1][y1]=50;
for(cnt=0; cnt<=35 ; cnt++)//nah, don't calculate how much: just do it ;)
{
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
//if((mazeDistance[x1][y1]!=0)){foundOne !=0 ;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeDistance[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeDistance[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeDistance[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeDistance[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeDistance[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeDistance[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeDistance[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeDistance[colX][rowY-1]-1;
}
mazeDistance[colX][rowY]=getLargestValue(l,t,r,d,mazeDistance[colX][rowY]);
}
}
}
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeDistance[colX][rowY]=50-mazeDistance[colX][rowY];
}
}
for(compare=0; compare<=35;compare++)
{
if(foundOne){return;};
for(rowY=12;rowY>=0;rowY--)
{
if(foundOne){return;};
for(colX=0;colX<=12;colX++)
{
if((mazeVisited[colX][rowY]==0)&&(mazeDistance[colX][rowY]==compare))
{
getPathComplicated(x1,y1,colX,rowY);
foundOne =1;
return;
}
}
}
}
//alle besucht
ecrobot_sound_tone(400,250,100);
systick_wait_ms(250);
ecrobot_sound_tone(900,250,100);
systick_wait_ms(250);
if(!allVisited)
getPathComplicated(x1,y1,6,6);
setAllVisited(1);
}
void getPathComplicated(signed char x1, signed char y1, signed char xDestination, signed char yDestination)
{
if((x1==xDestination)&&(y1==yDestination))
{
stopBoth();
ecrobot_sound_tone(100,500,100);
systick_wait_ms(2000);
endAll();
addToPath(none);
return;
}
//display_currentNode();
//systick_wait_ms(1000);
/*
display_clear(1);
display_goto_xy(0,0);
display_string("gPC called");
display_goto_xy(0,1);
display_int((int)x1,1);
display_goto_xy(3,1);
display_int((int)y1,1);
display_goto_xy(0,2);
display_int((int)xDestination,1);
display_goto_xy(3,2);
display_int((int)yDestination,1);
display_update();
*/
//systick_wait_ms(1000);
signed char xCurrent;
signed char yCurrent;
signed char startReached = 0;
signed char endReached = 0;
signed char xDistance= xDestination - x1;
signed char yDistance= yDestination - y1;
signed char yD2=0;
signed char xD2=0;
if(yDistance<0){yD2=-yDistance;}else{yD2=yDistance;}
if(xDistance<0){xD2=-xDistance;}else{xD2=xDistance;}
char maxIterations = (xD2+yD2)/2+40;//40 is safety padding
char cntLoops;
if(maxIterations<0)
maxIterations=-maxIterations;
//************************************** MAIN BODY ******
int x,y;
for(x=0;x<=12;x++)
{
for(y=0;y<=12;y++)
{
mazeStart[x][y]=0;
mazeEnd[x][y]=0;
}
}
mazeStart[x1][y1]=50;
int colX=0;
int rowY=0;
mazeEnd[xDestination][yDestination]=50;
signed char l,r,t,d;
for(cntLoops=0; cntLoops<=maxIterations*2; cntLoops++)
{
if((startReached==1) && (endReached==1))
{
cntLoops = (maxIterations*2)+1;
}
//***************************set mazeStart's values checks now if values overflow
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if(mazeStart[xDestination][yDestination]!=0){endReached = 1;break;};
//check if the field has the path to the adjecant field
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeStart[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeStart[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeStart[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeStart[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeStart[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeStart[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeStart[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeStart[colX][rowY-1]-1;
}
mazeStart[colX][rowY]=getLargestValue(l,t,r,d,mazeStart[colX][rowY]);
}
}
//*************************** set mazeEnd's values
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if((mazeEnd[x1][y1]!=0)){startReached = 1;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeEnd[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeEnd[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeEnd[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeEnd[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeEnd[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeEnd[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeEnd[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeEnd[colX][rowY-1]-1;
}
mazeEnd[colX][rowY]=getLargestValue(l,t,r,d,mazeEnd[colX][rowY]);
}
}
}
//prnt values both matrixes now added into mazeStart
int ty, tx;
tx=ty=0;
for(ty=12; ty>=0; ty--)
{
for(tx=0; tx<=12; tx++)
{
mazeStart[tx][ty]=mazeStart[tx][ty]+mazeEnd[tx][ty];
mazeEnd[tx][ty]=0;
}
}
//now add way of highest numbers to path so it can be printed or driven
xCurrent = x1;
yCurrent = y1;
signed char xDisLeft = xDistance;
signed char yDisLeft = yDistance;
signed char pathValue = mazeStart[x1][y1];
mazeEnd[xCurrent][yCurrent]=1; //repurposing mazeE to store already visited Nodes
if(endReached==1)
{
while((xCurrent!=xDestination) || (yCurrent != yDestination))
{
if((xDisLeft>0)||(((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue||(hasDirection(left,xCurrent,yCurrent)==0)))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(right,xCurrent,yCurrent) && (mazeStart[xCurrent+1][yCurrent]==pathValue) && (mazeEnd[xCurrent+1][yCurrent]==0))
{
addToPath(right);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent++;
xDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(xDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(left,xCurrent,yCurrent) && (mazeStart[xCurrent-1][yCurrent]==pathValue) && (mazeEnd[xCurrent-1][yCurrent]==0))
{
addToPath(left);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent--;
xDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft>0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(top,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent+1]==pathValue) && (mazeEnd[xCurrent][yCurrent+1]==0))
{
addToPath(top);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent++;
yDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(down,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent-1]==pathValue) && (mazeEnd[xCurrent][yCurrent-1]==0))
{
addToPath(down);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent--;
yDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
}
}
/*
display_goto_xy(0,3);
display_string("ret:");
display_goto_xy(4,3);
display_int(pathStorage[1],1);
display_update();*/
//systick_wait_ms(2000);
}
//走行状態設定関数
void RN_setting()
{
int forward_speed;
PWMValues pValues;
switch (setting_mode){
//キャリブレーション状態
case (RN_SETTINGMODE_START):
if(Calibration_doCalibrate(&mCalibration) == 1)
setting_mode = RN_RUN;
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):
//wait_count++;
//RA_linetrace_PID_balanceoff(100);
pValues = PWMValGenerator_calTailRunPWM(&mPWMValGenerator,50,PIDCalcDebug_PIDLinetrace(&mPIDCalcDebug));
nxt_motor_set_speed(NXT_PORT_C, pValues.pwmL, 1);
nxt_motor_set_speed(NXT_PORT_B, pValues.pwmR, 1);
/*
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500)
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE_START;
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
wait_count = 0;
}
*/
break;
case (RN_SLOPE_START):
RA_linetrace_PID_balanceoff(70);
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_now_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if(distance_now_slope - distance_before_slope > 30)
{
setting_mode = RN_SLOPE_DOWN;
}
break;
case (RN_SLOPE_DOWN):
RA_linetrace_PID_balanceoff(55);
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1))
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE_AFTER;
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_peak_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
}
break;
case (RN_SLOPE_AFTER):
RA_linetrace_PID_balanceoff(55);
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_after_slope = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if(distance_after_slope - distance_peak_slope > 30)
{
setting_mode = RN_SLOPE_END;
}
break;
case (RN_SLOPE_END):
RA_linetrace_PID_balanceoff(65);
break;
default:
break;
}
}
//走行状態設定関数
void RN_setting()
{
static int wait_count = 0;
switch (setting_mode){
//キャリブレーション状態
case (RN_START):
RN_calibrate();
break;
//対戦中
case (RN_RUN):
(void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE); /* スティック入力 */
cmd_forward = -((S8)bt_receive_buf[0])/2; /* 前進量(そのままでは早すぎるので値を半分） */
cmd_turn = ((S8)bt_receive_buf[1]); /* 旋回量 */
//ターボチェック
if(boost() == 1)
{
setting_mode = RN_BOOST;
ecrobot_sound_tone(980,512,100);
}
else /* ターボ無し、スティック操作 */
{
nxt_motor_set_speed(NXT_PORT_C, cmd_forward + cmd_turn/2, 1);
nxt_motor_set_speed(NXT_PORT_B, cmd_forward - cmd_turn/2, 1);
}
if(ecrobot_get_touch_sensor(NXT_PORT_S4) == 1) /* ヒットチェック */
{
ecrobot_sound_tone(980,512,100);
nxt_motor_set_speed(NXT_PORT_C,100,1);
nxt_motor_set_speed(NXT_PORT_B,-100,1);
wait_count = 0;
setting_mode = RN_PUSHBUTTON;
}
break;
//ターボ動作(ver 2.0新機能）
case (RN_BOOST):
wait_count++;
/* 両車輪にモータ操作量の最大値を送信（スティック操作不可） */
nxt_motor_set_speed(NXT_PORT_C,127,1);
nxt_motor_set_speed(NXT_PORT_B,127,1);
/* 1秒後に通常モードに復帰 */
if(wait_count > 125)
{
setting_mode = RN_RUN;
wait_count = 0;
}
break;
//敗北動作
case (RN_PUSHBUTTON):
wait_count++;
if(wait_count >= 150)
{
TailAngleChange(ANGLEOFZERO);
nxt_motor_set_speed(NXT_PORT_C,0,1);
nxt_motor_set_speed(NXT_PORT_B,0,1);
}
if(wait_count == 1000)
setting_mode = RN_START;
break;
default:
break;
}
}
//キャリブレーション
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 RN_setting()
{
static int timecounter = 0;
float weight = 0.6;
static int markerflag = 0;
switch (setting_mode){
//キャリブレーション状態
case (RN_SETTINGMODE_START):
if(RN_calibrate() == 1)
{
setting_mode = RN_RUN;
PWMGeneratorModeChange(RN_MODE_TAIL);
TailAngleChange(ANGLEOFDOWN);
}
break;
//通常走行状態
case (RN_RUN):
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
setSection_in();
if(getInitGyroOffset() - 50 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && timecounter > 1000)
{
ecrobot_sound_tone(880, 512, 30);
setting_mode = RN_SLOPE;
timecounter = 0;
}
break;
case (RN_SLOPE):
setSection_in();
if(runningSlope() == 1)
setting_mode = RN_RUN_SECOND;
break;
case (RN_RUN_SECOND):
setSection_in();
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(markerDetector() == 1)
markerflag+=1;
if(markerflag == 2)
setting_mode = RN_LOOKUPGATE;
break;
case (RN_LOOKUPGATE):
if(runningLookUpGate() == 1)
setting_mode = RN_RUN_THIRD;
break;
case (RN_RUN_THIRD):
setCmdForward(RA_speed(60));
setCmdTurn(RA_linetrace_PID(getCmdForward()));
break;
/*
case (RN_DRIFTTURN):
break;
*/
default:
break;
}
timecounter++;
}
//走行設定関数
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;
}
}
/* インコース走行区間検出 */
void setSection_out(){
static SECT_OUT crt_sect = START;
static float buf_x = 0.0, buf_y = 0.0, buf_l = 0.0, buf_th = 0.0;
float def_x = x_r - buf_x;
float def_y = y_r - buf_y;
float def_l = dist - buf_l;
float def_th = theta - buf_th;
switch(crt_sect){
case (START): //スタート→坂道
if(GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500){
ecrobot_sound_tone(220, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = UP_SLOPE;
}
trgt_R = 0.0;
break;
case (UP_SLOPE): //坂道始点→頂点
if(def_l >= 30 && GYRO_OFFSET - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)){
ecrobot_sound_tone(233, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = DOWN_SLOPE;
}
trgt_R = 0.0;
break;
case (DOWN_SLOPE): //頂点→坂道終点
//if(dist >= 390){
if(def_l >= 90){
ecrobot_sound_tone(246, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_CORNER;
}
SPEED_COUNT = 50;
trgt_R = 0.0;
break;
case (FST_CORNER): //坂道終点→第一カーブ
if(def_x >= 74 && def_y >= 70 && def_l >= 110 && def_th >= 90){
ecrobot_sound_tone(261, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_STRAIGHT;
}
SPEED_COUNT = 60;
trgt_R = 67.59;
break;
case (FST_STRAIGHT): //第一カーブ終点→第一ストレート
if(def_l >= 115){
ecrobot_sound_tone(277, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_CORNER;
}
trgt_R = 0.0;
break;
case (SND_CORNER): //第一ストレート終点→第二カーブ
if(def_x <= -95 && def_y <= -5 && def_l >= 245 && def_th >= 240){
ecrobot_sound_tone(293, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_STRAIGHT;
}
trgt_R = 56.59;
break;
case (SND_STRAIGHT): //第二カーブ終点→第二ストレート
if(def_x >= 40 && def_y <= -15 && def_l >= 40){
ecrobot_sound_tone(311, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_CORNER;
}
trgt_R = 0.0;
break;
case (TRD_CORNER): //第二ストレート終点→第三カーブ
if(def_x <= -80 && def_y <= -80 && def_l >= 235 && def_th <= -210){
ecrobot_sound_tone(329, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_STRAIGHT;
}
trgt_R = -64.02;
break;
case (TRD_STRAIGHT): //第三カーブ終点→第三ストレート
if(def_x <= -50 && def_y >= 105 && def_l >= 115){
ecrobot_sound_tone(349, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FIN_APPROACH;
}
trgt_R = 0.0;
break;
case (FIN_APPROACH): //第三ストレート終点→マーカー
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (STEPS): //階段
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (90DTURN):
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
default:
break;
}
}
//キャリブレーション関数
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;
}
}
}
int runningSlope()
{
static int timecounter = 0;
static int distanceslopestart = 0;
static int distanceslopeup = 0;
static int distanceslopetop = 0;
static int distanceslopedown = 0;
int slopeendflag = 0;
int weight = 1.0;
switch(runningslope){
case (SLOPE_START):
if(timecounter == 0)
distanceslopestart = getNowDistance();
distanceslopeup = getNowDistance();
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(distanceslopeup - distanceslopestart > 10 && timecounter > 300)
{
ecrobot_sound_tone(800, 512, 20);
runningslope = SLOPE_TOP;
timecounter = 0;
}
break;
case (SLOPE_TOP):
setCmdForward(RA_speed(50));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(getInitGyroOffset() + 50 < (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)/* && timecounter > 300*/)
{
ecrobot_sound_tone(820, 512, 20);
runningslope = SLOPE_DOWN;
timecounter = 0;
}
break;
case (SLOPE_DOWN):
if(timecounter == 0)
distanceslopetop = getNowDistance();
distanceslopedown = getNowDistance();
setCmdForward(RA_speed(50));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(distanceslopedown - distanceslopetop > 30/* && timecounter > 300*/)
{
runningslope = SLOPE_END;
timecounter = 0;
ecrobot_sound_tone(840, 512, 20);
}
break;
case (SLOPE_END):
setCmdForward(RA_speed(80));
setCmdTurn(weight * RA_linetrace_PID(getCmdForward())+(1 - weight) * RA_curvatureCtrl_PID(getTargetR()));
if(getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)/* && timecounter > 300*/)
{
ecrobot_sound_tone(900, 512, 20);
slopeendflag = 1;
timecounter = 0;
}
break;
default:
break;
}
timecounter++;
return slopeendflag;
}
/* 走行区間検出 */
void setSection(){
static IN_SECTION crt_sect = START;
static float buf_x = 0.0, buf_y = 0.0, buf_l = 0.0, buf_th = 0.0;
float def_x = x_r - buf_x;
float def_y = y_r - buf_y;
float def_l = dist - buf_l;
float def_th = theta - buf_th;
switch(crt_sect){
case (START): //スタート→坂道
if(dist >= 10){
ecrobot_sound_tone(220, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = UP_SLOPE;
}
trgt_R = 0.0;
break;
case (UP_SLOPE): //坂道始点→頂点
if(def_l >= 10){
ecrobot_sound_tone(233, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = DOWN_SLOPE;
}
trgt_R = 0.0;
break;
case (DOWN_SLOPE): //頂点→坂道終点
//if(dist >= 390){
if(def_l >= 90){
ecrobot_sound_tone(246, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_CORNER;
}
trgt_R = 0.0;
break;
case (FST_CORNER): //坂道終点→第一カーブ
if(def_x >= 74 && def_y >= 70 && def_l >= 110 && def_th >= 90){
ecrobot_sound_tone(261, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_STRAIGHT;
}
trgt_R = 67.59;
break;
case (FST_STRAIGHT): //第一カーブ終点→第一ストレート
if(def_l >= 115){
ecrobot_sound_tone(277, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_CORNER;
}
trgt_R = 0.0;
break;
case (SND_CORNER): //第一ストレート終点→第二カーブ
if(def_x <= -95 && def_y <= -5 && def_l >= 245 && def_th >= 240){
ecrobot_sound_tone(293, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_STRAIGHT;
}
trgt_R = 56.59;
break;
case (SND_STRAIGHT): //第二カーブ終点→第二ストレート
if(def_x >= 40 && def_y <= -15 && def_l >= 40){
ecrobot_sound_tone(311, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_CORNER;
}
trgt_R = 0.0;
break;
case (TRD_CORNER): //第二ストレート終点→第三カーブ
if(def_x <= -80 && def_y <= -80 && def_l >= 235 && def_th <= -210){
ecrobot_sound_tone(329, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_STRAIGHT;
}
trgt_R = -64.02;
break;
case (TRD_STRAIGHT): //第三カーブ終点→第三ストレート
if(def_x <= -50 && def_y >= 105 && def_l >= 115){
ecrobot_sound_tone(349, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FIN_APPROACH;
}
trgt_R = 0.0;
break;
case (FIN_APPROACH): //第三ストレート終点→マーカー
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
default:
break;
}
}
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 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 Sound_soundTone(Sound *this_Sound, U32 freq, U32 ms, U32 vol){
ecrobot_sound_tone(freq,ms,vol);
}
/* ______________アウトコース走行区間検出_______________________ */
void setSection_out(){
/* アウトコース走行区間 */
static int wait_count = 0;
wait_count++;
float def_x = getXCoo() - buf_x;
float def_y = getYCoo() - buf_y;
float def_l = getDistance() - buf_l;
float def_th = getTheta( ) - buf_th;
switch(crt_sect){
case (START): //スタート→坂道
if(getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1) && wait_count > 500){
ecrobot_sound_tone(220, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = UP_SLOPE;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (UP_SLOPE): //坂道始点→頂点
if(def_l >= 30 && getInitGyroOffset() - 30 > (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1)){
ecrobot_sound_tone(233, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = DOWN_SLOPE;
trgt_speed = trgt_speed -10;
}
trgt_R = 0.0;
break;
case (DOWN_SLOPE): //頂点→坂道終点
//if(getDistance() >= 390){
if(def_l >= 90){
ecrobot_sound_tone(246, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_CORNER;
trgt_speed = trgt_speed - 10;
}
trgt_R = 0.0;
break;
case (FST_CORNER): //坂道終点→第一カーブ
if(def_x >= 74 && def_y >= 70 && def_l >= 110 && def_th >= 90){
ecrobot_sound_tone(261, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FST_STRAIGHT;
trgt_speed = trgt_speed +20;
}
trgt_R = 67.59;
break;
case (FST_STRAIGHT): //第一カーブ終点→第一ストレート
if(def_l >= 115){
ecrobot_sound_tone(277, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_CORNER;
trgt_speed = trgt_speed +10;
}
trgt_R = 0.0;
break;
case (SND_CORNER): //第一ストレート終点→第二カーブ
if(def_x <= -95 && def_y <= -5 && def_l >= 245 && def_th >= 240){
ecrobot_sound_tone(293, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = SND_STRAIGHT;
trgt_speed = trgt_speed;
}
trgt_R = 56.59;
break;
case (SND_STRAIGHT): //第二カーブ終点→第二ストレート
if(def_x >= 40 && def_y <= -15 && def_l >= 40){
ecrobot_sound_tone(311, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_CORNER;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (TRD_CORNER): //第二ストレート終点→第三カーブ
if(def_x <= -80 && def_y <= -80 && def_l >= 235 && def_th <= -210){
ecrobot_sound_tone(329, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = TRD_STRAIGHT;
trgt_speed = trgt_speed;
}
trgt_R = -64.02;
break;
case (TRD_STRAIGHT): //第三カーブ終点→第三ストレート
if(def_x <= -50 && def_y >= 105 && def_l >= 115){
ecrobot_sound_tone(349, 100, 50);
changeSection(&buf_x, &buf_y, &buf_l, &buf_th);
crt_sect = FIN_APPROACH;
trgt_speed = trgt_speed;
}
trgt_R = 0.0;
break;
case (FIN_APPROACH): //第三ストレート終点→マーカー
if(1){
ecrobot_sound_tone(369, 100, 50);
trgt_speed = trgt_speed;
}
trgt_R = 51.80;
break;
case (STEPS): //階段
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
case (TURN90D):
if(1){
ecrobot_sound_tone(369, 100, 50);
}
trgt_R = 51.80;
break;
default:
trgt_speed = 0;
trgt_R = 0.0;
break;
}
}
//*****************************************************************************
// 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 */
}
