#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 "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
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_mode(uint8_t mode)
+robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
{
char buf[BUFSIZ];
int len;
- if (cobboard.mode == mode)
- return 0;
+ 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;
+ }
- cobboard.mode = mode;
- len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode);
+ len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags);
hostsim_lock();
write(fdw, buf, len);
hostsim_unlock();
static int8_t
robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
{
- // void *void_cmd = buf;
+ 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;
}
+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 */
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 */
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 */