void comm_test(void)
{
if (!goal_init){
select_goal();
}
/* Flash color of LED for defending goal */
set(TCCR0B,CS02); //start timer
set(TCCR0B,CS00);
positioning_LED(goal); //turn on LED
}
void trace() //一週目
{
short start_frag=0;
short trace_roop_frag=0;
short i=0;
short permit_goal=0;
circl_count=1;
LED1=ON;
TIMER_WAIT(1000);
LED1=OFF;
ch_para();
//ch_para2(0);
select_P();
// select_sub();
select_I();
// select_D();
// select_G();
select_goal();
// select_corner();
select_speed();
while(trace_roop_frag==0){
init_thre();
calc_senc();
if(cari_SEN[1] > 20.0){
if(Dispermit_goal==1){
start_frag=1;
LED2=ON;
TIMER_WAIT(500);
}
else{
LED2=ON;
permit_goal=1;
}
}
if(cari_SEN[6] > 20.0 && permit_goal==1){
start_frag=1;
LED3=ON;
TIMER_WAIT(500);
}
else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない
LED3=ON;
Dispermit_goal=1;
}
if(start_frag==1){
countdown();
TIMER_WAIT(1000);
for(i=0; i<201; i++){
ENC_dis[i]=0.0;
ENC_check[i]=0;
RL_check[i]=3;
}
for(i=0; i<5; i++)
SC_box[i]=ST;
zero_gyro=get_gyro();
ENC_R=15000;
ENC_sub_R=0;
ENC_L=15000;
ENC_sub_L=0;
EncTimeCount=0;
ENC_count=0;
dis_count=0;
mot_STB(START_A);
start0=1;
mot_frag=1;
LED1=OFF;
while(stop_frag==0){
if(PORTE.PORT.BIT.B0 == 0){
stop_frag=1;
TIMER_WAIT(500);
}
}
stop_manager();
trace_roop_frag=1;
}
else{}
}
}
void trace2() //二週目
{
short start_frag=0;
short trace_roop_frag=0;
short permit_goal=0;
circl_count=2;
LED2=ON;
TIMER_WAIT(1000);
LED2=OFF;
// ch_para2(0);
ch_para();
select_P();
select_I();
// select_D();
// select_G();
select_goal();
// select_corner();
select_speed();
while(trace_roop_frag==0){
init_thre();
calc_senc();
if(cari_SEN[1] > 20.0){
if(Dispermit_goal==1){
start_frag=1;
LED2=ON;
TIMER_WAIT(500);
}
else{
LED2=ON;
permit_goal=1;
}
}
if(cari_SEN[6] > 20.0 && permit_goal==1){
start_frag=1;
LED3=ON;
TIMER_WAIT(500);
}
else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない
LED3=ON;
Dispermit_goal=1;
}
if(start_frag==1){
countdown();
TIMER_WAIT(1000);
zero_gyro=get_gyro();
ENC_R=15000;
ENC_sub_R=0;
ENC_L=15000;
ENC_sub_L=0;
EncTimeCount=0;
ENC_count=0;
dis_count=0;
mot_STB(START_A);
start0=1;
mot_frag=1;
while(stop_frag==0){}
stop_manager();
trace_roop_frag=1;
}
else{}
}
}
