X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Frobotsim.c;h=df8ee0ebcc2c9dac32903b6aea6c2fd47953b214;hb=c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4;hp=d0aa3ae7ea00b7f2a7c14af014089db38f101667;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;p=aversive.git diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index d0aa3ae..df8ee0e 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -44,10 +44,12 @@ #include #include #include +#include #include #include +#include "../common/i2c_commands.h" #include "strat.h" #include "strat_utils.h" #include "main.h" @@ -56,24 +58,142 @@ 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; } /* must be called periodically */ @@ -84,9 +204,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 +217,9 @@ void robotsim_update(void) double xrr, yrr; /* rear right */ double xfr, yfr; /* front right */ + int oppx, oppy; + double oppa, oppd; + /* time shift the command */ l_pwm_shift[i] = l_pwm; r_pwm_shift[i] = r_pwm; @@ -104,8 +230,29 @@ 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; + 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); + beaconboard.oppx = oppx; + beaconboard.oppy = oppy; + beaconboard.oppa = DEG(oppa); + if (beaconboard.oppa < 0) + beaconboard.oppa += 360; + beaconboard.oppd = oppd; + IRQ_UNLOCK(flags); + } } x = position_get_x_double(&mainboard.pos); @@ -141,10 +288,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 */