cob detection

diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
index f9e9b55..3f5a2eb 100644 (file)
--- a/projects/microb2010/ballboard/i2c_protocol.c
+++ b/projects/microb2010/ballboard/i2c_protocol.c
@@ -47,6 +47,7 @@
 #include "../common/i2c_commands.h"
 #include "main.h"
 #include "state.h"
 #include "../common/i2c_commands.h"
 #include "main.h"
 #include "state.h"

+#include "sensor.h"

 #include "actuator.h"
 
 void i2c_protocol_init(void)
 #include "actuator.h"
 
 void i2c_protocol_init(void)


@@ -75,6 +76,8 @@ void i2c_send_status(void)
        ans.hdr.cmd =  I2C_ANS_BALLBOARD_STATUS;
        ans.status = 0x55; /* XXX */
        ans.ball_count = state_get_ball_count();
        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);
 
        i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
                 sizeof(ans), I2C_CTRL_GENERIC);

diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
index 0758279..a267d78 100755 (executable)
--- a/projects/microb2010/ballboard/main.h
+++ b/projects/microb2010/ballboard/main.h
@@ -19,7 +19,7 @@
  *
  */
 
  *
  */
 

-/* was sensorboard in 2009 */
+/* was mainboard in 2009 */

 
 #define LED_TOGGLE(port, bit) do {             \
                if (port & _BV(bit))            \
 
 #define LED_TOGGLE(port, bit) do {             \
                if (port & _BV(bit))            \

diff --git a/projects/microb2010/ballboard/sensor.c b/projects/microb2010/ballboard/sensor.c
index 6cfb491..fe7f92a 100644 (file)
--- a/projects/microb2010/ballboard/sensor.c
+++ b/projects/microb2010/ballboard/sensor.c
@@ -1,7 +1,7 @@
-/*  
+/*

  *  Copyright Droids Corporation (2009)
  *  Olivier MATZ <zer0@droids-corp.org>
  *  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
  *  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


@@ -40,6 +40,7 @@
 
 #include "main.h"
 #include "sensor.h"
 
 #include "main.h"
 #include "sensor.h"

+#include "../common/i2c_commands.h"

 
 /************ ADC */
 
 
 /************ ADC */
 


@@ -75,13 +76,13 @@ int16_t rii_strong(struct adc_infos *adc, int16_t val)
 #define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
 
 /* define which ADC to poll, see in sensor.h */
 #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 },
        [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) }, */
        /* add adc on "cap" pins if needed */
 /*     [ADC_CAP1] = { .config = ADC_CONF(10) }, */
 /*     [ADC_CAP2] = { .config = ADC_CONF(11) }, */


@@ -92,7 +93,7 @@ static struct adc_infos adc_infos[ADC_MAX] = {
 static void adc_event(int16_t result);
 
 /* called every 10 ms, see init below */
 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);
 {
        /* launch first conversion */
        adc_launch(adc_infos[0].config);


@@ -149,10 +150,10 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
        [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_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 */
        [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 */


@@ -166,7 +167,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
 /* value of filtered sensors */
 static uint16_t sensor_filtered = 0;
 
 /* 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)
  * 0-3:  PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
  * 4-5:  PORTL 0->1 (cap5 -> cap6)
  * 6-7:  PORTE 3->4 (cap7 -> cap8)


@@ -186,7 +187,7 @@ uint16_t sensor_get_all(void)
 uint8_t sensor_get(uint8_t i)
 {
        uint16_t tmp = sensor_get_all();
 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 */
 }
 
 /* get the physical value of pins */


@@ -221,8 +222,11 @@ static void do_boolean_sensors(void *dummy)
                        if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
                                sensor_filter[i].prev = 0;
                }
                        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);
                }
        }
                        tmp |= (1UL << i);
                }
        }


@@ -231,7 +235,108 @@ static void do_boolean_sensors(void *dummy)
        IRQ_UNLOCK(flags);
 }
 
        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
 
 /************ global sensor init */
 #define BACKGROUND_ADC  0


