#include <blocking_detection_manager.h>
#include <robot_system.h>
#include <position_manager.h>
+#include <trajectory_manager_utils.h>
#include <parse.h>
#include <rdline.h>
+#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 */
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 */
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;
/* 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);
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 */
git.droids-corp.org / aversive.git / blobdiff