void Go_Ball_Area(Let *let) {
static uint8 step = 0;
static uint16 count = 0;
static uint16 limit = 1;
if(let->area != 4)
{
Line_Trace(let, MODE_FORWARD);
}
else if(let->area == 4)
{
if(let->place > 0)
{
let->mode = MODE_MOVE;
return;
}
if(step == 0)
{
Motor_Right(0);
Motor_Left(0);
limit = 50;
} else if(step == 1) {
PID_Motor_Left(0);
PID_Motor_Right(0);
limit = 50;
}
else if(step == 2)
{
Motor_Right(-7000);
Motor_Left(-7000);
limit = 40;
} else if(step == 3)
{
Motor_Right(0);
Motor_Left(0);
limit = 20;
}
else if(step == 4)
{
let->mode = MODE_SEEK;
step = 0;
return;
}
if(limit == count)
{
count = 0;
step++;
return;
}
count++;
}
}
void Shooting_tennis_ball(Let *let) {
static uint8 step = 0;
static uint16 count = 0;
static uint16 limit = 1;
if(let->area <= 0)
{
Line_Trace(let, MODE_FORWARD);
}
else if(let->area == 1)
{
if(step == 0)
{
Motor_Right(7000);
Motor_Left(-200);
limit = 80;
} else if(step == 1)
{
Motor_Right(0);
Motor_Left(0);
let->updown = DOWN;
let->grab = RELEASE;
limit = 10;
} else if(step == 2)
{
let->updown = DOWN;
limit = 100;
} else if(step == 3)
{
let->updown = UP;
Motor_Right(-7000);
limit = 75;
} else if(step == 4)
{
Motor_Right(0);
Motor_Left(0);
let->mode = MODE_LINE_TRACE;
}
if(limit == count)
{
count = 0;
step++;
return;
}
count++;
}
}
void move(Let *let)
{
static uint16 count = 0;
uint16 limit = 10;
if(let->place == 1)
{
let->mode = MODE_SEEK;
return;
}
else if(let->place > 0)
{
Line_Trace(let,MODE_FORWARD);
}
count++;
if(count == (limit*let->place))
{
count = 0;
let->mode = MODE_SEEK;
return;
}
}
void Return(Let *let)
{
static uint8 step = 0;
static uint16 count = 0;
static uint16 limit = 1;
Sensor_LED_Write(0);
if(let->place < 3)
{
if(step == 0)
{
Motor_Right(-3500);
Motor_Left(-3500);
//limit = let->count + let->place*40;
limit = let->count;
} else if(step == 1)
{
if(let->count_r > 0)
{
Motor_Right(3000);//反時計
Motor_Left(-3000);
limit = (int)(let->count_r);
}
else if(let->count_r <= 0)
{
Motor_Right(-3000);
Motor_Left(3000);
limit = (int)(-let->count_r);
}
} else if(step == 2)
{
if(let->color == MISS)
{
Motor_Right(0);
Motor_Left(0);
step = 0;
count = 0;
let->count = 0;
let->count_r = 0;
let->mode = MODE_SEEK;
return;
}
if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
{
step++;
Motor_Right(0);
Motor_Left(0);
count = 0;
return;
}
else
{
Motor_Right(-7000);
Motor_Left(-7000);
limit = 0;
}
} else if(step == 3)
{
Motor_Right(7000);
Motor_Left(7000);
limit = 35;
}
else if(step == 4)
{
Motor_Right(0);
Motor_Left(7000);
limit = 60;
} else if(step == 5)
{
Motor_Right(7000);
Motor_Left(7000);
let->area = 3;
PID_init(let);
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
limit = 35;
} else if(step == 6)
{
Line_Trace(let,MODE_BACKWARD);
limit = 0;
}
}
else if(let->place >= 3)
{
if(step == 0)
{
Motor_Right(-3500);
Motor_Left(-3500);
limit = 0;
if(UART_Line_Sensor_GetRxBufferSize())
{
let->slave.Trans = (uint8)UART_Line_Sensor_GetChar();
}
if(let->slave.status.e)
{
step = 1;
count = 0;
Debug_LED_Write(1);
return;
}
} else if(step == 1)
{
if(let->count_r > 0)
{
Motor_Right(3000);//反時計
Motor_Left(-3000);
limit = (int)(let->count_r + 20);
}
else if(let->count_r <= 0)
{
Motor_Right(-3000);
Motor_Left(3000);
limit = (int)(-let->count_r);
}
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
} else if(step == 2)
{
if(let->color == MISS)
{
Motor_Right(0);
Motor_Left(0);
step = 0;
count = 0;
let->count = 0;
let->count_r = 0;
let->mode = MODE_SEEK;
return;
}
if((Switch_L_Read() == 1) && (Switch_R_Read() == 1))
{
step++;
Motor_Right(0);
Motor_Left(0);
count = 0;
return;
}
else
{
Motor_Right(-7000);
Motor_Left(-7000);
limit = 0;
}
} else if(step == 3)
{
Motor_Right(7000);
Motor_Left(7000);
limit = 35;
}
else if(step == 4)
{
Motor_Right(0);
Motor_Left(7000);
limit = 60;
} else if(step == 5)
{
Motor_Right(7000);
Motor_Left(7000);
let->area = 3;
PID_init(let);
UART_Line_Sensor_ClearRxBuffer();
let->slave.Trans = 0;
limit = 35;
} else if(step == 6)
{
Line_Trace(let,MODE_BACKWARD);
limit = 0;
}
}
if(let->color == RED)
{
if(let->area == 0)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
else if(let->color == YELLO)
{
if(let->area == 1)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
else if(let->color == BLUE)
{
if(let->area == 2)
{
Motor_Right(0);
Motor_Left(0);
count = 0;
step = 0;
let->mode = MODE_SHOOT;
}
}
count++;
if(limit == count)
{
count = 0;
step++;
return;
}
}
///*
int main(){ //第3回試走会用
int ball_count = 0;
bool area_flag = OFF;
float serch_posi = 0.0;
float move_dis = 0.0;
state = init_state;
// color = RED;
while(1){ //while開始
switch(state){
case init_state:
initialize();
state = free_ball;
//state = ball_search;
//state = finish;
break;
case free_ball:
Line_Trace(3);
turn(85.0, FAST);
wait(0.25);
servo_throw();
wait(0.25);
servo_ini();
wait(0.25);
turn(-85.0,FAST);
Line_Trace(2);
state = ball_search;
break;
case ball_search:
if(move_flag){
while(l_Y <= move_dis)
Line_Trace();
stop();
move_flag = OFF;
}
else{
if(color != NO_BALL){
while(l_Y <= serch_posi)
Line_Trace();
stop();
}
}
serch();
state = ball_shoot;
//if((l_time+35) > LIMIT)
// state = finish;
break;
case ball_shoot:
wait(0.25);
//color = what_color();
if(color != NO_BALL){ //色を得られたら
//turn(180, FAST);
//wait(0.25);
if(area_flag){
if(color == YELLO)
Line_Trace(color);
else
Line_Trace(color + 1);
}
else{
if(color == YELLO)
Line_Trace(color - 1);
else
Line_Trace(color);
}
Ball_Shoot();
ball_count++;
if(ball_count == B_GOAL || (l_time+35) > LIMIT){
state = finish;
break;
}
if(color == YELLO)
Line_Trace(color - 1);
else
Line_Trace(color);
}
//目標まで行ったかの確認
state = ball_search;
if(move_flag)
state = area_move;
break;
case area_move:
move_dis = 250;
if(serch_posi >= 1200 - move_dis) //行き過ぎ防止用
move_dis = 0;
serch_posi += move_dis;
area_flag = ON;
state = ball_search;
break;
case finish:
fine();
break;
}//switch終了
}//while終了
}//main終了