@@ -240,18 +345,19 @@ static void do_boolean_sensors(void *dummy)
 static void do_sensors(void *dummy)
 {
        if (BACKGROUND_ADC)
 static void do_sensors(void *dummy)
 {
        if (BACKGROUND_ADC)

-         do_adc(NULL);
+               do_adc(NULL);

        do_boolean_sensors(NULL);
        do_boolean_sensors(NULL);

+       do_cob_detection();

 }
 
 void sensor_init(void)
 {
        adc_init();
        if (BACKGROUND_ADC)
 }
 
 void sensor_init(void)
 {
        adc_init();
        if (BACKGROUND_ADC)

-         adc_register_event(adc_event);
+               adc_register_event(adc_event);

        /* CS 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);
 
 }
                                                ADC_PRIO);
 
 }

diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h
index c2b18a7..0d047d5 100644 (file)
--- a/projects/microb2010/ballboard/sensor.h
+++ b/projects/microb2010/ballboard/sensor.h
@@ -1,7 +1,7 @@
-/*  
+/*

  *  Copyright Droids Corporation (2009)
  *  Olivier MATZ <zer0@droids-corp.org>
  *  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
  *  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


@@ -32,10 +32,10 @@
 #define S_CAP2         1
 #define S_CAP3         2
 #define S_CAP4         3
 #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
 #define S_RESERVED1    8
 #define S_RESERVED2    9
 #define S_RESERVED3   10


@@ -54,3 +54,6 @@ int16_t sensor_get_adc(uint8_t i);
 /* get filtered values of boolean sensors */
 uint16_t sensor_get_all(void);
 uint8_t sensor_get(uint8_t i);
 /* 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);

diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
index 105ca7d..4751285 100755 (executable)
--- a/projects/microb2010/cobboard/main.h
+++ b/projects/microb2010/cobboard/main.h
@@ -19,7 +19,7 @@
  *
  */
 
  *
  */
 

-/* was mainboard in 2009 */
+/* was sensorboard in 2009 */

 
 #define LED_TOGGLE(port, bit) do {             \
                if (port & _BV(bit))            \
 
 #define LED_TOGGLE(port, bit) do {             \
                if (port & _BV(bit))            \

diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index 7b04977..fa17fca 100644 (file)
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -40,6 +40,7 @@
 #define I2C_COB_WHITE   1
 #define I2C_COB_UNKNOWN 2
 #define I2C_COB_REMOVED 3
 #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;
 
 struct i2c_cmd_hdr {
        uint8_t cmd;


@@ -151,6 +152,10 @@ struct i2c_ans_ballboard_status {
        uint8_t status;
 
        uint8_t ball_count;
        uint8_t status;
 
        uint8_t ball_count;

+
+       /* detection of cobs */
+       uint8_t lcob;
+       uint8_t rcob;

 };
 
 #endif /* _I2C_PROTOCOL_H_ */
 };
 
 #endif /* _I2C_PROTOCOL_H_ */

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 6ce17cf..d264622 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -284,6 +284,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
        }
 
        case I2C_ANS_BALLBOARD_STATUS: {
        }
 
        case I2C_ANS_BALLBOARD_STATUS: {

+               uint8_t tmp;

                struct i2c_ans_ballboard_status * ans =
                        (struct i2c_ans_ballboard_status *)buf;
 
                struct i2c_ans_ballboard_status * ans =
                        (struct i2c_ans_ballboard_status *)buf;
 


@@ -292,6 +293,12 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                ballboard.mode = ans->mode;
                ballboard.status = ans->status;
                ballboard.ball_count = ans->ball_count;
                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;
        }
 
                break;
        }
 

diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index 8616303..90e9ff1 100755 (executable)
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -185,6 +185,8 @@ int main(void)
        memset(&mainboard, 0, sizeof(mainboard));
        mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
                DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
        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();
 
        /* UART */
        uart_init();

diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 4fa3dd8..c38a234 100755 (executable)
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -208,6 +208,8 @@ struct ballboard {
        volatile uint8_t mode;
        uint8_t status;
        uint8_t ball_count;
        volatile uint8_t mode;
        uint8_t status;
        uint8_t ball_count;

+       uint8_t lcob;
+       uint8_t rcob;

 };
 
 extern struct genboard gen;
 };
 
 extern struct genboard gen;

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 52b151e..2c60428 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -113,9 +113,9 @@ void strat_init(void)
        time_reset();
        interrupt_traj_reset();
 
        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 */
        i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 
        /* used in strat_base for END_TIMER */


@@ -147,17 +147,25 @@ void strat_exit(void)
 /* called periodically */
 void strat_event(void *dummy)
 {
 /* 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();
 }
        /* limit speed when opponent is close */
        strat_limit_speed();
 }


@@ -171,20 +179,20 @@ static uint8_t strat_beginning(void)
        strat_set_speed(600, SPEED_ANGLE_FAST);
 #else
        /* 250 */
        strat_set_speed(600, SPEED_ANGLE_FAST);
 #else
        /* 250 */

