#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;
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)
{
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);
((local_r_pwm * 1000 * FILTER2)/1000);
/* basic collision detection */
- a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_FRONT);
- d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_FRONT);
+ 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;
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 */