From b022f257a5ee568737e1a684a83d0154397fffcb Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 16 Apr 2010 00:15:22 +0200 Subject: [PATCH] 20100416 --- projects/microb2010/ballboard/main.h | 12 +- projects/microb2010/cobboard/actuator.c | 4 +- projects/microb2010/cobboard/cs.c | 12 +- projects/microb2010/cobboard/i2c_protocol.c | 18 +- projects/microb2010/cobboard/main.c | 7 +- projects/microb2010/cobboard/main.h | 12 +- projects/microb2010/cobboard/sensor.c | 71 ++- projects/microb2010/cobboard/sensor.h | 2 + projects/microb2010/cobboard/spickle.c | 20 +- projects/microb2010/cobboard/state.c | 26 +- projects/microb2010/common/i2c_commands.h | 5 +- projects/microb2010/mainboard/Makefile | 2 +- .../microb2010/mainboard/commands_mainboard.c | 79 ++- projects/microb2010/mainboard/display.py | 11 +- projects/microb2010/mainboard/i2c_protocol.c | 45 +- projects/microb2010/mainboard/main.h | 12 +- projects/microb2010/mainboard/robotsim.c | 15 +- projects/microb2010/mainboard/robotsim.h | 1 + projects/microb2010/mainboard/sensor.c | 14 +- projects/microb2010/mainboard/sensor.h | 4 +- projects/microb2010/mainboard/strat.c | 3 + projects/microb2010/mainboard/strat.h | 33 +- projects/microb2010/mainboard/strat_base.h | 5 +- projects/microb2010/mainboard/strat_corn.c | 548 ++++++++++++++++++ projects/microb2010/mainboard/strat_corn.h | 36 ++ 25 files changed, 853 insertions(+), 144 deletions(-) create mode 100644 projects/microb2010/mainboard/strat_corn.c create mode 100644 projects/microb2010/mainboard/strat_corn.h diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index cde4608..9f10c35 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -30,13 +30,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 1b6e91f..8dfe8bb 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -49,8 +49,8 @@ //#define COBROLLER_SPEED 400 #define SERVO_DOOR_OPEN 260 -#define SERVO_DOOR_CLOSED 510 -#define SERVO_DOOR_BLOCK 510 +#define SERVO_DOOR_CLOSED 500 +#define SERVO_DOOR_BLOCK 500 #define SERVO_CARRY_L_OPEN 295 #define SERVO_CARRY_L_CLOSED 400 // 510 diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index a718970..0ed2f1a 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -102,8 +102,8 @@ static void do_cs(__attribute__((unused)) void *dummy) /* urgent case: stop power on blocking */ if (cobboard.flags & DO_ERRBLOCKING) { - if (bd_get(&cobboard.left_spickle.bd) || - bd_get(&cobboard.right_spickle.bd) || + if (/* bd_get(&cobboard.left_spickle.bd) || */ + /* bd_get(&cobboard.right_spickle.bd) || */ bd_get(&cobboard.shovel.bd)) { printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); @@ -149,8 +149,8 @@ void microb_cs_init(void) /* ---- CS left_spickle */ /* PID */ pid_init(&cobboard.left_spickle.pid); - pid_set_gains(&cobboard.left_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.left_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.left_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.left_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.left_spickle.pid, 10); pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); @@ -169,8 +169,8 @@ void microb_cs_init(void) /* ---- CS right_spickle */ /* PID */ pid_init(&cobboard.right_spickle.pid); - pid_set_gains(&cobboard.right_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.right_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.right_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.right_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.right_spickle.pid, 10); pid_set_derivate_filter(&cobboard.right_spickle.pid, 4); diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 8d86b49..9b2a9e7 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -99,17 +99,11 @@ static void i2c_send_status(void) ans.right_cobroller_speed = cobboard.right_cobroller_speed; ans.cob_count = state_get_cob_count(); -; + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); } -static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) -{ - state_set_mode(cmd->mode); - return 0; -} - void i2c_recvevent(uint8_t * buf, int8_t size) { void *void_cmd = buf; @@ -136,6 +130,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } +#if 0 case I2C_CMD_COBBOARD_SET_MODE: { struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; @@ -144,6 +139,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) i2c_set_mode(cmd); break; } +#endif case I2C_CMD_GENERIC_SET_COLOR: { @@ -164,16 +160,18 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } #endif - + /* Add other commands here ...*/ case I2C_REQ_COBBOARD_STATUS: { - //struct i2c_req_cobboard_status *cmd = void_cmd; + struct i2c_req_cobboard_status *cmd = void_cmd; if (size != sizeof (struct i2c_req_cobboard_status)) goto error; - + + /* mode is in req */ + state_set_mode(cmd->mode); i2c_send_status(); break; } diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 1f4065b..d21e7b7 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -207,8 +207,8 @@ int main(void) PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); - PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | - PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4); + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, @@ -271,6 +271,9 @@ int main(void) gen.log_level = 5; cobboard.flags |= DO_CS; + spickle_pack(I2C_LEFT_SIDE); + spickle_pack(I2C_RIGHT_SIDE); + state_machine(); cmdline_interact(); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 699a5c1..0c12fdc 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -30,13 +30,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index 5bbf43f..c471033 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * 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 @@ -38,6 +38,7 @@ #include #include +#include "../common/i2c_commands.h" #include "main.h" #include "sensor.h" @@ -75,7 +76,7 @@ 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 */ -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 }, @@ -91,7 +92,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 do_adc(__attribute__((unused)) void *dummy) +static void do_adc(__attribute__((unused)) void *dummy) { /* launch first conversion */ adc_launch(adc_infos[0].config); @@ -146,10 +147,10 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP2] = { 10, 0, 3, 7, 0, 0 }, /* 1 */ [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ - [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ - [S_RCOB] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ + [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used + [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used + [S_RCOB] = { 1, 0, 0, 1, 0, 1 }, /* 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 */ @@ -163,7 +164,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { /* 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) @@ -218,7 +219,7 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy) if (sensor_filter[i].cpt <= sensor_filter[i].thres_off) sensor_filter[i].prev = 0; } - + if (sensor_filter[i].prev && !sensor_filter[i].invert) { tmp |= (1UL << i); } @@ -231,6 +232,51 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy) IRQ_UNLOCK(flags); } +static uint8_t lcob_cpt = 0, rcob_cpt = 0; +uint8_t cob_falling_edge(uint8_t side) +{ + uint8_t flags; + + if (side == I2C_LEFT_SIDE) { + IRQ_LOCK(flags); + if (lcob_cpt == 0) { + IRQ_UNLOCK(flags); + return 0; + } + lcob_cpt = 0; + IRQ_UNLOCK(flags); + return 1; + } + else { + IRQ_LOCK(flags); + if (rcob_cpt == 0) { + IRQ_UNLOCK(flags); + return 0; + } + rcob_cpt = 0; + IRQ_UNLOCK(flags); + return 1; + } +} + +static void cob_edge_manage(void) +{ + static uint8_t lprev, rprev; + uint8_t l, r; + l = sensor_get(S_LCOB); + r = sensor_get(S_RCOB); + /* falling edge */ + if (lprev != 0 && l == 0) + lcob_cpt = 10; + if (rprev != 0 && r == 0) + rcob_cpt = 10; + if (lcob_cpt > 0) + lcob_cpt --; + if (rcob_cpt > 0) + rcob_cpt --; + lprev = l; + rprev = r; +} /************ global sensor init */ @@ -240,6 +286,7 @@ static void do_sensors(__attribute__((unused)) void *dummy) { do_adc(NULL); do_boolean_sensors(NULL); + cob_edge_manage(); } void sensor_init(void) @@ -247,8 +294,8 @@ void sensor_init(void) adc_init(); 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); } diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 0ba6b0c..c24af73 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -54,3 +54,5 @@ 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); + +uint8_t cob_falling_edge(uint8_t side); diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index e1d9a7b..dcee125 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -60,23 +60,23 @@ struct spickle_params { }; static struct spickle_params spickle = { - .k1 = 500, + .k1 = 1000, .k2 = 20, .csb = { &cobboard.left_spickle, &cobboard.right_spickle, }, .pos_deployed = { - 40000, /* left */ - -40000, /* right */ + 0, /* left */ + 0, /* right */ }, .pos_mid = { - 20000, /* left */ - -20000, /* right */ + 25000, /* left */ + 26000, /* right */ }, .pos_packed = { - 0, /* left */ - 0, /* right */ + 53000, /* left */ + 54500, /* right */ }, }; @@ -84,9 +84,9 @@ static struct spickle_params spickle = { static void spickle_autopos(void) { printf_P(PSTR("spickle autopos...")); - pwm_ng_set(LEFT_SPICKLE_PWM, -500); - pwm_ng_set(RIGHT_SPICKLE_PWM, 500); - wait_ms(1000); + pwm_ng_set(LEFT_SPICKLE_PWM, -700); + pwm_ng_set(RIGHT_SPICKLE_PWM, -700); + wait_ms(2500); pwm_ng_set(LEFT_SPICKLE_PWM, 0); pwm_ng_set(RIGHT_SPICKLE_PWM, 0); encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0); diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index a16675d..389411e 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -86,25 +86,6 @@ static void state_debug_wait_key_pressed(void) while(!cmdline_keypressed()); } -/* return true if cob is present */ -static uint8_t state_cob_present(uint8_t side) -{ - if (side == I2C_LEFT_SIDE) - return sensor_get(S_LCOB); - else - return sensor_get(S_RCOB); -} - -/* return the detected color of the cob (only valid if present) */ -static uint8_t state_cob_color(uint8_t side) -{ - /* XXX no color sensor for now */ - if (side == I2C_LEFT_SIDE) - return I2C_COB_WHITE; - else - return I2C_COB_WHITE; -} - /* return true if the cob is correctly inside */ static uint8_t state_cob_inside(void) { @@ -186,11 +167,7 @@ static void state_do_harvest(uint8_t side) state_status = I2C_COBBOARD_STATUS_RBUSY; /* if there is no cob, return */ - if (state_cob_present(side)) - return; - - /* if it is black, nothing to do */ - if (state_cob_color(side) == I2C_COB_BLACK) + if (cob_falling_edge(side) == 0) return; STMCH_DEBUG("start"); @@ -330,6 +307,7 @@ void state_machine(void) state_do_eject(); } } + state_status = I2C_COBBOARD_STATUS_READY; } void state_init(void) diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index d6794af..4b8aea5 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -68,6 +68,7 @@ struct i2c_cmd_generic_color { #define I2C_CMD_COBBOARD_SET_MODE 0x02 +/* XXX disabled, use memory sync instead */ struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; @@ -77,7 +78,7 @@ struct i2c_cmd_cobboard_set_mode { #define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ #define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ #define I2C_COBBOARD_MODE_INIT 0x20 /* init state machine */ - uint8_t mode; + //uint8_t mode; }; #define I2C_CMD_BALLBOARD_SET_MODE 0x10 @@ -105,7 +106,7 @@ struct i2c_cmd_ballboard_set_mode { struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - int16_t sickle_left1_current; + uint8_t mode; }; #define I2C_ANS_COBBOARD_STATUS 0x81 diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 141943f..33b5c52 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -6,7 +6,7 @@ AVERSIVE_DIR = ../../.. SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c 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 +SRC += strat_utils.c strat_base.c strat.c strat_corn.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 8a79c35..7c42411 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,7 @@ #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -1202,6 +1203,65 @@ static void reverse_line(struct line_2pts *l) } #endif +/* return 1 if there is a corn near, and fill the index ptr */ +static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +{ +#define SENSOR_CORN_DIST 225 +#define SENSOR_CORN_ANGLE 90 + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a_rad = position_get_a_rad_double(&mainboard.pos); + double x_corn, y_corn; + int16_t x_corn_int, y_corn_int; + + if (side == I2C_LEFT_SIDE) { + x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + else { + x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + x_corn_int = x_corn; + y_corn_int = y_corn; + + *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (*corn_idx < 0) + return 0; + return 1; +} + +/* + * - send the correct commands to the spickles + * - return 1 if we need to stop (cobboard is stucked) +*/ +static uint8_t handle_spickles(void) +{ + int8_t corn_idx; + + if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) + i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + else + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + } +/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ +/* time_wait_ms(100); */ + + if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) + i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + else + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + } + + return 0; +} + static void line2line(uint8_t dir1, uint8_t num1, uint8_t dir2, uint8_t num2) { @@ -1211,6 +1271,7 @@ static void line2line(uint8_t dir1, uint8_t num1, struct line_2pts l1, l2; line_t ll1, ll2; point_t p; + uint8_t err; /* convert to 2 points */ num2line(&l1, dir1, num1); @@ -1255,11 +1316,22 @@ static void line2line(uint8_t dir1, uint8_t num1, else beta_deg = -60; } + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, p.x, p.y)); - wait_traj_end(0xFF); + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return; /* XXX do something */ + } + err = test_traj_end(0xFF); + } + return; } /* function called when cmd_test is parsed successfully */ @@ -1269,11 +1341,14 @@ static void cmd_test_parsed(void *parsed_result, void *data) strat_reset_pos(298.48, 309.21, 70.02); mainboard.angle.on = 1; mainboard.distance.on = 1; + strat_set_speed(250, SPEED_ANGLE_FAST); #endif + init_corn_table(0, 0); time_wait_ms(100); line2line(LINE_UP, 0, LINE_R_DOWN, 2); - line2line(LINE_R_DOWN, 2, LINE_R_UP, 1); + line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + line2line(LINE_R_UP, 2, LINE_UP, 5); trajectory_hardstop(&mainboard.traj); } diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index edcf494..eb63f03 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -258,14 +258,14 @@ def set_robot(): robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180), + lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis lspickle.size = (20, 320, 5) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180), + rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis rspickle.size = (20, 320, 5) @@ -297,7 +297,8 @@ def silent_mkfifo(f): except: pass -init_corn_table(random.randint(0,8), random.randint(0,3)) +#init_corn_table(random.randint(0,8), random.randint(0,3)) +init_corn_table(0, 0) waypoints = init_waypoints() toggle_obj_disp() diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index a4aa028..aed92c7 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -272,7 +272,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* status */ - cobboard.mode = ans->mode; + //cobboard.mode = ans->mode; cobboard.status = ans->status; cobboard.left_cobroller_speed = ans->left_cobroller_speed; cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed); @@ -354,6 +354,7 @@ static int8_t i2c_req_cobboard_status(void) int8_t err; buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS; + buf.mode = cobboard.mode; err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); @@ -390,20 +391,38 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_cobboard_mode_eject(void) +static int8_t i2c_cobboard_set_mode(uint8_t mode) { +#ifdef HOST_VERSION + return robotsim_i2c_cobboard_set_mode(mode); +#else + cobboard.mode = mode; + return 0; +#endif + +#if 0 /* old */ struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +#endif +} + +int8_t i2c_cobboard_mode_eject(void) +{ + /* XXXXXXXXX bad bad bad */ + uint8_t mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; + i2c_cobboard_set_mode(mode); + time_wait_ms(500); + mode = cobboard.mode & (~I2C_COBBOARD_MODE_EJECT); + i2c_cobboard_set_mode(mode); + return 0; } int8_t i2c_cobboard_mode_harvest(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) { mode |= I2C_COBBOARD_MODE_L_DEPLOY; mode |= I2C_COBBOARD_MODE_L_HARVEST; @@ -412,16 +431,13 @@ int8_t i2c_cobboard_mode_harvest(uint8_t side) mode |= I2C_COBBOARD_MODE_R_DEPLOY; mode |= I2C_COBBOARD_MODE_R_HARVEST; } - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_deploy(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) { mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); mode |= I2C_COBBOARD_MODE_L_DEPLOY; @@ -430,30 +446,23 @@ int8_t i2c_cobboard_mode_deploy(uint8_t side) mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); mode |= I2C_COBBOARD_MODE_R_DEPLOY; } - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_pack(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); else mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_init(void) { - struct i2c_cmd_cobboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = I2C_COBBOARD_MODE_INIT; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); } int8_t i2c_ballboard_set_mode(uint8_t mode) diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 6e3f8ad..21c0065 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -53,13 +53,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 5fc2e37..a0fa5a0 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -91,14 +91,17 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) return 0; } -static int8_t -robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +int8_t +robotsim_i2c_cobboard_set_mode(uint8_t mode) { char buf[BUFSIZ]; int len; - cobboard.mode = cmd->mode; - len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode); + if (cobboard.mode == mode) + return 0; + + cobboard.mode = mode; + len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode); hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); @@ -127,15 +130,17 @@ robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size) 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; } diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h index a9316b9..8246500 100644 --- a/projects/microb2010/mainboard/robotsim.h +++ b/projects/microb2010/mainboard/robotsim.h @@ -25,3 +25,4 @@ void robotsim_pwm(void *arg, int32_t val); int32_t robotsim_encoder_get(void *arg); int robotsim_init(void); void robotsim_dump(void); +int8_t robotsim_i2c_cobboard_set_mode(uint8_t mode); diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index a6f85de..473ed76 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -158,14 +158,14 @@ struct sensor_filter { * CAP 1,5,6,7,8 */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { - [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ - [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ - [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ - [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ - [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ - [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 6 */ - [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 4 */ + [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ + [S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ + [S_LCOB_WHITE] = { 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 */ diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h index 0df5567..0ab7a3b 100644 --- a/projects/microb2010/mainboard/sensor.h +++ b/projects/microb2010/mainboard/sensor.h @@ -36,8 +36,8 @@ #define S_COLUMN_LEFT 3 #define S_START_SWITCH 4 #define S_DISP_LEFT 5 -#define S_DISP_RIGHT 6 -#define S_CAP8 7 +#define S_RCOB_WHITE 6 +#define S_LCOB_WHITE 7 #define S_RESERVED1 8 #define S_RESERVED2 9 #define S_RESERVED3 10 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 97d159c..1a525e5 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -58,6 +58,7 @@ #include "main.h" #include "strat.h" #include "strat_base.h" +#include "strat_corn.h" #include "strat_utils.h" #include "sensor.h" #include "actuator.h" @@ -138,12 +139,14 @@ void strat_dump_infos(const char *caller) * here */ void strat_reset_infos(void) { + init_corn_table(-1, -1); } /* call it just before launching the strat */ void strat_init(void) { /* XXX init rollers, .. */ + strat_reset_infos(); /* we consider that the color is correctly set */ diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index d852396..0a31073 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -45,20 +45,21 @@ /* * * - * - * - * +------------------------------------+ - * | | - * | | - * | | - * | | - * y | | - * | | - * |------ ------| - * | | | | - * | | | | - * +-----+------------------------+-----+ - * x + * vertical lines + * O 1 2 3 4 5 + * 2100 +-----|-----|-----|-----|-----|-----|-----+ + * | o o o | + * | o o o o | diag + * | o o o | lines + * 0/ o o o o \0 + * y | o o | + * 1/ o o \1 + * | | + * 2/------ ------\2 + * | | | | + * | | | | + * 0 +-----+-----------------------------+-----+ + * 0 x 3000 */ /* useful traj flags */ @@ -70,8 +71,8 @@ #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) /* default acc */ -#define ACC_DIST 5. -#define ACC_ANGLE 5. +#define ACC_DIST 10. +#define ACC_ANGLE 10. /* default speeds */ #define SPEED_DIST_FAST 2500. diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h index 6ff0666..d8b61e5 100644 --- a/projects/microb2010/mainboard/strat_base.h +++ b/projects/microb2010/mainboard/strat_base.h @@ -26,8 +26,9 @@ #define END_OBSTACLE 8 /* There is an obstacle in front of us */ #define END_ERROR 16 /* Cannot do the command */ #define END_INTR 32 /* interrupted by user */ -#define END_TIMER 64 /* we don't a lot of time */ -#define END_RESERVED 128 /* reserved */ +#define END_TIMER 64 /* we don't have a lot of time */ +#define END_RESERVED 128 /* reserved... be careful, sometimes error is + coded on a int8_t */ /* only valid after a END_OBSTACLE */ struct opponent_obstacle { diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c new file mode 100644 index 0000000..72029b3 --- /dev/null +++ b/projects/microb2010/mainboard/strat_corn.c @@ -0,0 +1,548 @@ +/* + * Copyright Droids, Microb Technology (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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + + +#include +#include +#include +#include + +#include +#include + +#include + +/* XXX TODO +static +const +change x,y -> i,j to avoid confusion with coords +could be optimized in mem space: it is not needed to store the x,y coord, + we can process it from idx. however it will be less optimized for speed + +*/ + +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 +#define TYPE_UNKNOWN 5 + +/* XXX enum possible ? else just rename */ +#define START 0 +#define UP 1 +#define UP_RIGHT 2 +#define DOWN_RIGHT 3 +#define DOWN 4 +#define DOWN_LEFT 5 +#define UP_LEFT 6 +#define END 7 + +struct point { + int32_t x; + int32_t y; +}; + +struct djpoint { + struct point pos; + uint16_t weight; + struct djpoint *parent; + + uint8_t type:3; + uint8_t parent_pos:3; + uint8_t updated:1; + uint8_t todo:1; +}; + +uint8_t corn_table[CORN_NB]; + +const uint8_t corn_coord_i[CORN_NB] = { + 0, 0, 0, 2, 2, 2, 4, 4, 6, + 6, 8, 8, 10, 10, 10, 12, 12, 12, +}; + +const uint8_t corn_coord_j[CORN_NB] = { + 2, 4, 6, 3, 5, 7, 4, 6, 5, + 7, 4, 6, 3, 5, 7, 2, 4, 6, +}; + +static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; + +/* table to find the symetric idx */ +uint8_t corn_sym[] = { + 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 +}; + +uint8_t corn_side_confs[9][2] = { + { 1, 4 }, + { 0, 4 }, + { 2, 4 }, + { 2, 3 }, + { 0, 3 }, + { 1, 3 }, + { 1, 6 }, + { 0, 6 }, + { 2, 6 }, +}; +uint8_t corn_center_confs[4][2] = { + { 5, 8 }, + { 7, 8 }, + { 5, 9 }, + { 7, 8 }, +}; + + +/* return index from neigh pointer */ +#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) + +void dump(void) +{ + int8_t i, j; + struct djpoint *pt; + + printf_P(PSTR(" ")); + for (i=0; i=0; j--) { + printf_P(PSTR("%3d "), j/2); + + if ((j&1) == 0) + printf_P(PSTR(" ")); + + for (i=0; itype == TYPE_OBSTACLE) + printf_P(PSTR(" X ")); + else if (pt->type == TYPE_DANGEROUS) + printf_P(PSTR(" D ")); + else if (pt->type == TYPE_WHITE_CORN) + printf_P(PSTR(" W ")); + else if (pt->type == TYPE_BLACK_CORN) + printf_P(PSTR(" B ")); + else if (pt->type == TYPE_WAYPOINT) + printf_P(PSTR(" %5d "), pt->weight); + else + printf_P(PSTR(" ? ")); + } + printf_P(PSTR("\r\n")); + } +} + +static inline uint8_t opposite_position(uint8_t pos) +{ + pos += 3; + if (pos > UP_LEFT) + pos -= 6; + return pos; +} + +/* return coord of the entry in the table from the pointer */ +static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) +{ + int8_t idx = PT2IDX(pt); + *x = idx / WAYPOINTS_NBY; + *y = idx % WAYPOINTS_NBY; +} + +/* get the neighbour of the point at specified position */ +static struct djpoint *get_neigh(struct djpoint *pt, + uint8_t position) +{ + int8_t i,j; + struct djpoint *neigh; + + djpoint2ij(pt, &i, &j); + + switch (position) { + case UP: + j++; + break; + case UP_RIGHT: + if (!(i & 1)) j++; + i++; + break; + case DOWN_RIGHT: + if (i & 1) j--; + i++; + break; + case DOWN: + j--; + break; + case DOWN_LEFT: + if (i & 1) j--; + i--; + break; + case UP_LEFT: + if (!(i & 1)) j++; + i--; + break; + default: + return NULL; + } + if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return NULL; + + neigh = &djpoints[i][j]; + + if (neigh->type != TYPE_WAYPOINT) + return NULL; + + return neigh; +} + +static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) +{ + struct djpoint *neigh = NULL; + uint8_t pos = *position + 1; + + for (pos = *position + 1; pos < END; pos++) { + neigh = get_neigh(pt, pos); + if (neigh != NULL) + break; + } + + *position = pos; + return neigh; +} + +/* browse all points */ +#define POINT_FOREACH(cur) \ + for (cur = &djpoints[0][0]; \ + cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ + cur ++) + +/* XXX comment */ +#define NEIGH_FOREACH(neigh, pos, point) \ + for (pos = START, neigh = get_next_neigh(point, &pos); \ + neigh; \ + neigh = get_next_neigh(point, &pos)) + +int dijkstra_init(void) +{ + return 0; +} + +static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +{ + double vx, vy; + vx = p2->pos.x - p1->pos.x; + vy = p2->pos.y - p1->pos.y; + return sqrt(vx * vx + vy * vy); +} + +void dijkstra_process_neighs(struct djpoint *pt) +{ + struct djpoint *neigh; + uint8_t pos, parent_pos; + uint16_t weight; + int8_t i,j; + + djpoint2ij(pt, &i, &j); + printf_P(PSTR("at %d %d:\r\n"), i, j); + + NEIGH_FOREACH(neigh, pos, pt) { + weight = pt->weight + dist(pt, neigh); + parent_pos = opposite_position(pos); + + /* bonus if we keep the same direction */ + if (parent_pos == pt->parent_pos || + pt->parent_pos == END) + weight = weight - 1; + + printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, + pt->parent_pos, parent_pos, + neigh->weight, weight); + if (neigh->weight == 0 || weight < neigh->weight) { + djpoint2ij(neigh, &i, &j); + //printf_P(PSTR(" %d,%d updated\r\n"), i, j); + neigh->weight = weight; + neigh->parent_pos = parent_pos; + neigh->updated = 1; + } + } +} + +int dijkstra(struct djpoint *start) +{ + struct djpoint *cur; + uint8_t todolist = 1; + + start->todo = 1; + + while (todolist) { + printf_P(PSTR("\r\n")); + dump(); + /* process all neighbours of todo list */ + POINT_FOREACH(cur) { + if (!cur->todo) + continue; + dijkstra_process_neighs(cur); + cur->todo = 0; + } + + /* convert updated list in todo list */ + todolist = 0; + POINT_FOREACH(cur) { + if (!cur->updated) + continue; + todolist = 1; + cur->todo = 1; + cur->updated = 0; + } + } + return 0; /* XXX */ +} + +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) +{ + uint8_t n; + for (n = 0; n < CORN_NB; n ++) { + if (i == corn_coord_i[n] && + j == corn_coord_j[n]) + return n; + } + return -1; +} + +int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j) +{ + if (idx >= CORN_NB) + return -1; + *i = corn_coord_i[idx]; + *j = corn_coord_j[idx]; + return 0; +} + +/* return the index of the closest corn at these coordinates. If the + * corn is really too far (~20cm), return -1. The x and y pointer are + * updated with the real position of the corn */ +int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y) +{ + uint8_t idx = -1, n, i, j; + int16_t d, x_corn, y_corn; + int16_t x_corn_min = 0, y_corn_min = 0; + int16_t d_min = 0; + + for (n = 0; n < CORN_NB; n ++) { + corn_idx_to_coordij(n, &i, &j); + x_corn = (OFFSET_CORN_X + i*STEP_CORN_X); + y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y); + d = xy_norm(x_corn, y_corn, *x, *y); + if (d < 200 && (d_min == 0 || d < d_min)) { + d_min = d; + idx = n; + x_corn_min = x_corn; + y_corn_min = y_corn; + } + } + if (d_min == 0) + return -1; + + *x = x_corn_min; + *y = y_corn_min; + + return idx; +} + +int8_t corn_get_sym(int8_t i) +{ + if (i >= CORN_NB) + return -1; + return corn_sym[i]; +} + +void init_corn_table(int8_t conf_side, int8_t conf_center) +{ + int8_t sym, i; + + /* before match */ + if (conf_side == -1) { + for (i=0; ipos.x = x; + pt->pos.y = y; + + pt->type = TYPE_WAYPOINT; + pt->parent_pos = END; + pt->updated = 0; + pt->todo = 0; + + y += STEP_CORN_Y; + } + + x += STEP_CORN_X; + } +} + +/* update the type and weight of waypoints, before starting + * dijkstra */ +void update_waypoints(void) +{ + int8_t i, j, c; + struct djpoint *pt; + + for (i=0; iweight = 0; + + /* corn */ + c = ijcoord_to_corn_idx(i, j); + if (c >= 0) { + pt->type = corn_table[c]; + continue; + } + /* too close of border */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + pt->type = TYPE_WAYPOINT; + } + } +} + +int get_path(struct djpoint *start, struct djpoint *end) +{ + struct djpoint *cur; + uint8_t prev_direction = 0; + + for (cur=start; + cur != NULL && cur->parent_pos != END && cur != end; + cur = get_neigh(cur, cur->parent_pos)) { + if (prev_direction != cur->parent_pos) { + printf_P(PSTR("%d %d (%d)\r\n"), + cur->pos.x, cur->pos.y, + cur->parent_pos); + } + prev_direction = cur->parent_pos; + } + printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y); + + return 0; /* XXX */ +} + +#if 0 +int main(void) +{ + struct djpoint *start; + struct djpoint *end; + + start = &djpoints[1][1]; + end = &djpoints[12][1]; + + init_corn_table(0, 0); + init_waypoints(); + update_waypoints(); + + dijkstra(end); + dump(); + + get_path(start, end); + + return 0; +} +#endif diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h new file mode 100644 index 0000000..8ab2398 --- /dev/null +++ b/projects/microb2010/mainboard/strat_corn.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids, Microb Technology (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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#define CORN_NB 18 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 + +extern uint8_t corn_table[CORN_NB]; + +int8_t ijcoord_to_corn_idx(int8_t i, int8_t j); +int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); +void init_corn_table(int8_t conf_side, int8_t conf_center); -- 2.20.1