#include <parse.h>
#include <rdline.h>
+#include "../common/i2c_commands.h"
#include "strat.h"
#include "strat_utils.h"
#include "main.h"
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
{
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), "%d %d %d\n",
- position_get_x_s16(&mainboard.pos),
- position_get_y_s16(&mainboard.pos),
- position_get_a_deg_s16(&mainboard.pos));
+ 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_mode(uint8_t mode)
+{
+ char buf[BUFSIZ];
+ int len;
+
+ if (cobboard.mode == mode)
+ return 0;
+
+ cobboard.mode = mode;
+ len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode);
+ 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)
{