X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Frobotsim.c;h=1b103d869c1e15e5a07507bbb06ad517a5ef66af;hp=15b107563e38e59085e6b8dbc40ab6f00d11e8b5;hb=51418f44261edc59d818ca990456726027e366ad;hpb=ebfaaedd491e61696cc93b353471be15408d23e4 diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 15b1075..1b103d8 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,9 +58,23 @@ 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 @@ -66,16 +82,110 @@ void robotsim_dump(void) { char buf[BUFSIZ]; int len; + int16_t x, y, a; - 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)); + 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; + + ballboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + 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]) { +#if 0 /* deleted */ + case I2C_CMD_COBBOARD_SET_MODE: + { + struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; + robotsim_i2c_cobboard_set_mode(cmd); + break; + } +#endif + 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 */ void robotsim_update(void) { @@ -84,9 +194,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 +207,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 +220,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); @@ -118,8 +255,8 @@ void robotsim_update(void) ((local_r_pwm * 1000 * FILTER2)/1000); /* basic collision detection */ - a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2); - d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); + d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); xfl = x + cos(a+a2) * d; yfl = y + sin(a+a2) * d; @@ -141,10 +278,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 */ @@ -156,9 +292,9 @@ void robotsim_pwm(void *arg, int32_t val) { // printf("%p, %d\n", arg, val); if (arg == LEFT_PWM) - l_pwm = val; + l_pwm = (val / 1.55); else if (arg == RIGHT_PWM) - r_pwm = val; + r_pwm = (val / 1.55); } int32_t robotsim_encoder_get(void *arg)