void strat_prepare(void)
{
//initalise kx factor
robot_kx = TEAM_SIDE_VALUE(-1, 1);
//send color to meca
ROME_SENDWAIT_MECA_SET_ROBOT_COLOR(&rome_meca, robot_state.team == TEAM_GREEN);
ROME_LOG(&rome_paddock,DEBUG,"Strat prepare");
// initialize asserv
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
set_xya_wait(0,0,0);
ROME_SENDWAIT_ASSERV_GOTO_XY(&rome_asserv, 0, 0, 0);
ROME_SENDWAIT_ASSERV_SET_HTRAJ_XY_STEERING(&rome_asserv, 1.5, 0.03);
ROME_SENDWAIT_ASSERV_SET_HTRAJ_XY_CRUISE(&rome_asserv, 15, 0.03);
// autoset robot
// x in starting area
set_xya_wait(KX(0), 0, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
autoset(ROBOT_SIDE_BACK,AUTOSET_MAIN, KX(1500-AUTOSET_OFFSET), 0);
goto_xya(KX(1000), 0, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
// y on building area
goto_xya(KX(800), 400, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_UP));
autoset(ROBOT_SIDE_BACK,AUTOSET_UP, 0, 2000-AUTOSET_OFFSET);
// check the state of the cylinder
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_CHECK_EMPTY);
//evade the border, leaving space for galipette
goto_xya(KX(900), 1600, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_UP));
//go in front of starting area
goto_xya(KX(1300), 1500, arfast(ROBOT_SIDE_BALLEATER,TABLE_SIDE_DOWN));
//wait for meca to end checking cylinder
_wait_meca_ready();
if (!cylinder_is_empty()){
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_TRASH_BEGINMATCH);
}
//wait for meca to end all orders before shutting down asserv
_wait_meca_ready();
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 0);
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 0);
}
void strat_run(void)
{
ROME_LOG(&rome_paddock,INFO,"Go !!!");
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 1);
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
order_result_t or_take_water_near = ORDER_FAILURE;
order_result_t or_empty_water_near = ORDER_FAILURE;
order_result_t or_take_water_far = ORDER_FAILURE;
order_result_t or_empty_water_far = ORDER_FAILURE;
while (!( or_take_water_near == ORDER_SUCCESS &&
or_take_water_far == ORDER_SUCCESS &&
or_empty_water_near == ORDER_SUCCESS &&
or_empty_water_far == ORDER_SUCCESS ) ){
idle();
if (or_take_water_near != ORDER_SUCCESS){
or_take_water_near = take_water(DISPENSER_NEAR);
or_empty_water_near = empty_cylinder();
}
//go to the middle to reset the Y axis
if (goto_pathfinding_node(PATHFINDING_GRAPH_NODE_MIDDLE_BOT, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_DOWN))
== ORDER_SUCCESS){
goto_xya(KX(0), 500, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
autoset(ROBOT_SIDE_BACK, AUTOSET_DOWN, 0, 250+AUTOSET_OFFSET);
goto_xya(KX(0), 550, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
}
//if oponent blocked us on the path to the middle, go back near starting area
else {
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_WATER_TOWER, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
}
if (or_take_water_far != ORDER_SUCCESS){
or_take_water_far = take_water(DISPENSER_FAR);
or_empty_water_far = empty_cylinder();
}
}
ROME_LOG(&rome_paddock,INFO,"That's all folks !");
idle_delay_ms(3000);
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 0);
ROME_SENDWAIT_MECA_SET_POWER(&rome_meca, 0);
}
void key_action(void)
{
unsigned char i;
i=init_key();
delay_ad9833(50000);
switch(i)
{
case 0:
break;
case 1://选择方波
itswave=0;
change_vol(itsvol);
autoset(itswave,itsfreq);
break;
case 2://选择正弦波
itswave=1;
change_vol(itsvol);
autoset(itswave,itsfreq);
break;
case 3://选择三角波
itswave=2;
change_vol(itsvol);
autoset(itswave,itsfreq);
break;
case 4://恢复初始值
itswave=1,itsfreq=100,itsvol=10;
change_vol(itsvol);
autoset(itswave,itsfreq);
break;
case 5://频率加（50hz）
itsfreq+=50;
if(itsfreq>=20000)
{
itsfreq=20000;
}
autoset(itswave,itsfreq);
break;
case 6://频率减（50hz）
itsfreq-=50;
if(itsfreq<=50)
{
itsfreq=50;
}
autoset(itswave,itsfreq);
break;
case 7://频率加（100hz）
itsfreq+=100;
if(itsfreq>=20000)
{
itsfreq=20000;
}
autoset(itswave,itsfreq);
break;
case 8://频率减（100hz）
itsfreq-=100;
if(itsfreq<=100)
{
itsfreq=100;
}
autoset(itswave,itsfreq);
break;
case 9://幅度加（0.1V）
itsvol+=1;
if(itsvol>=50)
{
itsvol=50;
}
change_vol(itsvol);
break;
case 10://幅度减（0.1V)
itsvol-=1;
if(itsvol<=1)
{
itsvol=1;
}
change_vol(itsvol);
break;
case 11://幅度加（1V）
itsvol+=10;
if(itsvol>=50)
{
itsvol=50;
}
change_vol(itsvol);
break;
case 12://幅度减（1V)
itsvol-=10;
if(itsvol<=10)
{
itsvol=10;
}
change_vol(itsvol);
break;
case 13:
save();
break;
case 14:
load();
break;
case 15:
exit();
break;
case 16://频率加（1000hz）
autoset_test(itswave,itsfreq/10);
// break;
// thanks();
break;
default:
break;
}
}
order_result_t take_water(dispenser_t dispenser){
order_result_t or;
//save the amount of water we already have to check if we managed to "open" a dispenser
uint8_t nb_empty = robot_state.cylinder_nb_empty;
//dispenser positions
int16_t near_pos = 2000-840;
int16_t far_pos = -(1500-620);
//balleater configuration
int16_t balleater_depth = AUTOSET_OFFSET + 35;
int16_t approach_depth = balleater_depth + 60;
int16_t approach_side = 150;
int16_t traj1[4];
int16_t traj2[2];
float angle;
//prepare meca to take water
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_PREPARE_LOAD_WATER);
_wait_meca_ground_clear();
set_speed(RS_FAST);
switch(dispenser){
case DISPENSER_NEAR:{
ROME_LOG(&rome_paddock,DEBUG,"Going to start area dispenser");
angle = arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_MAIN);
//send first position order
or = goto_xya(KX(1200), near_pos + approach_side, angle);
if (or!= ORDER_SUCCESS)
return or;
//prepare next move orders
//dispenser near is alongside Y axis
traj1[0] = KX(1500-approach_depth);
traj1[1] = near_pos + approach_side;
traj1[2] = KX(1500-(balleater_depth+10));
traj1[3] = near_pos;
traj2[0] = KX(1500-300);
traj2[1] = near_pos;
break;
}
case DISPENSER_FAR:{
ROME_LOG(&rome_paddock,INFO,"Going to opposite dispenser");
//send first position order
//go to the bee corner to be near borders for autoset
or = goto_pathfinding_node(PATHFINDING_GRAPH_NODE_OPPOSITE_BEE, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
if (or != ORDER_SUCCESS){
ROME_LOG(&rome_paddock,INFO,"Aborting opposite dispenser");
//go back in the middle
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_MIDDLE_BOT, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
return or;
}
//we did a very long move, so try to launch an autoset
//or = goto_xya(KX(-1200), 150, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
//autoset(ROBOT_SIDE_BACK,AUTOSET_DOWN, 0, AUTOSET_OFFSET);
or = goto_xya(KX(-1350), 300, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
autoset(ROBOT_SIDE_BALLEATER,AUTOSET_AUX, KX(-1500+AUTOSET_OFFSET), 0);
or = goto_xya(KX(-1200), 300, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
//prepare next move orders
//dispenser far is alongside X axis
angle = arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_DOWN);
traj1[0] = KX(far_pos-approach_side);
traj1[1] = approach_depth;
traj1[2] = KX(far_pos);
traj1[3] = balleater_depth-25;
traj2[0] = KX(far_pos);
traj2[1] = 500;
break;
}
default:
return ORDER_FAILURE;
}
set_speed(RS_SLOW);
//go to dispenser
or = goto_traj(traj1, angle);
if (or!= ORDER_SUCCESS)
return or;
//take the water
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_LOAD_WATER);
_wait_meca_ground_clear();
set_speed(RS_NORMAL);
//just go back a little bit
or = goto_traj(traj2, angle);
//if the water in cylinder changed, we scored !
if (nb_empty != robot_state.cylinder_nb_empty)
update_score(10);
set_speed(RS_NORMAL);
return or;
}
