/*
* get last 2 columns
* must be called after the first temple building
*/
uint8_t strat_static_columns_pass2(void)
{
uint16_t old_spdd, old_spda;
uint8_t side, err, next_mode;
DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
strat_get_speed(&old_spdd, &old_spda);
if (get_color() == I2C_COLOR_RED)
side = I2C_RIGHT_SIDE;
else
side = I2C_LEFT_SIDE;
if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
next_mode = I2C_MECHBOARD_MODE_STORE;
else
next_mode = I2C_MECHBOARD_MODE_HARVEST;
switch (strat_infos.s_cols.configuration) {
/* configuration 1: 4 cols on line 0 */
case 1:
if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) {
/* go on line 2 */
strat_set_speed(2000, 700);
trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
i2c_mechboard_mode_prepare_pickup_next(side,
next_mode);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
trajectory_goto_forward_xy_abs(&mainboard.traj,
LINE2_X,
COLOR_Y(400));
err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2,
TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
}
else {
/* go on line 1 */
strat_set_speed(2000, 700);
trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
i2c_mechboard_mode_prepare_pickup_next(side,
next_mode);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
err = goto_and_avoid_forward(LINE1_X,
COLOR_Y(400),
TRAJ_FLAGS_NO_NEAR,
TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
}
ERROUT(END_TRAJ);
break;
/* configuration 2: 2 cols on line 0,
all other colums are on line 1 */
case 2:
/* go on line 1 */
strat_set_speed(2000, 700);
trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
i2c_mechboard_mode_prepare_pickup_next(side,
next_mode);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
err = goto_and_avoid_forward(COL10_X, COLOR_Y(400),
TRAJ_FLAGS_NO_NEAR,
TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
ERROUT(END_TRAJ);
break;
/* configuration 3: 2 cols on line 0,
all other colums are on line 2 */
case 3:
/* go on line 2 */
strat_set_speed(2000, 700);
trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
i2c_mechboard_mode_prepare_pickup_next(side,
next_mode);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
trajectory_goto_forward_xy_abs(&mainboard.traj,
LINE2_X,
COLOR_Y(400));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
ERROUT(END_TRAJ);
break;
/* configuration 4: 2 cols on line 0,
2 on line 1, 2 on line 2 */
case 4:
/* go on line 1 */
strat_set_speed(600, 2000);
trajectory_d_a_rel(&mainboard.traj, -BIG_DIST,
COLOR_A(-135));
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900),
TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
i2c_mechboard_mode_prepare_pickup_next(side,
next_mode);
strat_set_speed(2000, 2000);
trajectory_d_rel(&mainboard.traj, -BIG_DIST);
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100),
TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
ERROUT(err);
DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
err = goto_and_avoid_forward(LINE1_X,
COLOR_Y(400),
TRAJ_FLAGS_NO_NEAR,
TRAJ_FLAGS_NO_NEAR);
ERROUT(END_TRAJ);
break;
default:
break;
}
/* should not reach this point */
ERROUT(END_ERROR);
end:
strat_set_speed(old_spdd, old_spda);
return err;
}
void strat_do_emergency_behaviour(void){
//when pince is down... :s
static uint24_t starttime;
if(beudatraj.state==READY){
int16_t my_y = position_get_y_s16(&beudabot.posr);
switch (Strat_Action) {
case 0:
/*Go p1*/
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y);
Strat_Action++;
break;
case 1:
/*Go p2*/
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y);
Strat_Action++;
break;
case 2:
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj, BUILD_AREA_1b1_X-30,BUILD_AREA_1b1_Y);
else
trajectory_goto_xy_abs(&beudatraj, BUILD_AREA_1b4_X-30,BUILD_AREA_1b4_Y);
Strat_Action++;
break;
case 3:
trajectory_a_abs(&beudatraj,0);
Strat_Action++;
break;
case 4:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
trajectory_d_a_rel(&beudatraj, 30, 0);
Strat_Action++;
break;
case 5:
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_EJECT);
starttime = gettime();
Strat_Action++;
break;
case 6:
if(gettime()>starttime + DELAY_TAPIS_EJECT)
Strat_Action++;
break;
case 7:
trajectory_d_a_rel(&beudatraj, -30, 0);
Strat_Action++;
break;
case 8:
Strat_Action=0;
Strat_State=STRAT_EXIT;
break;
}
}
}
void strat_get_elt_rand_disp_position(void){
int8_t i2c_ans;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_SLOW, QR_SPEED_D_NEG_SLOW); // Vitesse
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y+30);
else
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
Strat_Action++;
break;
case 1:
if(beudaoa.state == OA_IN_TARGET){
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
trajectory_d_a_rel(&beudatraj,30,0);
}
Strat_Action++;
break;
case 2:
i2c_ans=i2c_send_and_receive(I2C_CAPTEUR_IR_ADDR,I2C_IR_CHECK_IN_FRONT);
if(i2c_ans==1){
/*disp is not here*/
//rand_disp_position=ELT_LOCATION_RAND2;
Strat_Action++;
}
else if(i2c_ans==2) {
/*Yeay we got it ! !*/
Strat_Action=0;
Strat_State = STRAT_CONSTRUCT_TEMPLE_2_1;
//rand_disp_position=ELT_LOCATION_RAND1;
}
break;
case 3:
trajectory_d_a_rel(&beudatraj,-50,0);
Strat_Action++;
break;
case 4:
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
else
trajectory_goto_xy_abs(&beudatraj, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
break;
case 5:
/*the disp isn't the 1st, we get to the 2nd*/
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y+30);
else
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
Strat_Action++;
break;
}
}
}
void strat_go_get_elts_disp(struct strategy *strat,int8_t type){ //type = 0 FIXED,1 RAND1,2 RAND2
static uint8_t elts_to_store;
uint24_t starttime;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_A_POS_SLOW, QR_SPEED_A_NEG_SLOW); // Vitesse
if(beudabot.color==COLOR_GREEN){
if(type==0)
oa_set_target(&beudaoa, G_ELT_DISP_FIXED_X-30, G_ELT_DISP_FIXED_Y);
else if(type==1)
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
else if(type==2)
oa_set_target(&beudaoa, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
}
else{
if(type==0)
oa_set_target(&beudaoa, R_ELT_DISP_FIXED_X-30, R_ELT_DISP_FIXED_Y);
else if(type==1)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y-30);
else if(type==2)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_2_X, R_ELT_DISP_RAND_2_Y-30);
}
Strat_Action++;
break;
case 1:
if(beudaoa.state == OA_IN_TARGET)
if(beudabot.color ==COLOR_GREEN){
if(type==0)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_FIXED_X, G_ELT_DISP_FIXED_Y);
else if(type==1)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y);
else if (type==2)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y);
}
else{
if(type==0)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_FIXED_X, R_ELT_DISP_FIXED_Y);
else if(type==1)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y);
else if (type==2)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_RAND_2_X, R_ELT_DISP_RAND_2_Y);
}
Strat_Action++;
break;
case 2:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
trajectory_d_a_rel(&beudatraj, ELT_DISP_IN_OUT_DIST, 0); //we move till the disp
starttime = gettime();
Strat_Action++;
clearBit(strat_flags,TWO_ELTS_LOADED); //we prepare the flag...
break;
case 3:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
if(elts_to_store==2){
/*Woow I got 2 elts, let's store them now, I suppose the tapis-roulo stoped rolling by himself now..*/
if(strat_store_elt(elts_to_store)){
/*if storage proc ended...*/
Strat_Action++; //we got 2, let's take some other two..
setBit(strat_flags,TWO_ELTS_LOADED);
}
}
else{
//if it's taking too much time we go builg smtg
/*we make a special move to get them....or not*/
if(gettime()> starttime + DELAY_TAPIS_BOUFFE)
Strat_Action++;
}
break;
case 4:
/*we suck again here...*/
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
Strat_Action++;
break;
case 5:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
if(elts_to_store==2){
/*Woow I got 2 elts, let's store them now, I suppose the tapis-roulo stoped rolling by himself now..*/
if(strat_store_elt(elts_to_store)){
/*if storage proc ended...*/
Strat_State=STRAT_CONSTRUCT_TEMPLE_2_1; //we got 2, let's build smtg
setBit(strat_flags,I_CAN_BUILD_A_TEMPLE);
}
}
else{
//if it's taking too much time we go builg smtg
/*we make a special move to get them....or not*/
if(gettime()> starttime + DELAY_TAPIS_BOUFFE){
Strat_State=STRAT_CONSTRUCT_TEMPLE_2_1;
/*we can't wait more... let's just build smtg*/
clearBit(strat_flags,I_CAN_BUILD_A_TEMPLE);//without a lint...
}
}
break;
}
}
}
void strat_go_get_lint(struct strategy * strat,uint8_t position){
static uint24_t starttime;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_A_POS_SLOW, QR_SPEED_A_NEG_SLOW); // Vitesse
if(position==1){
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, G_LINT_DISP_1_X-20,G_LINT_DISP_1_Y);
else
oa_set_target(&beudaoa, R_LINT_DISP_1_X-20,R_LINT_DISP_1_Y);
}
else if(position==2){
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, G_LINT_DISP_2_X-20,G_LINT_DISP_2_Y);
else
oa_set_target(&beudaoa, R_LINT_DISP_2_X-20,R_LINT_DISP_2_Y);
}
Strat_Action++;
break;
case 1:
if(beudaoa.state==OA_IN_TARGET){
i2c_send_only(I2C_LINTO_ADDR,I2C_LINT_GET_DOWN);
Strat_Action++;
}
break;
case 2:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
trajectory_d_a_rel(&beudatraj, 20, 0);
Strat_Action++;
break;
case 3:
//i2c lint remont
i2c_send_only(I2C_LINTO_ADDR,I2C_LINT_GET_UP);
starttime = gettime();
Strat_Action++;
break;
case 4:
if(gettime() > starttime + DELAY_LINT_UP){
if(i2c_send_and_receive(I2C_LINTO_ADDR, I2C_GOT_SMTG)==1){
//go get another one, somewhere else...
Strat_Action = 0;
Strat_State = STRAT_GET_LINT_2;
}
else{ //2
#ifdef DEBUG_STRAT
UART_CPutString("strat> we got a lint !! \r\n");
#endif
Strat_Action = 0;
Strat_State = STRAT_GET_ELT_ON_FIXED_DISP;
}
}
break;
}
}
}
void strat_construct_temple(struct strategy * strat,uint8_t build_area,uint8_t half){
static uint24_t starttime;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
if(build_area==3){
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, BUILD_AREA_3b1_X,BUILD_AREA_3b1_Y);
else
oa_set_target(&beudaoa, BUILD_AREA_3b2_X,BUILD_AREA_3b2_Y);
}
else if(build_area==2){
if(half == 1)
oa_set_target(&beudaoa, BUILD_AREA_2b1_X,BUILD_AREA_2b1_Y);
else
oa_set_target(&beudaoa, BUILD_AREA_2b2_X,BUILD_AREA_2b2_Y);
}
else if(build_area==1){
/*here we should check our pos and determinate the closest one*/
if(half == 1)
oa_set_target(&beudaoa, BUILD_AREA_1b1_X,BUILD_AREA_1b1_Y);
else if(half == 2)
oa_set_target(&beudaoa, BUILD_AREA_1b2_X,BUILD_AREA_1b2_Y);
else if(half == 3)
oa_set_target(&beudaoa, BUILD_AREA_1b3_X,BUILD_AREA_1b3_Y);
else if(half == 4)
oa_set_target(&beudaoa, BUILD_AREA_1b4_X,BUILD_AREA_1b4_Y);
}
Strat_Action++;
break;
case 1:
if(beudaoa.state==OA_IN_TARGET){
//trajectory_turnto_xy_behind(&beudatraj, 105, 150);
if(build_area==3){
if(beudabot.color==COLOR_GREEN)
trajectory_a_abs(&beudatraj,-90);
else
trajectory_a_abs(&beudatraj,90);
}
else if(build_area==2 || build_area==1){
trajectory_a_abs(&beudatraj,180);
}
}
Strat_Action++;
break;
case 2:
//here whe sould give the back to the build area x
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
trajectory_d_a_rel(&beudatraj, -20, 0);
Strat_Action++;
break;
case 3:
//envoi d'ordres i2c à la pinc pour construire
i2c_send_only(I2C_PINCE_ADDR,I2C_PINCE_CONSTRUCT);
starttime = gettime();
Strat_Action++;
break;
case 4:
if((gettime() > starttime + DELAY_PINCE_CONSTRUCTION)){
if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==0){
//we wait more
if(gettime() > (starttime + 2*DELAY_PINCE_CONSTRUCTION)){
//there is a problem here...
//drop the shit and let's continue the sequ...
//pince is maybe just f****d up ...
/*A CHANGER !!!!!*/
Strat_State = STRAT_EMERGENCY_BEHAVIOUR;
#ifdef DEBUG_STRAT
UART_CPutString("Pince is down... !\r\n");
#endif
setBit(strat_flags,PINCE_IS_SHITTY);
}
}
else if (i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==2){
//Damn it! there is already a construction here... let's move and try somewhere else!..
Strat_Action = 0;
//if(Strat_State <STRAT_CONSTRUCT_TEMPLE_1_2)
strat_go_next_build_area(); //this function will change strat state to build somewhere else...
current_building_area++; //for next call of strat_go_next_build_area();
#ifdef DEBUG_STRAT
UART_CPutString("Playing against RCVA || Microb ???!\r\n");
#endif
}
else if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==1){
//finish building, let's put the lint if flag I_CAN_BUILD_A_TEMPLE is set
Strat_Action++;
}
}
break;
case 5:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
trajectory_d_a_rel(&beudatraj, -20, 0);
}
Strat_Action++;
break;
case 6:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
trajectory_d_a_rel(&beudatraj, 0, 180);
}
Strat_Action++;
break;
case 7:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
trajectory_d_a_rel(&beudatraj, 20, 0);
}
Strat_Action++;
break;
case 8:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
i2c_send_only(I2C_LINTO_ADDR, I2C_LINT_DEPOSIT_DOWN);
starttime = gettime();
}
Strat_Action++;
break;
case 9:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
if(gettime() > starttime + DELAY_LINT_DOWN){
Strat_State=STRAT_GET_LINT_1;
}
}
else
Strat_State=STRAT_GET_LINT_1;
//get a lint, then elts, to build another one...
break;
}
}
}
