void dual_quadramp_do_filter(struct dual_quadramp_filter *q) { int32_t x_speed, y_speed; int32_t x_acc, y_acc; int x_distance, y_distance; x_distance = abs(q->x_pos - q->x_consign); y_distance = abs(q->y_pos - q->y_consign); x_speed = (int32_t)(q->speed * x_distance / (float)(x_distance + y_distance)); y_speed = (int32_t)(q->speed * y_distance / (float)(x_distance + y_distance)); x_acc = (int32_t)(q->acc * x_distance / (float)(x_distance + y_distance)); y_acc = (int32_t)(q->acc * y_distance / (float)(x_distance + y_distance)); quadramp_set_1st_order_vars(&q->x_quadramp, x_speed, x_speed); quadramp_set_2nd_order_vars(&q->x_quadramp, x_acc, x_acc); quadramp_set_1st_order_vars(&q->y_quadramp, y_speed, y_speed); quadramp_set_2nd_order_vars(&q->y_quadramp, y_acc, y_acc); q->x_pos = quadramp_do_filter((void *)&q->x_quadramp, q->x_consign); q->y_pos = quadramp_do_filter((void *)&q->y_quadramp, q->y_consign); }
/** set speed consign in quadramp filter */ void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed) { struct quadramp_filter * q_d, * q_a; q_d = traj->csm_distance->consign_filter_params; q_a = traj->csm_angle->consign_filter_params; quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed)); quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed)); }
int main(void) { struct quadramp_filter q; int32_t in=0, out=0, prev_out=0; int32_t i=0; quadramp_init(&q); for (i=0 ; i<4000 ; i++) { switch(i) { case 0: quadramp_set_1st_order_vars(&q, 50, 100); quadramp_set_2nd_order_vars(&q, 1, 2); in = 10000; break; case 600: in = 9000; break; case 630: in = 10000; break; case 1000: in = -5000; break; case 1500: in = -4000; break; case 2000: quadramp_set_1st_order_vars(&q, 10, 10); quadramp_set_2nd_order_vars(&q, 2, 2); in = 10000; break; case 3000: quadramp_set_1st_order_vars(&q, 100, 100); break; default: break; } out = quadramp_do_filter(&q, in); printf("%" SCNu32 " %" SCNu32 " %" SCNu32 " %" SCNu32 "\n", i, in, out, out-prev_out); prev_out = out; } return 0; }
void strat_go_get_elts_on_table(struct strategy * strat){ static uint8_t elts_to_store; static uint24_t starttime; static uint8_t storage_lock; 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*/ 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: Strat_Action=0; Strat_State=STRAT_EMERGENCY_BEHAVIOUR; break; /* case 2: //before continuing the seq, we check how many elts we got elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN); Strat_Action++; break;*/ case 3: if(elts_to_store==2){ if(strat_store_elt(elts_to_store)){ Strat_Action++; setBit(strat_flags,TWO_ELTS_LOADED); i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); //after storage, it stops bouffing ?? } } else //if we don't have 2elts we just continu the seq. Strat_Action++; break; case 4: /*here, if we got 1 or 2 elements, they're stored, and the pince is well placed*/ quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_SLOW, QR_SPEED_D_NEG_SLOW); // Vitesse if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+25); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-25); Strat_Action++; break; case 5: elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN); Strat_Action++; break; case 6: if(elts_to_store==2){ if(strat_store_elt(elts_to_store)){ /*Warning: no timeout here, we trust the tapis-roulo at this time...*/ if(strat_flags & TWO_ELTS_LOADED){ //Yes !! we got 4 elts, lets build a real temple! i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment Strat_Action = 0; Strat_State++; setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation... clearBit(strat_flags,TWO_ELTS_LOADED); } } } else Strat_Action++; break; case 7: /* here the situation is: - we got 2 elts stored, and we are looking for 2 others... - we have nothing.. (check strat_flags & TWO_ELTS_LOADED the know the answer...) */ if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+25); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-25); Strat_Action++; break; case 8: elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN); Strat_Action++; break; case 9: if(elts_to_store==2){ //owkey, we have now 2 elts if(strat_store_elt(elts_to_store)){ if(strat_flags & TWO_ELTS_LOADED){ //AWESOME, we got 2elts inside, and 2elts stored... i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment Strat_Action = 0; Strat_State++; //let's build !! clearBit(strat_flags,TWO_ELTS_LOADED); setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation lock... } } } else Strat_Action++; break; case 10: if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+50); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-50); Strat_Action++; break; case 11: elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN); Strat_Action++; break; case 12: if(elts_to_store==2){ if(strat_store_elt(elts_to_store)){ if(strat_flags & TWO_ELTS_LOADED){ //AWESOME, we got 2elts inside, and 2elts stored... Strat_Action = 0; i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment Strat_State++; //next op is building... setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation... clearBit(strat_flags,TWO_ELTS_LOADED); } } } else Strat_Action++; break; case 13: if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+50); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-50); Strat_Action++; break; case 14: /*I still don't have 4 elts :(, let's build with what we have*/ UART_CPutString("Going to build with less than 4elts :( go back and try again ???\r\n"); Strat_Action=0; Strat_State++; clearBit(strat_flags,TWO_ELTS_LOADED); break; } }
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; } } }
void robot_cs_init(robot_cs_t* rcs) { // zeroing structures rcs->hrs = NULL; rcs->hpm = NULL; // set CS on rcs->active = 1; // CS not reactivated since last tick rcs->reactivated = 0; // setup PIDs pid_init(&pid_x); pid_init(&pid_y); pid_init(&pid_angle); pid_set_gains(&pid_x, SETTING_PID_X_GAIN_P, SETTING_PID_X_GAIN_I, SETTING_PID_X_GAIN_D); pid_set_maximums(&pid_x, SETTING_PID_X_MAX_IN, SETTING_PID_X_MAX_I, SETTING_PID_X_MAX_OUT); pid_set_out_shift(&pid_x, SETTING_PID_X_SHIFT); pid_set_gains(&pid_y, SETTING_PID_Y_GAIN_P, SETTING_PID_Y_GAIN_I, SETTING_PID_Y_GAIN_D); pid_set_maximums(&pid_y, SETTING_PID_Y_MAX_IN, SETTING_PID_Y_MAX_I, SETTING_PID_Y_MAX_OUT); pid_set_out_shift(&pid_y, SETTING_PID_Y_SHIFT); pid_set_gains(&pid_angle, SETTING_PID_A_GAIN_P, SETTING_PID_A_GAIN_I, SETTING_PID_A_GAIN_D); pid_set_maximums(&pid_angle, SETTING_PID_A_MAX_IN, SETTING_PID_A_MAX_I, SETTING_PID_A_MAX_OUT); pid_set_out_shift(&pid_angle, SETTING_PID_A_SHIFT); // quadramp quadramp_set_1st_order_vars(&qramp_angle, SETTING_QRAMP_A_SPEED, SETTING_QRAMP_A_SPEED); quadramp_set_2nd_order_vars(&qramp_angle, SETTING_QRAMP_A_ACC, SETTING_QRAMP_A_ACC); // setup CSMs cs_init(&csm_x); cs_init(&csm_y); cs_init(&csm_angle); cs_set_consign_filter(&csm_x, NULL, NULL); cs_set_consign_filter(&csm_y, NULL, NULL); cs_set_consign_filter(&csm_angle, &quadramp_do_filter, &qramp_angle); cs_set_correct_filter(&csm_x, &pid_do_filter, &pid_x); cs_set_correct_filter(&csm_y, &pid_do_filter, &pid_y); cs_set_correct_filter(&csm_angle, &pid_do_filter, &pid_angle); cs_set_feedback_filter(&csm_x, NULL, NULL); cs_set_feedback_filter(&csm_y, NULL, NULL); cs_set_feedback_filter(&csm_angle, NULL, NULL); cs_set_process_out(&csm_x, &get_robot_x, rcs); cs_set_process_out(&csm_y, &get_robot_y, rcs); cs_set_process_out(&csm_angle, &get_robot_a, rcs); cs_set_process_in(&csm_x, NULL, NULL); cs_set_process_in(&csm_y, NULL, NULL); cs_set_process_in(&csm_angle, NULL, NULL); }
void microb_cs_init(void) { /* ROBOT_SYSTEM */ rs_init(&mainboard.rs); rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM); /* increase gain to decrease dist, increase left and it will turn more left */ rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value, LEFT_ENCODER, IMP_COEF * -1.0000); rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, RIGHT_ENCODER, IMP_COEF * 1.0000); /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); /* POSITION MANAGER */ position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); //position_set_centrifugal_coef(&mainboard.pos, 0.000016); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ trajectory_init(&mainboard.traj); trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs, &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); /* ---- CS angle */ /* PID */ pid_init(&mainboard.angle.pid); pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); pid_set_out_shift(&mainboard.angle.pid, 10); pid_set_derivate_filter(&mainboard.angle.pid, 4); /* QUADRAMP */ quadramp_init(&mainboard.angle.qr); quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ /* CS */ cs_init(&mainboard.angle.cs); cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr); cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid); cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs); cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs); cs_set_consign(&mainboard.angle.cs, 0); /* Blocking detection */ bd_init(&mainboard.angle.bd); bd_set_speed_threshold(&mainboard.angle.bd, 80); bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); /* QUADRAMP */ quadramp_init(&mainboard.distance.qr); quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ /* CS */ cs_init(&mainboard.distance.cs); cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr); cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid); cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs); cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs); cs_set_consign(&mainboard.distance.cs, 0); /* Blocking detection */ bd_init(&mainboard.distance.bd); bd_set_speed_threshold(&mainboard.distance.bd, 60); bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); /* ---- CS fessor */ fessor_autopos(); /* PID */ pid_init(&mainboard.fessor.pid); pid_set_gains(&mainboard.fessor.pid, 300, 10, 150); pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.fessor.pid, 10); pid_set_derivate_filter(&mainboard.fessor.pid, 4); /* CS */ cs_init(&mainboard.fessor.cs); cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid); cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL); cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC); fessor_up(); /* ---- CS elevator */ elevator_autopos(); /* PID */ pid_init(&mainboard.elevator.pid); pid_set_gains(&mainboard.elevator.pid, 300, 10, 150); pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095); pid_set_out_shift(&mainboard.elevator.pid, 10); pid_set_derivate_filter(&mainboard.elevator.pid, 4); /* CS */ cs_init(&mainboard.elevator.cs); cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid); cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL); cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC); elevator_down(); /* ---- CS wheel */ /* PID */ pid_init(&mainboard.wheel.pid); pid_set_gains(&mainboard.wheel.pid, 100, 100, 0); pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095); pid_set_out_shift(&mainboard.wheel.pid, 5); pid_set_derivate_filter(&mainboard.wheel.pid, 4); /* CS */ cs_init(&mainboard.wheel.cs); cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid); cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL); cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL); cs_set_consign(&mainboard.wheel.cs, 1000); /* set them on !! */ mainboard.angle.on = 0; mainboard.distance.on = 0; mainboard.fessor.on = 1; mainboard.elevator.on = 0; mainboard.wheel.on = 1; mainboard.flags |= DO_CS; scheduler_add_periodical_event_priority(do_cs, NULL, 5000L / SCHEDULER_UNIT, CS_PRIO); }
void htrajectory_setASpeed( htrajectory_t *htj, double speed, double acc ) { quadramp_set_1st_order_vars(htj->qramp_a, speed, speed); quadramp_set_2nd_order_vars(htj->qramp_a, acc, acc); }