X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Frobotsim.c;h=69711558b03c4e3f87fffc7bf8825c78b1bd6b21;hp=d0aa3ae7ea00b7f2a7c14af014089db38f101667;hb=HEAD;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index d0aa3ae..6971155 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -44,36 +44,179 @@ #include #include #include +#include #include #include +#include "../common/i2c_commands.h" #include "strat.h" #include "strat_utils.h" #include "main.h" +uint8_t robotsim_blocking = 0; + static int32_t l_pwm, r_pwm; static int32_t l_enc, r_enc; static int fdr, fdw; +/* + * Debug with GDB: + * + * (gdb) handle SIGUSR1 pass + * Signal Stop Print Pass to program Description + * SIGUSR1 Yes Yes Yes User defined signal 1 + * (gdb) handle SIGUSR2 pass + * Signal Stop Print Pass to program Description + * SIGUSR2 Yes Yes Yes User defined signal 2 + * (gdb) handle SIGUSR1 noprint + * Signal Stop Print Pass to program Description + * SIGUSR1 No No Yes User defined signal 1 + * (gdb) handle SIGUSR2 noprint + */ /* */ -#define FILTER 97 +#define FILTER 98 #define FILTER2 (100-FILTER) #define SHIFT 4 void robotsim_dump(void) +{ + char buf[BUFSIZ]; + int len; + int16_t x, y, a; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + a = position_get_a_deg_s16(&mainboard.pos); +/* y = COLOR_Y(y); */ +/* a = COLOR_A(a); */ + + len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n", + x, y, a); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); +} + +static int8_t +robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) { char buf[BUFSIZ]; int len; - len = snprintf(buf, sizeof(buf), "%d %d %d\n", - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); + ballboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode); + if (cmd->mode == I2C_BALLBOARD_MODE_EJECT) + ballboard.ball_count = 0; hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +{ + if (cmd->mode == I2C_COBBOARD_MODE_EJECT) + cobboard.cob_count = 0; + return 0; +} + +int8_t +robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags) +{ + char buf[BUFSIZ]; + int len; + + if (side == I2C_LEFT_SIDE) { + if (cobboard.lspickle == flags) + return 0; + else + cobboard.lspickle = flags; + } + if (side == I2C_RIGHT_SIDE) { + if (cobboard.rspickle == flags) + return 0; + else + cobboard.rspickle = flags; + } + + len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + case I2C_CMD_BALLBOARD_SET_MODE: + { + struct i2c_cmd_ballboard_set_mode *cmd = void_cmd; + robotsim_i2c_ballboard_set_mode(cmd); + break; + } + + default: + break; + } + return 0; +} + +static int8_t +robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + + case I2C_CMD_COBBOARD_SET_MODE: + { + struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; + robotsim_i2c_cobboard_set_mode(cmd); + break; + } + + default: + break; + } + return 0; +} + +int8_t +robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size) +{ + if (addr == I2C_BALLBOARD_ADDR) + return robotsim_i2c_ballboard(addr, buf, size); + else if (addr == I2C_COBBOARD_ADDR) + return robotsim_i2c_cobboard(addr, buf, size); + return 0; +} + +static void beacon_update(void) +{ + uint8_t flags; + int16_t oppx, oppy; + double oppa, oppd; + + IRQ_LOCK(flags); + if (ballboard.opponent_x == I2C_OPPONENT_NOT_THERE) { + IRQ_UNLOCK(flags); + return; + } + oppx = ballboard.opponent_x; + oppy = ballboard.opponent_y; + abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa); + ballboard.opponent_a = DEG(oppa); + if (ballboard.opponent_a < 0) + ballboard.opponent_a += 360; + ballboard.opponent_d = oppd; + IRQ_UNLOCK(flags); } /* must be called periodically */ @@ -84,9 +227,12 @@ void robotsim_update(void) static int32_t l_speed, r_speed; static unsigned i = 0; static unsigned cpt = 0; + + uint8_t flags; int32_t local_l_pwm, local_r_pwm; double x, y, a, a2, d; - char cmd = 0; + char cmd[BUFSIZ]; + int n, pertl = 0, pertr = 0; /* corners of the robot */ double xfl, yfl; /* front left */ @@ -94,6 +240,11 @@ void robotsim_update(void) double xrr, yrr; /* rear right */ double xfr, yfr; /* front right */ + int oppx, oppy; + double oppa, oppd; + + beacon_update(); + /* time shift the command */ l_pwm_shift[i] = l_pwm; r_pwm_shift[i] = r_pwm; @@ -104,8 +255,31 @@ void robotsim_update(void) /* read command */ if (((cpt ++) & 0x7) == 0) { - if (read(fdr, &cmd, 1) != 1) - cmd = 0; + n = read(fdr, &cmd, BUFSIZ - 1); + if (n < 1) + n = 0; + cmd[n] = 0; + } + + /* perturbation */ + if (cmd[0] == 'l') + pertl = 1; + else if (cmd[0] == 'r') + pertr = 1; + else if (cmd[0] == 'b') + robotsim_blocking = 1; + if (cmd[0] == 'o') { + if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) { + abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa); + IRQ_LOCK(flags); + ballboard.opponent_x = oppx; + ballboard.opponent_y = oppy; + ballboard.opponent_a = DEG(oppa); + if (ballboard.opponent_a < 0) + ballboard.opponent_a += 360; + ballboard.opponent_d = oppd; + IRQ_UNLOCK(flags); + } } x = position_get_x_double(&mainboard.pos); @@ -141,10 +315,9 @@ void robotsim_update(void) if (!is_in_area(xfr, yfr, 0) && r_speed > 0) r_speed = 0; - /* perturbation */ - if (cmd == 'l') + if (pertl) l_enc += 5000; /* push 1 cm */ - if (cmd == 'r') + if (pertr) r_enc += 5000; /* push 1 cm */ /* XXX should lock */