/* * 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; } } }