#include "../common/i2c_commands.h"
#include "main.h"
#include "state.h"
+#include "sensor.h"
#include "actuator.h"
void i2c_protocol_init(void)
ans.hdr.cmd = I2C_ANS_BALLBOARD_STATUS;
ans.status = 0x55; /* XXX */
ans.ball_count = state_get_ball_count();
+ ans.lcob = cob_detect_left();
+ ans.rcob = cob_detect_right();
i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
sizeof(ans), I2C_CTRL_GENERIC);
*
*/
-/* was sensorboard in 2009 */
+/* was mainboard in 2009 */
#define LED_TOGGLE(port, bit) do { \
if (port & _BV(bit)) \
-/*
+/*
* Copyright Droids Corporation (2009)
* Olivier MATZ <zer0@droids-corp.org>
- *
+ *
* 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
#include "main.h"
#include "sensor.h"
+#include "../common/i2c_commands.h"
/************ ADC */
#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
/* define which ADC to poll, see in sensor.h */
-static struct adc_infos adc_infos[ADC_MAX] = {
-
+static struct adc_infos adc_infos[ADC_MAX] = {
+
[ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
[ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
[ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
[ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
-
+
/* add adc on "cap" pins if needed */
/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
static void adc_event(int16_t result);
/* called every 10 ms, see init below */
-static void do_adc(void *dummy)
+static void do_adc(void *dummy)
{
/* launch first conversion */
adc_launch(adc_infos[0].config);
[S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
[S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */
[S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
- [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
- [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */
- [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
- [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
+ [S_R_IR] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+ [S_R_US] = { 1, 0, 0, 1, 0, 1 }, /* 5 */
+ [S_L_US] = { 1, 0, 0, 1, 0, 1 }, /* 6 */
+ [S_L_IR] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
[S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
[S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
[S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
/* value of filtered sensors */
static uint16_t sensor_filtered = 0;
-/* sensor mapping :
+/* sensor mapping :
* 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
* 4-5: PORTL 0->1 (cap5 -> cap6)
* 6-7: PORTE 3->4 (cap7 -> cap8)
uint8_t sensor_get(uint8_t i)
{
uint16_t tmp = sensor_get_all();
- return (tmp & _BV(i));
+ return !!(tmp & _BV(i));
}
/* get the physical value of pins */
if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
sensor_filter[i].prev = 0;
}
-
- if (sensor_filter[i].prev) {
+
+ if (sensor_filter[i].prev && !sensor_filter[i].invert) {
+ tmp |= (1UL << i);
+ }
+ else if (!sensor_filter[i].prev && sensor_filter[i].invert) {
tmp |= (1UL << i);
}
}
IRQ_UNLOCK(flags);
}
+static volatile uint8_t lcob_seen = I2C_COB_NONE, rcob_seen = I2C_COB_NONE;
+
+uint8_t cob_detect_left(void)
+{
+ uint8_t flags;
+ uint8_t ret;
+ IRQ_LOCK(flags);
+ ret = lcob_seen;
+ lcob_seen = I2C_COB_NONE;
+ IRQ_UNLOCK(flags);
+ return ret;
+}
+
+uint8_t cob_detect_right(void)
+{
+ uint8_t flags;
+ uint8_t ret;
+ IRQ_LOCK(flags);
+ ret = rcob_seen;
+ rcob_seen = I2C_COB_NONE;
+ IRQ_UNLOCK(flags);
+ return ret;
+}
+
+#define COB_MIN_DETECT 4
+#define COB_MAX_DETECT 50
+static void do_cob_detection(void)
+{
+ uint8_t flags;
+ uint16_t tmp = sensor_get_all();
+ uint8_t l_us = !!(tmp & _BV(S_L_US));
+ uint8_t r_us = !!(tmp & _BV(S_R_US));
+ uint8_t l_ir = !!(tmp & _BV(S_L_IR));
+ uint8_t r_ir = !!(tmp & _BV(S_R_IR));
+ static uint8_t l_us_prev, r_us_prev, l_ir_prev, r_ir_prev;
+ static uint8_t l_cpt_on, l_cpt_off;
+ static uint8_t r_cpt_on, r_cpt_off;
+
+ /* rising edge on US */
+ if (l_us_prev == 0 && l_us == 1) {
+ l_cpt_off = 0;
+ l_cpt_on = 0;
+ }
+
+ /* us is on */
+ if (l_us) {
+ if (l_ir && l_cpt_on < COB_MAX_DETECT)
+ l_cpt_on ++;
+ else if (l_cpt_off < COB_MAX_DETECT)
+ l_cpt_off ++;
+ }
+
+ /* falling edge on US */
+ if (l_us_prev == 1 && l_us == 0) {
+ /* detection should not be too short or too long */
+ if ((l_cpt_off + l_cpt_on) < COB_MAX_DETECT &&
+ (l_cpt_off + l_cpt_on) > COB_MIN_DETECT) {
+ IRQ_LOCK(flags);
+ if (l_cpt_on > l_cpt_off)
+ lcob_seen = I2C_COB_WHITE;
+ else
+ lcob_seen = I2C_COB_BLACK;
+ IRQ_UNLOCK(flags);
+ }
+ }
+
+ /* rising edge on US */
+ if (r_us_prev == 0 && r_us == 1) {
+ r_cpt_off = 0;
+ r_cpt_on = 0;
+ }
+
+ /* us is on */
+ if (r_us) {
+ if (r_ir && r_cpt_on < COB_MAX_DETECT)
+ r_cpt_on ++;
+ else if (r_cpt_off < COB_MAX_DETECT)
+ r_cpt_off ++;
+ }
+
+ /* falling edge on US */
+ if (r_us_prev == 1 && r_us == 0) {
+
+ /* detection should not be too short or too long */
+ if ((r_cpt_off + r_cpt_on) < COB_MAX_DETECT &&
+ (r_cpt_off + r_cpt_on) > COB_MIN_DETECT) {
+ IRQ_LOCK(flags);
+ if (r_cpt_on > r_cpt_off)
+ rcob_seen = I2C_COB_WHITE;
+ else
+ rcob_seen = I2C_COB_BLACK;
+ IRQ_UNLOCK(flags);
+ }
+ }
+
+
+ l_us_prev = l_us;
+ r_us_prev = r_us;
+ l_ir_prev = l_ir;
+ r_ir_prev = r_ir;
+}
/************ global sensor init */
#define BACKGROUND_ADC 0
static void do_sensors(void *dummy)
{
if (BACKGROUND_ADC)
- do_adc(NULL);
+ do_adc(NULL);
do_boolean_sensors(NULL);
+ do_cob_detection();
}
void sensor_init(void)
{
adc_init();
if (BACKGROUND_ADC)
- adc_register_event(adc_event);
+ adc_register_event(adc_event);
/* CS EVENT */
- scheduler_add_periodical_event_priority(do_sensors, NULL,
- 10000L / SCHEDULER_UNIT,
+ scheduler_add_periodical_event_priority(do_sensors, NULL,
+ 10000L / SCHEDULER_UNIT,
ADC_PRIO);
}
-/*
+/*
* Copyright Droids Corporation (2009)
* Olivier MATZ <zer0@droids-corp.org>
- *
+ *
* 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
#define S_CAP2 1
#define S_CAP3 2
#define S_CAP4 3
-#define S_CAP5 4
-#define S_CAP6 5
-#define S_CAP7 6
-#define S_CAP8 7
+#define S_R_IR 4
+#define S_R_US 5
+#define S_L_US 6
+#define S_L_IR 7
#define S_RESERVED1 8
#define S_RESERVED2 9
#define S_RESERVED3 10
/* get filtered values of boolean sensors */
uint16_t sensor_get_all(void);
uint8_t sensor_get(uint8_t i);
+
+uint8_t cob_detect_left(void);
+uint8_t cob_detect_right(void);
*
*/
-/* was mainboard in 2009 */
+/* was sensorboard in 2009 */
#define LED_TOGGLE(port, bit) do { \
if (port & _BV(bit)) \
#define I2C_COB_WHITE 1
#define I2C_COB_UNKNOWN 2
#define I2C_COB_REMOVED 3
+#define I2C_COB_NONE 4
struct i2c_cmd_hdr {
uint8_t cmd;
uint8_t status;
uint8_t ball_count;
+
+ /* detection of cobs */
+ uint8_t lcob;
+ uint8_t rcob;
};
#endif /* _I2C_PROTOCOL_H_ */
}
case I2C_ANS_BALLBOARD_STATUS: {
+ uint8_t tmp;
struct i2c_ans_ballboard_status * ans =
(struct i2c_ans_ballboard_status *)buf;
ballboard.mode = ans->mode;
ballboard.status = ans->status;
ballboard.ball_count = ans->ball_count;
+ tmp = ans->lcob;
+ if (tmp != I2C_COB_NONE)
+ ballboard.lcob = tmp;
+ tmp = ans->rcob;
+ if (tmp != I2C_COB_NONE)
+ ballboard.rcob = tmp;
break;
}
memset(&mainboard, 0, sizeof(mainboard));
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
+ ballboard.lcob = I2C_COB_NONE;
+ ballboard.rcob = I2C_COB_NONE;
/* UART */
uart_init();
volatile uint8_t mode;
uint8_t status;
uint8_t ball_count;
+ uint8_t lcob;
+ uint8_t rcob;
};
extern struct genboard gen;
time_reset();
interrupt_traj_reset();
- //i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
- //i2c_cobboard_harvest(I2C_LEFT_SIDE);
- //i2c_cobboard_harvest(I2C_RIGHT_SIDE);
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_harvest(I2C_LEFT_SIDE);
+ i2c_cobboard_harvest(I2C_RIGHT_SIDE);
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
/* used in strat_base for END_TIMER */
/* called periodically */
void strat_event(void *dummy)
{
-#if 0
- /* pack or deploy spickle */
- if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) {
- if (sensor_get(S_LCOB_PRESENT)) {
- if (sensor_get(S_LCOB_WHITE))
- i2c_ballboard_set_mode();
- else
- ;
- }
- }
-#endif
+ uint8_t flags;
+ uint8_t lcob, rcob;
+
+ IRQ_LOCK(flags);
+ lcob = ballboard.lcob;
+ ballboard.lcob = I2C_COB_NONE;
+ rcob = ballboard.rcob;
+ ballboard.rcob = I2C_COB_NONE;
+ IRQ_UNLOCK(flags);
+
+ if (lcob == I2C_COB_WHITE)
+ DEBUG(E_USER_STRAT, "lcob white");
+ if (lcob == I2C_COB_BLACK)
+ DEBUG(E_USER_STRAT, "lcob black");
+ if (rcob == I2C_COB_WHITE)
+ DEBUG(E_USER_STRAT, "rcob white");
+ if (rcob == I2C_COB_BLACK)
+ DEBUG(E_USER_STRAT, "rcob black");
+
/* limit speed when opponent is close */
strat_limit_speed();
}
strat_set_speed(600, SPEED_ANGLE_FAST);
#else
/* 250 */
- strat_set_speed(600, SPEED_ANGLE_FAST);
+ strat_set_speed(250, SPEED_ANGLE_FAST);
#endif
- strat_set_speed(600, 60); /* OK */
- // strat_set_speed(250, 28); /* OK */
+ // strat_set_speed(600, 60); /* OK */
+ strat_set_speed(250, 28); /* OK */
- trajectory_d_a_rel(&mainboard.traj, 500, COLOR_A(20));
+ trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
TRAJ_FLAGS_STD);
strat_set_acc(ACC_DIST, ACC_ANGLE);
-#if 0
+#if 1
l1:
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
#define WAYPOINTS_NBY 8
/*
- * Corn position and lines
+ * Corn position and lines, valid for YELLOW.
*
* vertical lines
* O 1 2 3 4 5
* 1/ c0 c15 \1
* |-----+ +-----|
* 2/ | | \2
- * | | | |
+ * | Yellow Blue |
* 0 +-----+-----------------------------+-----+
* 0 x 3000
*
* 2 |
* 1-----+ +-----|
* 0 | | |
- * | | | |
+ * | Yellow Blue |
* 0 +-----+-----------------------------+-----+
* 0 x 3000
*
+ *
+ * Corn position and lines, valid for BLUE.
+ *
+ * vertical lines
+ * 5 4 3 2 1 0
+ * 0 +-----|-----|-----|-----|-----|-----|-----+
+ * | c14 c9 c5 |
+ * | c17 c11 c7 c2 | diag
+ * | c13 c8 c4 | lines
+ * 0/ c16 c10 c6 c1 \0
+ * y | c12 c3 |
+ * 1/ c15 c0 \1
+ * |-----+ +-----|
+ * 2/ | | \2
+ * | Yellow Blue |
+ * 2100 +-----+-----------------------------+-----+
+ * 3000 x 0
+ *
+ * Ball (tomato) and i,j coords (for waypoints)
+ *
+ * 0 +-12-11-10--9--8--7--6--5--4--3--2--1--0--+
+ * 7 t9 t5 |
+ * 6 t11 t7 t3 |
+ * 5 t13 t8 t4 t1 |
+ * 4 t10 t6 t2 |
+ * y 3 t12 t0 |
+ * 2 |
+ * 1-----+ +-----|
+ * 0 | | |
+ * | Yellow Blue |
+ * 2100 +-----+-----------------------------+-----+
+ * 3000 x 0
+ *
*/
/* useful traj flags */
#include <math.h>
#include <aversive.h>
+#include <aversive/error.h>
#include <aversive/pgmspace.h>
#include <ax12.h>
line_t ll1, ll2;
point_t p;
uint8_t err;
+ uint16_t a_speed, d_speed;
/* convert to 2 points */
num2line(&l1, dir1, num1);
num2line(&l2, dir2, num2);
- printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
- printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
+ DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+ l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
+ DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+ l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
/* convert to line eq and find intersection */
pts2line(&l1.p1, &l1.p2, &ll1);
}
}
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
+ strat_get_speed(&d_speed, &a_speed);
+
+ /* XXX 600 -> cste */
+ if (err == 0 && d_speed < 600 &&
+ mainboard.traj.state == RUNNING_CLITOID_LINE)
+ strat_set_speed(600, SPEED_ANGLE_FAST);
+ err = wait_traj_end(0xFF);
+
return err;
}
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
# CONFIG_MODULE_CIRBUF is not set
# CONFIG_MODULE_CIRBUF_LARGE is not set
# CONFIG_MODULE_FIXED_POINT is not set
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
# CONFIG_MODULE_UART is not set
# CONFIG_MODULE_UART_9BITS is not set
# CONFIG_MODULE_UART_CREATE_CONFIG is not set
# Control system modules
#
# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
# CONFIG_MODULE_PID is not set
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# Radio devices
#
+
+#
+# Some radio devices require SPI to be activated
+#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
+
+#
+# Crypto modules depend on utils module
+#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
#
# Encodings modules
#
+
+#
+# Encoding modules depend on utils module
+#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
+
+#
+# Debug modules depend on utils module
+#
# CONFIG_MODULE_DIAGNOSTIC is not set
# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
CONFIG_MODULE_ERROR=y
#do_random_test()
#do_graph_2d_simple_error()
#do_graph_2d_move_error()
-#do_graph_2d_ad_error()
-do_graph_2d_ad_error_mm()
+do_graph_2d_ad_error()
+#do_graph_2d_ad_error_mm()
git.droids-corp.org / aversive.git / commitdiff