-       strat_set_speed(600, SPEED_ANGLE_FAST);
+       strat_set_speed(250, SPEED_ANGLE_FAST);

 #endif
 
 
 #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);
 
        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);
  l1:
        if (get_cob_count() >= 5)
                strat_set_speed(600, SPEED_ANGLE_FAST);

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index ba51434..9fa720a 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -55,7 +55,7 @@
 #define WAYPOINTS_NBY 8
 
 /*
 #define WAYPOINTS_NBY 8
 
 /*

- * Corn position and lines
+ * Corn position and lines, valid for YELLOW.

  *
  *           vertical lines
  *            O     1     2     3     4     5
  *
  *           vertical lines
  *            O     1     2     3     4     5


@@ -68,7 +68,7 @@
  *     1/  c0                                 c15 \1
  *      |-----+                             +-----|
  *     2/     |                             |     \2
  *     1/  c0                                 c15 \1
  *      |-----+                             +-----|
  *     2/     |                             |     \2

- *      |     |                             |     |
+ *      | Yellow                           Blue   |

  *   0  +-----+-----------------------------+-----+
  *     0                  x                      3000
  *
  *   0  +-----+-----------------------------+-----+
  *     0                  x                      3000
  *


@@ -83,10 +83,43 @@
  *      2                                         |
  *      1-----+                             +-----|
  *      0     |                             |     |
  *      2                                         |
  *      1-----+                             +-----|
  *      0     |                             |     |

- *      |     |                             |     |
+ *      | Yellow                           Blue   |

  *   0  +-----+-----------------------------+-----+
  *     0                  x                      3000
  *
  *   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 */
  */
 
 /* useful traj flags */

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 31afd43..5224787 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -27,6 +27,7 @@
 #include <math.h>
 
 #include <aversive.h>
 #include <math.h>
 
 #include <aversive.h>

+#include <aversive/error.h>

 #include <aversive/pgmspace.h>
 
 #include <ax12.h>
 #include <aversive/pgmspace.h>
 
 #include <ax12.h>


@@ -140,15 +141,16 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        line_t ll1, ll2;
        point_t p;
        uint8_t err;
        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);
 
 
        /* 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);
 
        /* convert to line eq and find intersection */
        pts2line(&l1.p1, &l1.p2, &ll1);


@@ -213,7 +215,15 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
                }
        }
 
                }
        }
 

-       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;
 }
 
        return err;
 }
 

diff --git a/projects/microb2010/tests/tourel_beacon/.config.old b/projects/microb2010/tests/tourel_beacon/.config.old
index 0182964..0db9b78 100644 (file)
--- a/projects/microb2010/tests/tourel_beacon/.config.old
+++ b/projects/microb2010/tests/tourel_beacon/.config.old
@@ -1,5 +1,5 @@
 #
 #

-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit

 #
 
 #
 #
 
 #


@@ -74,6 +74,10 @@ CONFIG_FORMAT_BINARY=y
 #
 # Base modules
 #
 #
 # 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
 # CONFIG_MODULE_CIRBUF is not set
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 # CONFIG_MODULE_FIXED_POINT is not set


@@ -94,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 #
 # Communication modules
 #
 #
 # 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
 # CONFIG_MODULE_UART is not set
 # CONFIG_MODULE_UART_9BITS is not set
 # CONFIG_MODULE_UART_CREATE_CONFIG is not set


@@ -178,6 +186,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 # Control system modules
 #
 # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER 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
 # CONFIG_MODULE_PID is not set
 # CONFIG_MODULE_PID_CREATE_CONFIG is not set
 # CONFIG_MODULE_RAMP is not set


@@ -188,12 +200,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 #
 # Radio devices
 #
 #
 # 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
 #
 # 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
 # CONFIG_MODULE_AES is not set
 # CONFIG_MODULE_AES_CTR is not set
 # CONFIG_MODULE_MD5 is not set


@@ -203,12 +223,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 #
 # Encodings modules
 #
 #
 # Encodings modules
 #

+
+#
+# Encoding modules depend on utils module
+#

 # CONFIG_MODULE_BASE64 is not set
 # CONFIG_MODULE_HAMMING is not set
 
 #
 # Debug modules
 #
 # 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
 # CONFIG_MODULE_DIAGNOSTIC is not set
 # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
 CONFIG_MODULE_ERROR=y

diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py
index fa87953..0d97622 100644 (file)
--- a/projects/microb2010/tests/tourel_beacon/graph.py
+++ b/projects/microb2010/tests/tourel_beacon/graph.py
@@ -367,5 +367,5 @@ def do_graph_2d_move_error():
 #do_random_test()
 #do_graph_2d_simple_error()
 #do_graph_2d_move_error()
 #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()