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);
}
