SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
SRC += strat_utils.c strat_base.c strat.c strat_corn.c
-SRC += strat_db.c strat_avoid.c
+SRC += strat_db.c strat_avoid.c beacon.c
ifeq ($(H),1)
SRC += robotsim.c
endif
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <i2c.h>
+#include <clock_time.h>
+
+#include <scheduler.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "strat_utils.h"
+
+#define BEACON_UART_NUM 0
+
+#define INIT 0
+#define OPP0 1
+#define OPP1 2
+#define OPP2 3
+#define OPP3 4
+#define STA0 5
+#define STA1 6
+#define STA2 7
+#define STA3 8
+
+static volatile uint8_t opp_age = 0;
+static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
+static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
+
+#ifndef HOST_VERSION
+static void beacon_uart_cb(char c)
+{
+ static uint8_t state;
+ static uint16_t d, a, x, y;
+
+ /* init command */
+ if ((c & 0x80) == 0)
+ state = INIT;
+
+ switch (state) {
+ case INIT:
+ /* recv opp */
+ if (c == 0) {
+ state = OPP0;
+ d = 0;
+ a = 0;
+ }
+ /* recv opp */
+ else if (c == 0) {
+ state = STA0;
+ x = 0;
+ y = 0;
+ }
+ break;
+ case OPP0:
+ d = ((uint16_t)c) & 0x7F;
+ break;
+ case OPP1:
+ d |= (((uint16_t)c << 7) & 0x3F80);
+ break;
+ case OPP2:
+ a = ((uint16_t)c) & 0x7F;
+ break;
+ case OPP3:
+ a |= (((uint16_t)c << 7) & 0x3F80);
+ opp_a = a;
+ opp_d = d;
+ opp_age = 0;
+ break;
+ case STA0:
+ x = ((uint16_t)c) & 0x7F;
+ break;
+ case STA1:
+ x |= (((uint16_t)c << 7) & 0x3F80);
+ break;
+ case STA2:
+ y = ((uint16_t)c) & 0x7F;
+ break;
+ case STA3:
+ y |= (((uint16_t)c << 7) & 0x3F80);
+ beaconboard.posx = x;
+ beaconboard.posy = y;
+ break;
+ /* XXX STA4 with angle */
+ default:
+ state = INIT;
+ break;
+ }
+}
+#endif
+
+static void beacon_event(void *dummy)
+{
+#ifdef HOST_VERSION
+ uint8_t flags;
+ int16_t oppx, oppy;
+ double oppa, oppd;
+
+ IRQ_LOCK(flags);
+ if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
+ IRQ_UNLOCK(flags);
+ return;
+ }
+ oppx = beaconboard.oppx;
+ oppy = beaconboard.oppy;
+ abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
+ beaconboard.oppa = DEG(oppa);
+ if (beaconboard.oppa < 0)
+ beaconboard.oppa += 360;
+ beaconboard.oppd = oppd;
+ IRQ_UNLOCK(flags);
+#else
+ uint8_t flags;
+ double fd, fa, fx, fy;
+ int16_t id, ia, ix, iy;
+
+ /* if beacon is too old, remove it */
+ IRQ_LOCK(flags);
+ if (opp_age < 3)
+ opp_age ++;
+ else
+ beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
+
+ ia = opp_a;
+ id = opp_d;
+ IRQ_UNLOCK(flags);
+
+ fa = ia;
+ fa = RAD(fa);
+ fd = id;
+ rel_da_to_abs_xy(fd, fa, &fx, &fy);
+
+ ix = fx;
+ iy = fy;
+
+ IRQ_LOCK(flags);
+ beaconboard.oppx = ix;
+ beaconboard.oppy = iy;
+ beaconboard.oppa = ia;
+ beaconboard.oppd = id;
+ IRQ_UNLOCK(flags);
+#endif
+}
+
+void beacon_init(void)
+{
+#ifndef HOST_VERSION
+ uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
+#endif
+ scheduler_add_periodical_event_priority(beacon_event, NULL,
+ 100000L / SCHEDULER_UNIT,
+ BEACON_PRIO);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+void beacon_init(void);
#include "strat_db.h"
#include "strat_avoid.h"
#include "i2c_protocol.h"
+#include "beacon.h"
/* 0 means "programmed"
struct mainboard mainboard;
volatile struct cobboard cobboard;
volatile struct ballboard ballboard;
+volatile struct beaconboard beaconboard;
#ifndef HOST_VERSION
/***********************/
ballboard.lcob = I2C_COB_NONE;
ballboard.rcob = I2C_COB_NONE;
+ beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
+
/* UART */
uart_init();
uart_register_rx_event(CMDLINE_UART, emergency);
/* TIME */
time_init(TIME_PRIO);
-#ifndef HOST_VERSION
/* sensors, will also init hardware adc */
sensor_init();
+ /* beacon */
+ beacon_init();
+
+#ifndef HOST_VERSION
/* start i2c slave polling */
scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
#define CS_PRIO 100
#define STRAT_PRIO 30
#define I2C_POLL_PRIO 20
+#define BEACON_PRIO 15
#define EEPROM_TIME_PRIO 10
#define CS_PERIOD 5000L /* in microsecond */
uint8_t rcob;
};
+/* state of beaconboard, sync'd through uart */
+struct beaconboard {
+ int16_t oppx;
+ int16_t oppy;
+ int16_t oppa;
+ int16_t oppd;
+ uint16_t posx;
+ uint16_t posy;
+};
+
extern struct genboard gen;
extern struct mainboard mainboard;
extern volatile struct cobboard cobboard;
extern volatile struct ballboard ballboard;
+extern volatile struct beaconboard beaconboard;
/* start the bootloader */
void bootloader(void);
#include <blocking_detection_manager.h>
#include <robot_system.h>
#include <position_manager.h>
+#include <trajectory_manager_utils.h>
#include <parse.h>
#include <rdline.h>
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;
+
/* 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;
+ 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);
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 */
/************ global sensor init */
#define BACKGROUND_ADC 0
-#ifndef HOST_VERSION
/* called every 10 ms, see init below */
static void do_sensors(void *dummy)
{
+#ifndef HOST_VERSION
if (BACKGROUND_ADC)
do_adc(NULL);
do_boolean_sensors(NULL);
+#endif
sensor_obstacle_update();
}
-#endif
void sensor_init(void)
{
-#ifdef HOST_VERSION
- return;
-#else
+#ifndef HOST_VERSION
adc_init();
if (BACKGROUND_ADC)
adc_register_event(adc_event);
+#endif
+
/* CS EVENT */
scheduler_add_periodical_event_priority(do_sensors, NULL,
10000L / SCHEDULER_UNIT,
ADC_PRIO);
-#endif
}
return I2C_COLOR_YELLOW;
}
-/* get the xy pos of the opponent robot */
-int8_t get_opponent_xy(int16_t *x, int16_t *y)
+/* get the da pos of the opponent robot */
+int8_t get_opponent_da(int16_t *d, int16_t *a)
{
uint8_t flags;
- return -1; // XXX
+ int16_t x;
+
IRQ_LOCK(flags);
-/* *x = ballboard.opponent_x; */
-/* *y = ballboard.opponent_y; */
+ *d = beaconboard.oppd;
+ *a = beaconboard.oppa;
+ x = beaconboard.oppx;
IRQ_UNLOCK(flags);
- if (*x == I2C_OPPONENT_NOT_THERE)
+ if (x == I2C_OPPONENT_NOT_THERE)
return -1;
return 0;
}
-/* get the da pos of the opponent robot */
-int8_t get_opponent_da(int16_t *d, int16_t *a)
+/* get the xy pos of the opponent robot */
+int8_t get_opponent_xy(int16_t *x, int16_t *y)
{
uint8_t flags;
- int16_t x_tmp;
- return -1; // XXX
+
IRQ_LOCK(flags);
-/* x_tmp = ballboard.opponent_x; */
-/* *d = ballboard.opponent_d; */
-/* *a = ballboard.opponent_a; */
+ *x = beaconboard.oppx;
+ *y = beaconboard.oppy;
IRQ_UNLOCK(flags);
- if (x_tmp == I2C_OPPONENT_NOT_THERE)
+ if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
+
return 0;
}
int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
{
uint8_t flags;
- return -1; // XXX
+
IRQ_LOCK(flags);
-/* *x = ballboard.opponent_x; */
-/* *y = ballboard.opponent_y; */
-/* *d = ballboard.opponent_d; */
-/* *a = ballboard.opponent_a; */
+ *x = beaconboard.oppx;
+ *y = beaconboard.oppy;
+ *d = beaconboard.oppd;
+ *a = beaconboard.oppa;
IRQ_UNLOCK(flags);
if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
- return 0;
-}
-uint8_t opponent_is_behind(void)
-{
-/* int8_t opp_there; */
-/* int16_t opp_d, opp_a; */
-/* opp_there = get_opponent_da(&opp_d, &opp_a); */
-/* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */
-/* return 1; */
return 0;
}
int8_t get_opponent_da(int16_t *d, int16_t *a);
int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a);
int16_t distance_from_opponent(int16_t x, int16_t y);
-uint8_t opponent_is_behind(void);
uint8_t get_ball_count(void);
uint8_t get_cob_count(void);
git.droids-corp.org / aversive.git / commitdiff