#define ROLLER_OFF 0
#define ROLLER_REVERSE ROLLER_SPEED
-#define FORKROT_DEPLOYED -50000
+#define FORKROT_DEPLOYED -55000
+#define FORKROT_MID -33000
#define FORKROT_PACKED 0
-#define FORKTRANS_LEFT 0
-#define FORKTRANS_MIDDLE 500
-#define FORKTRANS_RIGHT 1000
-
void roller_on(void)
{
cs_set_consign(&ballboard.roller.cs, ROLLER_ON);
cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
}
-void fork_left(void)
-{
- cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_LEFT);
-}
-
-void fork_right(void)
+void fork_mid(void)
{
- cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_RIGHT);
+ cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID);
}
-void fork_middle(void)
+void actuator_init(void)
{
- cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_MIDDLE);
+ printf_P(PSTR("fork autopos..."));
+ pwm_ng_set(FORKROT_PWM, 400);
+ wait_ms(1000);
+ pwm_ng_set(FORKROT_PWM, 0);
+ encoders_spi_set_value(FORKROT_ENCODER, 0);
+ printf_P(PSTR("ok\r\n"));
+ ballboard.forkrot.on = 1;
}
void fork_deploy(void);
void fork_pack(void);
-void fork_left(void);
-void fork_right(void);
-void fork_middle(void);
+void fork_mid(void);
+
+void actuator_init(void);
state_set_mode(I2C_BALLBOARD_MODE_EJECT);
else if (!strcmp_P(res->arg1, PSTR("harvest")))
state_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+ else if (!strcmp_P(res->arg1, PSTR("prepare")))
+ state_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+ else if (!strcmp_P(res->arg1, PSTR("take")))
+ state_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
/* other commands */
}
prog_char str_state1_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
-prog_char str_state1_arg1[] = "init#eject#harvest#off";
+prog_char str_state1_arg1[] = "init#eject#harvest#off#prepare#take";
parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
prog_char help_state1[] = "set ballboard mode";
static void cmd_state2_parsed(void *parsed_result,
__attribute__((unused)) void *data)
{
- struct cmd_state2_result *res = parsed_result;
- uint8_t mode;
-
- if (!strcmp_P(res->arg2, PSTR("left"))) {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
- }
- else {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
- }
- //state_set_mode(mode);
}
prog_char str_state2_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "prepare#take";
+prog_char str_state2_arg1[] = "xxx";
parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
prog_char str_state2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
fork_deploy();
else if (!strcmp_P(res->arg1, PSTR("pack")))
fork_pack();
- else if (!strcmp_P(res->arg1, PSTR("left")))
- fork_left();
- else if (!strcmp_P(res->arg1, PSTR("right")))
- fork_right();
else if (!strcmp_P(res->arg1, PSTR("middle")))
- fork_middle();
+ fork_mid();
}
prog_char str_fork_arg0[] = "fork";
parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
-prog_char str_fork_arg1[] = "deploy#pack#left#right#middle";
+prog_char str_fork_arg1[] = "deploy#pack#middle";
parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
prog_char help_fork[] = "move fork";
/* QUADRAMP */
quadramp_init(&ballboard.forkrot.qr);
- quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */
+ quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 800); /* set speed */
quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
/* CS */
/* set them on !! */
ballboard.roller.on = 1;
ballboard.forktrans.on = 1;
- ballboard.forkrot.on = 1;
+ ballboard.forkrot.on = 0;
scheduler_add_periodical_event_priority(do_cs, NULL,
&PORTD, 4);
PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
&PORTD, 5);
- PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
- &PORTD, 6);
+ PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED, &PORTD, 6);
PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
&PORTD, 7);
sei();
+ actuator_init();
+
printf_P(PSTR("\r\n"));
printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
#define OFF I2C_BALLBOARD_MODE_OFF
#define HARVEST I2C_BALLBOARD_MODE_HARVEST
#define EJECT I2C_BALLBOARD_MODE_EJECT
-#define PREP_L_FORK I2C_BALLBOARD_MODE_PREP_L_FORK
-#define TAKE_L_FORK I2C_BALLBOARD_MODE_TAKE_L_FORK
-#define PREP_R_FORK I2C_BALLBOARD_MODE_PREP_R_FORK
-#define TAKE_R_FORK I2C_BALLBOARD_MODE_TAKE_R_FORK
+#define PREP_FORK I2C_BALLBOARD_MODE_PREP_FORK
+#define TAKE_FORK I2C_BALLBOARD_MODE_TAKE_FORK
uint8_t state_debug = 0;
case INIT:
state_init();
+ fork_pack();
state_mode = OFF;
state_status = I2C_BALLBOARD_STATUS_F_READY;
break;
case OFF:
state_status = I2C_BALLBOARD_STATUS_F_READY;
roller_off();
+ fork_pack();
break;
case HARVEST:
state_status = I2C_BALLBOARD_STATUS_F_READY;
+ fork_pack();
state_do_harvest();
break;
case EJECT:
state_status = I2C_BALLBOARD_STATUS_F_BUSY;
+ fork_pack();
state_do_eject();
state_status = I2C_BALLBOARD_STATUS_F_READY;
state_mode = HARVEST;
break;
+ case PREP_FORK:
+ roller_off();
+ fork_deploy();
+ break;
+
+ case TAKE_FORK:
+ roller_off();
+ fork_mid();
+ time_wait_ms(1300);
+ state_mode = OFF;
+ break;
+
default:
break;
}
state_init();
else if (!strcmp_P(res->arg1, PSTR("eject")))
state_set_mode(I2C_COBBOARD_MODE_EJECT);
- else if (!strcmp_P(res->arg1, PSTR("kickstand")))
- state_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+ state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+ state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
else if (!strcmp_P(res->arg1, PSTR("ignore_i2c")))
state_set_i2c_ignore(1);
else if (!strcmp_P(res->arg1, PSTR("care_i2c")))
prog_char str_state1_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
-prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand";
+prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand_up#kickstand_down";
parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
prog_char help_state1[] = "set cobboard mode";
#define SHOVEL_DOWN 100
#define SHOVEL_MID 4500
#define SHOVEL_UP 11000
-#define SHOVEL_KICKSTAND 12800
+#define SHOVEL_KICKSTAND_UP 12800
+#define SHOVEL_KICKSTAND_DOWN 10000
static int32_t shovel_k1 = 1000;
static int32_t shovel_k2 = 20;
void shovel_down(void)
{
+ shovel_current_limit_enable(0);
quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);
quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 80);
cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN);
void shovel_mid(void)
{
+ shovel_current_limit_enable(0);
quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);
quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 80);
cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID);
void shovel_up(void)
{
+ shovel_current_limit_enable(0);
if (state_get_cob_count() <= 1)
quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500);
else
cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP);
}
-void shovel_kickstand(void)
+void shovel_kickstand_up(void)
{
+ shovel_set_current_limit_coefs(1000, 20);
+ shovel_current_limit_enable(1);
quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200);
quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10);
- cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND);
+ cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_UP);
+}
+
+void shovel_kickstand_down(void)
+{
+ shovel_set_current_limit_coefs(500, 0);
+ shovel_current_limit_enable(1);
+ quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200);
+ quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10);
+ cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_DOWN);
}
uint8_t shovel_is_up(void)
void shovel_down(void);
void shovel_mid(void);
void shovel_up(void);
-void shovel_kickstand(void);
+void shovel_kickstand_up(void);
+void shovel_kickstand_down(void);
uint8_t shovel_is_up(void);
uint8_t shovel_is_down(void);
#define INIT(mode) ((mode) == I2C_COBBOARD_MODE_INIT)
#define HARVEST(mode) ((mode) == I2C_COBBOARD_MODE_HARVEST)
#define EJECT(mode) ((mode) == I2C_COBBOARD_MODE_EJECT)
-#define KICKSTAND(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND)
+#define KICKSTAND_UP(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND_UP)
+#define KICKSTAND_DOWN(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND_DOWN)
uint8_t state_debug = 0;
shovel_down();
servo_carry_close();
servo_door_close();
- shovel_current_limit_enable(0);
/* pack/deply spickles, enable/disable roller */
cobroller_off(I2C_LEFT_SIDE);
}
/* help to climb the hill */
- if (KICKSTAND(state_mode)) {
+ if (KICKSTAND_UP(state_mode)) {
+ state_status = I2C_COBBOARD_STATUS_KICKSTAND_UP;
servo_carry_open();
servo_door_open();
- shovel_current_limit_enable(1);
- shovel_kickstand();
+ shovel_kickstand_up();
+ }
+
+ if (KICKSTAND_DOWN(state_mode)) {
+ state_status = I2C_COBBOARD_STATUS_KICKSTAND_DOWN;
+ servo_carry_open();
+ servo_door_open();
+ shovel_kickstand_down();
}
/* eject */
#define I2C_COBBOARD_MODE_HARVEST 0x01 /* harvest mode */
#define I2C_COBBOARD_MODE_EJECT 0x02 /* eject cobs */
#define I2C_COBBOARD_MODE_INIT 0x03 /* init state machine */
-#define I2C_COBBOARD_MODE_KICKSTAND 0x04 /* help to climb the hill */
+#define I2C_COBBOARD_MODE_KICKSTAND_UP 0x04 /* help to climb the hill */
+#define I2C_COBBOARD_MODE_KICKSTAND_DOWN 0x05 /* help to climb the hill */
uint8_t mode;
};
#define I2C_BALLBOARD_MODE_OFF 0x01
#define I2C_BALLBOARD_MODE_HARVEST 0x02
#define I2C_BALLBOARD_MODE_EJECT 0x03
-#define I2C_BALLBOARD_MODE_PREP_L_FORK 0x04
-#define I2C_BALLBOARD_MODE_TAKE_L_FORK 0x05
-#define I2C_BALLBOARD_MODE_PREP_R_FORK 0x06
-#define I2C_BALLBOARD_MODE_TAKE_R_FORK 0x07
+#define I2C_BALLBOARD_MODE_PREP_FORK 0x04
+#define I2C_BALLBOARD_MODE_TAKE_FORK 0x05
uint8_t mode;
};
/* mode type are defined above: I2C_COBBOARD_MODE_xxx */
uint8_t mode;
-#define I2C_COBBOARD_STATUS_READY 0x00
-#define I2C_COBBOARD_STATUS_OFF 0x01
-#define I2C_COBBOARD_STATUS_LBUSY 0x02
-#define I2C_COBBOARD_STATUS_RBUSY 0x03
-#define I2C_COBBOARD_STATUS_EJECT 0x04
-#define I2C_COBBOARD_STATUS_KICKSTAND 0x05
+#define I2C_COBBOARD_STATUS_READY 0x00
+#define I2C_COBBOARD_STATUS_OFF 0x01
+#define I2C_COBBOARD_STATUS_LBUSY 0x02
+#define I2C_COBBOARD_STATUS_RBUSY 0x03
+#define I2C_COBBOARD_STATUS_EJECT 0x04
+#define I2C_COBBOARD_STATUS_KICKSTAND_UP 0x05
+#define I2C_COBBOARD_STATUS_KICKSTAND_DOWN 0x06
uint8_t status;
uint8_t cob_count;
IRQ_LOCK(flags);
if (opp_age < 3)
opp_age ++;
- else
+ else {
beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
+ IRQ_UNLOCK(flags);
+ return;
+ }
ia = opp_a;
id = opp_d;
}
else {
#ifdef HOST_VERSION
-#define PWM_INTERACT 300
+#define PWM_INTERACT 1200
#else
#define PWM_INTERACT 1200
#endif
i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
else if (!strcmp_P(res->arg1, PSTR("eject")))
i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
- else if (!strcmp_P(res->arg1, PSTR("kickstand")))
- i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
-prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand";
+prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
else if (!strcmp_P(res->arg1, PSTR("harvest")))
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+ else if (!strcmp_P(res->arg1, PSTR("prepare")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+ else if (!strcmp_P(res->arg1, PSTR("take")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
/* other commands */
}
prog_char str_ballboard_setmode1_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
-prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
+prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare";
parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
/* function called when cmd_ballboard_setmode2 is parsed successfully */
static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
{
- struct cmd_ballboard_setmode2_result *res = parsed_result;
- uint8_t mode = I2C_BALLBOARD_MODE_INIT;
-
- if (!strcmp_P(res->arg2, PSTR("left"))) {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
- }
- else {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
- }
- i2c_ballboard_set_mode(mode);
}
prog_char str_ballboard_setmode2_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
-prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
+prog_char str_ballboard_setmode2_arg1[] = "xxx";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
prog_char str_ballboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
else if (!strcmp_P(res->arg1, PSTR("set"))) {
position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
}
- else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
+ else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) ||
+ !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
mainboard.our_color = I2C_COLOR_BLUE;
#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
#endif
auto_position();
}
- else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
+ else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) ||
+ !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
mainboard.our_color = I2C_COLOR_YELLOW;
#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
auto_position();
}
+ if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
+ strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
+ prepare_hill(I2C_COLOR_BLUE, 340);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
+ strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
+ prepare_hill(I2C_COLOR_YELLOW, 340);
+ }
+
/* else it's just a "show" */
printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
position_get_x_double(&mainboard.pos),
prog_char str_position_arg0[] = "position";
parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
-prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
+prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow";
parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
prog_char help_position[] = "Show/reset (x,y,a) position";
parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
-parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
+parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8);
prog_char help_strat_conf3[] = "configure strat options";
parse_pgm_inst_t cmd_strat_conf3 = {
if (!strcmp_P(res->arg1, PSTR("test")))
subtraj_test();
- else if (!strcmp_P(res->arg1, PSTR("orange")))
- subtraj_test();
+ else if (!strcmp_P(res->arg1, PSTR("orange_yellow")))
+ run_to_the_hills(I2C_COLOR_YELLOW);
+ else if (!strcmp_P(res->arg1, PSTR("orange_blue")))
+ run_to_the_hills(I2C_COLOR_BLUE);
else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
position_set(&mainboard.pos, 2625,
COLOR_Y(1847), COLOR_A(0.00));
prog_char str_subtraj_arg0[] = "subtraj";
parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
-prog_char str_subtraj_arg1[] = "test#orange#tomato";
+prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#tomato";
parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
if not m:
m = re.match("cobboard=%s,%s"%(INT,INT), l)
if m:
- #print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
+ print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
side = int(m.groups()[0])
flags = int(m.groups()[1])
if side == 0:
#include "../common/i2c_commands.h"
#include "i2c_protocol.h"
#include "main.h"
+#include "cs.h"
#include "strat.h"
#include "strat_db.h"
#include "strat_base.h"
uint8_t k;
for (k = 0; k < TOMATO_NB; k++) {
- if (strat_db.tomato_table[k]->time_removed != -1 &&
- strat_db.tomato_table[k]->time_removed + 2 == time_get_s()) {
+ if (strat_db.tomato_table[k]->present == 1 &&
+ strat_db.tomato_table[k]->time_removed != -1 &&
+ strat_db.tomato_table[k]->time_removed + 2 <= time_get_s()) {
#ifdef HOST_VERSION
printf("remove tomato %d\n", k);
#endif
uint8_t i;
for (i = 0; i < CORN_NB; i++) {
- if (strat_db.corn_table[i]->time_removed != -1 &&
- strat_db.corn_table[i]->time_removed + 2 == time_get_s()) {
+ if (strat_db.corn_table[i]->present == 1 &&
+ strat_db.corn_table[i]->time_removed != -1 &&
+ strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) {
#ifdef HOST_VERSION
printf("remove cob %d\n", i);
#endif
}
/* go to eject point */
- trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
+ trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
TRAJ_FLAGS_NO_NEAR);
/* err is never == 0 because speedify_eject() always return 0 */
}
#endif
+/* get tomatoes near our goals (12,5 and 12,3) */
+uint8_t get_opp_oranges(void)
+{
+ int16_t x, y;
+ uint8_t i, j;
+ uint8_t err;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ /* only if oranges are present */
+ if (strat_db.opp_oranges_count == 0)
+ return END_TRAJ;
+
+ strat_db.opp_oranges_count = 0;
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+ return END_ERROR;
+
+ /* not on eject point */
+ if (i != 11 || j != 6)
+ return END_ERROR;
+
+ strat_want_pack = 1;
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ /* turn in the correct direction */
+ trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
+ err = wait_traj_end(TRAJ_FLAGS_STD);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ err = run_to_the_hills(get_opponent_color());
+
+ fail:
+ strat_want_pack = 0;
+ return err;
+}
+
/* get tomatoes near our goals (12,5 and 12,3) */
uint8_t get_orphan_tomatoes(void)
{
trajectory_d_rel(&mainboard.traj, -250);
err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
TRAJ_FLAGS_NO_NEAR);
-#ifdef HOST_VERSION /* simulator sees 2 blockings */
- if (err == END_BLOCKING) {
- trajectory_d_rel(&mainboard.traj, -250);
- err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
- TRAJ_FLAGS_NO_NEAR);
- }
-#endif
if (err != 0 && !TRAJ_SUCCESS(err))
goto fail;
return err;
}
-/* get oranges, must be called near game area */
-uint8_t run_to_the_hills(uint8_t orange_color)
-{
-#define HILL_LEN 500
-#define HILL_POSY_YELLOW 300
-#define HILL_POSY_BLUE 200
-#define HILL_POSX_BALLS_DOWN 900
-#define HILL_START_POSX 580
- double aa, ad;
- uint16_t sa, sd;
- uint8_t err;
- uint8_t our_color = get_color();
- int16_t startx, starty;
+#define HILL_LEN 1000
- strat_want_pack = 1;
- strat_get_acc(&ad, &aa);
- strat_get_speed(&sd, &sa);
+#define HILL_ANGLE 0
+#define HILL_POSY_YELLOW 310
+#define HILL_POSY_BLUE 190
- DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+#define HILL_POSX_BALLS_DOWN1 830
+#define HILL_POSX_BALLS_DOWN2 920
+#define HILL_POSX_BALLS_DOWN3 730
+#define HILL_START_POSX 580
+
+uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
+{
+ int16_t startx, starty;
+ uint8_t our_color = get_color();
+ uint8_t err;
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
if (orange_color == I2C_COLOR_YELLOW)
starty = HILL_POSY_YELLOW;
else
starty = HILL_POSY_BLUE;
if (orange_color == our_color)
- startx = HILL_START_POSX;
+ startx = posx;
else
- startx = AREA_X - HILL_START_POSX;
- trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
+ startx = AREA_X - posx;
+ trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ return err;
/* turn to the hills */
if (orange_color == our_color)
- trajectory_a_abs(&mainboard.traj, COLOR_A(0));
+ trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
else
- trajectory_a_abs(&mainboard.traj, COLOR_A(180));
+ trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ if (!TRAJ_SUCCESS(err))
+ return err;
- strat_set_acc(3, 3);
- strat_set_speed(300, 500);
- bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40);
+ return END_TRAJ;
+}
+
+/* get oranges, must be called near game area */
+uint8_t run_to_the_hills(uint8_t orange_color)
+{
+ double aa, ad;
+ uint16_t sa, sd;
+ uint8_t err;
+ uint8_t our_color = get_color();
+ int32_t p = pid_get_gain_P(&mainboard.angle.pid);
+ int32_t i = pid_get_gain_I(&mainboard.angle.pid);
+ int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+ int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
+ int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
+ int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+
+ strat_want_pack = 1;
+ strat_get_acc(&ad, &aa);
+ strat_get_speed(&sd, &sa);
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ err = prepare_hill(orange_color, HILL_START_POSX);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ strat_set_acc(5, ACC_ANGLE);
+ strat_set_speed(300, SPEED_ANGLE_SLOW);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
+ bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
+ bd_set_speed_threshold(&mainboard.distance.bd, 10);
support_balls_pack();
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+
+ /* decrease angle gains */
+ pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
/* here it is difficult to handle return values, because we
* are on the hill */
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
trajectory_d_rel(&mainboard.traj, HILL_LEN);
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > HILL_POSX_BALLS_DOWN,
- TRAJ_FLAGS_NO_NEAR);
- printf_P(PSTR("deploy\r\n"));
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+ HILL_POSX_BALLS_DOWN1,
+ TRAJ_FLAGS_SMALL_DIST);
+ DEBUG(E_USER_STRAT, "deploy support balls");
support_balls_deploy();
- i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ trajectory_only_a_rel(&mainboard.traj, 2);
+ err = WAIT_COND_OR_TE_TO(0, 0, 2200);
- time_wait_ms(2000);
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
+ strat_set_acc(3, 3);
+ strat_hardstop();
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
+
+ time_wait_ms(1800);
/* reach top, go down */
trajectory_d_rel(&mainboard.traj, -HILL_LEN);
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < HILL_POSX_BALLS_DOWN,
- TRAJ_FLAGS_NO_NEAR);
- printf_P(PSTR("pack\r\n"));
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+ HILL_POSX_BALLS_DOWN2,
+ TRAJ_FLAGS_SMALL_DIST);
+ DEBUG(E_USER_STRAT, "pack support balls");
support_balls_pack();
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- printf_P(PSTR("deploy\r\n"));
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+ HILL_POSX_BALLS_DOWN3,
+ TRAJ_FLAGS_SMALL_DIST);
+ DEBUG(E_USER_STRAT, "deploy support balls");
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ strat_set_acc(ad, aa);
support_balls_deploy();
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+
+ /* wait to be near the wall */
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200,
+ TRAJ_FLAGS_SMALL_DIST);
+
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+
+ /* restore BD coefs */
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+ bd_set_speed_threshold(&mainboard.distance.bd, 60);
/* calibrate position on the wall */
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
- strat_set_acc(ad, aa);
- trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
- err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
- TRAJ_FLAGS_NO_NEAR);
strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- err = strat_calib(-300, TRAJ_FLAGS_NO_NEAR);
+ err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
if (orange_color == our_color)
- position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR,
+ strat_reset_pos(ROBOT_HALF_LENGTH_REAR,
DO_NOT_SET_POS, COLOR_A(0));
else
- position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR,
+ strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_REAR,
DO_NOT_SET_POS, COLOR_A(180));
strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
- trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
- err = wait_traj_end(TRAJ_FLAGS_STD);
+
+ trajectory_d_rel(&mainboard.traj, 250);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (orange_color == I2C_COLOR_YELLOW)
+ trajectory_a_rel(&mainboard.traj, 90);
+ else
+ trajectory_a_rel(&mainboard.traj, -90);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ time_wait_ms(200);
+
+ strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+ err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
+ strat_reset_pos(DO_NOT_SET_POS,
+ COLOR_Y(ROBOT_HALF_LENGTH_REAR),
+ COLOR_A(90));
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+ trajectory_d_rel(&mainboard.traj, 250);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
/* revert acceleration and speed */
- i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ pid_set_gains(&mainboard.angle.pid, p, i, d);
+ pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
strat_want_pack = 0;
strat_set_speed(sd, sa);
- bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
support_balls_deploy();
return err;
}
/* get oranges */
if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
err = run_to_the_hills(get_color());
+ strat_db.our_oranges_count = 0;
do_initturn = 0;
}
}
}
+ /* if it's time to get opponent oranges, do it */
+ if (time_get_s() > strat_conf.opp_orange) {
+ err = get_opp_oranges();
+ if (err == END_ERROR) {
+ DEBUG(E_USER_STRAT,
+ "get_opp_oranges returned END_ERROR");
+ }
+ else if (err == END_TIMER) {
+ DEBUG(E_USER_STRAT, "End of time");
+ strat_exit();
+ break;
+ }
+ else if (!TRAJ_SUCCESS(err)) {
+ /* don't retry oranges if it failed */
+ strat_conf.opp_orange = 90;
+ strat_unblock();
+ }
+ }
+
/**********************/
/* harvest on circuit */
/**********************/
#define ACC_ANGLE 16.
/* default speeds */
-#define SPEED_DIST_FAST 1500.
-#define SPEED_ANGLE_FAST 1000.
+#define SPEED_DIST_FAST 1200.
+#define SPEED_ANGLE_FAST 800.
#define SPEED_DIST_SLOW 500.
#define SPEED_ANGLE_SLOW 500.
#define SPEED_DIST_VERY_SLOW 200.
void strat_event(void *dummy);
void strat_event_enable(void);
void strat_event_disable(void);
+uint8_t prepare_hill(uint8_t orange_color, int16_t posx);
uint8_t run_to_the_hills(uint8_t orange_color);
uint8_t get_orphan_tomatoes(void);
strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
strat_want_pack = 1;
- printf("PACK PACK\n");
-
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
goto fail;
strat_want_pack = 0;
- printf("UNPACK UNPACK\n");
/* do all lines of circuit */
for (idx = 1; idx < len; idx ++) {
/* try to unblock in any situation */
uint8_t strat_unblock(void)
{
- int16_t x, y;
+ int16_t x, y, posx, posy;
uint8_t i, j, k;
uint16_t old_dspeed, old_aspeed;
uint8_t err;
- uint16_t d_min = 0xFFFF, d;
+ uint16_t d_min = 0x7FFF, d;
const struct xy_point *pt;
DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
strat_hardstop();
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- x = position_get_x_s16(&mainboard.pos);
- y = position_get_y_s16(&mainboard.pos);
+ posx = position_get_x_s16(&mainboard.pos);
+ posy = position_get_y_s16(&mainboard.pos);
+ x = posx;
+ y = posy;
if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
x = -1;
/* find the nearest unblock point */
if (x == -1) {
- /* position may have been modified */
- x = position_get_x_s16(&mainboard.pos);
- y = position_get_y_s16(&mainboard.pos);
/* browse all points and find the nearest */
for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
pt = &unblock_pts[k];
- d = distance_between(x, y, pt->x, COLOR_Y(pt->y));
+ d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y));
if (d < d_min) {
d_min = d;
x = pt->x;
}
}
}
+ DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
+ __FUNCTION__, x, y);
/* XXX if opponent is too close, go back, or wait ? */
return err;
strat_set_speed(old_dspeed, old_aspeed);
- strat_want_pack = 0;
return END_TRAJ;
}
if (a == DO_NOT_SET_POS)
a = posa;
+ /* some issues with blocking on simulator */
+#ifdef HOST_VERSION
+ if (x == ROBOT_HALF_LENGTH_REAR)
+ x = ROBOT_HALF_LENGTH_REAR + 10;
+ if (x == AREA_X - ROBOT_HALF_LENGTH_REAR)
+ x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10;
+ if (y == ROBOT_HALF_LENGTH_REAR)
+ y = ROBOT_HALF_LENGTH_REAR + 10;
+ if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR)
+ y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10;
+ if (x == ROBOT_HALF_LENGTH_FRONT)
+ x = ROBOT_HALF_LENGTH_FRONT + 10;
+ if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT)
+ x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10;
+ if (y == ROBOT_HALF_LENGTH_FRONT)
+ y = ROBOT_HALF_LENGTH_FRONT + 10;
+ if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT)
+ y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10;
+#endif
DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
x == DO_NOT_SET_POS ? "" : "x",
y == DO_NOT_SET_POS ? "" : "y",
int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+ uint32_t i_thres = mainboard.distance.bd.i_thres;
+ int32_t k1 = mainboard.distance.bd.k1;
+ int32_t k2 = mainboard.distance.bd.k2;
+ uint16_t cpt_thres = mainboard.distance.bd.cpt_thres;
+
uint8_t err;
+ /* go with a lower angle pid, and with a sensible blocking
+ * detection */
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20);
pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
- pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
+ pid_set_gains(&mainboard.angle.pid, 500, 10, 4000);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
+ if (err != END_BLOCKING)
+ goto fail;
+
+ /* we detected a blocking, reset bd, remove angle pid and
+ * continue */
+ trajectory_d_rel(&mainboard.traj, dist);
+ pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
+ pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
+ time_wait_ms(300);
+ strat_hardstop();
+
+#ifdef HOST_VERSION
+ /* issue with block on simulator */
+ if (dist > 0)
+ trajectory_d_rel(&mainboard.traj, -10);
+ else
+ trajectory_d_rel(&mainboard.traj, 10);
+ wait_traj_end(END_TRAJ);
+#endif
+
+ fail:
pid_set_gains(&mainboard.angle.pid, p, i, d);
pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
+ bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres);
+
return err;
}
__err; \
}) \
+#define WAIT_COND_OR_TE_TO(cond, mask, timeout) \
+ ({ \
+ microseconds __us = time_get_us2(); \
+ uint8_t __ret = 0; \
+ while ( (! (cond)) && (__ret == 0)) { \
+ __ret = test_traj_end(mask); \
+ if (time_get_us2() - __us > (timeout)*1000L) { \
+ __ret = 0; \
+ break; \
+ } \
+ } \
+ if (!__ret) \
+ DEBUG(E_USER_STRAT, "cond / timeout at line %d", \
+ __LINE__); \
+ else \
+ DEBUG(E_USER_STRAT, "got %s (%d) at line %d", \
+ get_err(__ret), __ret, __LINE__); \
+ \
+ __ret; \
+ })
+
int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
int16_t distance_from_robot(int16_t x, int16_t y);
git.droids-corp.org / aversive.git / commitdiff