From: Olivier Matz Date: Sun, 21 Mar 2010 00:19:05 +0000 (+0100) Subject: code 2010 X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=1714f4ee916fca95ce24120ea6e698237913f947;p=aversive.git code 2010 --- diff --git a/projects/microb2010/cobboard/Makefile b/projects/microb2010/cobboard/Makefile index 0c93238..8293a3f 100644 --- a/projects/microb2010/cobboard/Makefile +++ b/projects/microb2010/cobboard/Makefile @@ -11,7 +11,7 @@ LDFLAGS = -T ../common/avr6.x SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_cobboard.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c -SRC += state.c ax12_user.c +SRC += state.c ax12_user.c spickle.c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 67598ea..8c15480 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -40,9 +40,9 @@ #include +#include "sensor.h" #include "../common/i2c_commands.h" #include "actuator.h" -#include "ax12_user.h" #include "main.h" void servo_carry_open(void) diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index cd2e28b..7ef1572 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -19,9 +19,14 @@ * */ +#ifndef _ACTUATOR_H_ +#define _ACTUATOR_H_ + void actuator_init(void); void servo_carry_open(void); void servo_carry_close(void); void servo_door_open(void); void servo_door_close(void); +#endif + diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c index 1723f2a..ca5ded6 100644 --- a/projects/microb2010/cobboard/commands.c +++ b/projects/microb2010/cobboard/commands.c @@ -72,6 +72,9 @@ extern parse_pgm_inst_t cmd_state_debug; extern parse_pgm_inst_t cmd_state_machine; extern parse_pgm_inst_t cmd_servo_door; extern parse_pgm_inst_t cmd_servo_carry; +extern parse_pgm_inst_t cmd_spickle; +extern parse_pgm_inst_t cmd_spickle_params; +extern parse_pgm_inst_t cmd_spickle_params_show; extern parse_pgm_inst_t cmd_test; @@ -126,6 +129,9 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_state_machine, (parse_pgm_inst_t *)&cmd_servo_door, (parse_pgm_inst_t *)&cmd_servo_carry, + (parse_pgm_inst_t *)&cmd_spickle, + (parse_pgm_inst_t *)&cmd_spickle_params, + (parse_pgm_inst_t *)&cmd_spickle_params_show, (parse_pgm_inst_t *)&cmd_test, NULL, diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index b822e4a..3c6e3e3 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -50,7 +50,7 @@ #include "state.h" #include "i2c_protocol.h" #include "actuator.h" -#include "actuator.h" +#include "spickle.h" extern uint16_t state_debug; @@ -448,6 +448,122 @@ parse_pgm_inst_t cmd_servo_carry = { }, }; +/**********************************************************/ +/* Spickles tests */ + +/* this structure is filled when cmd_spickle is parsed successfully */ +struct cmd_spickle_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_spickle is parsed successfully */ +static void cmd_spickle_parsed(void * parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_spickle_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("up"))) { + spickle_up(); + } + else if (!strcmp_P(res->arg1, PSTR("down"))) { + spickle_down(); + } + else if (!strcmp_P(res->arg1, PSTR("stop"))) { + spickle_stop(); + } + else if (!strcmp_P(res->arg1, PSTR("auto"))) { + spickle_auto(); + } + + printf_P(PSTR("done\r\n")); +} + +prog_char str_spickle_arg0[] = "spickle"; +parse_pgm_token_string_t cmd_spickle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0); +prog_char str_spickle_arg1[] = "auto#up#down#stop"; +parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1); + +prog_char help_spickle[] = "spickle auto mode: spickle auto delay_up delay_down"; +parse_pgm_inst_t cmd_spickle = { + .f = cmd_spickle_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spickle, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spickle_arg0, + (prog_void *)&cmd_spickle_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Set Spickle Params */ + +/* this structure is filled when cmd_spickle_params is parsed successfully */ +struct cmd_spickle_params_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_spickle_params is parsed successfully */ +static void cmd_spickle_params_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_spickle_params_result * res = parsed_result; + + + if (!strcmp_P(res->arg1, PSTR("delay"))) { + spickle_set_delays(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("coef"))) { + spickle_set_coefs(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("pos"))) { + spickle_set_pos(res->arg2, res->arg3); + } + + /* else show */ + spickle_dump_params(); +} + +prog_char str_spickle_params_arg0[] = "spickle_params"; +parse_pgm_token_string_t cmd_spickle_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0); +prog_char str_spickle_params_arg1[] = "delay#pos#coef"; +parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1); +parse_pgm_token_num_t cmd_spickle_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg2, INT32); +parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32); + +prog_char help_spickle_params[] = "Set spickle_params values"; +parse_pgm_inst_t cmd_spickle_params = { + .f = cmd_spickle_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spickle_params, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1, + (prog_void *)&cmd_spickle_params_arg2, + (prog_void *)&cmd_spickle_params_arg3, + NULL, + }, +}; + +prog_char str_spickle_params_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_spickle_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show); + +prog_char help_spickle_params_show[] = "show spickle params"; +parse_pgm_inst_t cmd_spickle_params_show = { + .f = cmd_spickle_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spickle_params_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1_show, + NULL, + }, +}; + /**********************************************************/ /* Test */ diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index 4d8ccb0..17d8e54 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -45,6 +45,7 @@ #include "main.h" #include "actuator.h" +#include "spickle.h" /* called every 5 ms */ static void do_cs(__attribute__((unused)) void *dummy) @@ -106,21 +107,15 @@ void microb_cs_init(void) /* ---- CS left_spickle */ /* PID */ pid_init(&cobboard.left_spickle.pid); - pid_set_gains(&cobboard.left_spickle.pid, 500, 40, 5000); - pid_set_maximums(&cobboard.left_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + 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_out_shift(&cobboard.left_spickle.pid, 10); pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); - /* QUADRAMP */ - quadramp_init(&cobboard.left_spickle.qr); - quadramp_set_1st_order_vars(&cobboard.left_spickle.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.left_spickle.qr, 20, 20); /* set accel */ - /* CS */ cs_init(&cobboard.left_spickle.cs); - cs_set_consign_filter(&cobboard.left_spickle.cs, quadramp_do_filter, &cobboard.left_spickle.qr); cs_set_correct_filter(&cobboard.left_spickle.cs, pid_do_filter, &cobboard.left_spickle.pid); - cs_set_process_in(&cobboard.left_spickle.cs, pwm_ng_set, LEFT_SPICKLE_PWM); + cs_set_process_in(&cobboard.left_spickle.cs, spickle_set, LEFT_SPICKLE_PWM); cs_set_process_out(&cobboard.left_spickle.cs, encoders_spi_get_value, LEFT_SPICKLE_ENCODER); cs_set_consign(&cobboard.left_spickle.cs, 0); @@ -132,21 +127,15 @@ void microb_cs_init(void) /* ---- CS right_spickle */ /* PID */ pid_init(&cobboard.right_spickle.pid); - pid_set_gains(&cobboard.right_spickle.pid, 500, 40, 5000); - pid_set_maximums(&cobboard.right_spickle.pid, 0, 5000, 2400); /* max is 12 V */ + 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_out_shift(&cobboard.right_spickle.pid, 10); - pid_set_derivate_filter(&cobboard.right_spickle.pid, 6); - - /* QUADRAMP */ - quadramp_init(&cobboard.right_spickle.qr); - quadramp_set_1st_order_vars(&cobboard.right_spickle.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.right_spickle.qr, 20, 20); /* set accel */ + pid_set_derivate_filter(&cobboard.right_spickle.pid, 4); /* CS */ cs_init(&cobboard.right_spickle.cs); - cs_set_consign_filter(&cobboard.right_spickle.cs, quadramp_do_filter, &cobboard.right_spickle.qr); cs_set_correct_filter(&cobboard.right_spickle.cs, pid_do_filter, &cobboard.right_spickle.pid); - cs_set_process_in(&cobboard.right_spickle.cs, pwm_ng_set, RIGHT_SPICKLE_PWM); + cs_set_process_in(&cobboard.right_spickle.cs, spickle_set, RIGHT_SPICKLE_PWM); cs_set_process_out(&cobboard.right_spickle.cs, encoders_spi_get_value, RIGHT_SPICKLE_ENCODER); cs_set_consign(&cobboard.right_spickle.cs, 0); @@ -158,19 +147,14 @@ void microb_cs_init(void) /* ---- CS shovel */ /* PID */ pid_init(&cobboard.shovel.pid); - pid_set_gains(&cobboard.shovel.pid, 500, 40, 5000); - pid_set_maximums(&cobboard.shovel.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.shovel.pid, 1000, 10, 200); + pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */ pid_set_out_shift(&cobboard.shovel.pid, 10); - pid_set_derivate_filter(&cobboard.shovel.pid, 6); - - /* QUADRAMP */ - quadramp_init(&cobboard.shovel.qr); - quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 1000); /* set speed */ - quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 20, 20); /* set accel */ + pid_set_derivate_filter(&cobboard.shovel.pid, 4); /* CS */ cs_init(&cobboard.shovel.cs); - cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); + //cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid); cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM); cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER); @@ -181,10 +165,10 @@ void microb_cs_init(void) bd_set_speed_threshold(&cobboard.shovel.bd, 150); bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40); - /* set them on !! */ - cobboard.left_spickle.on = 1; - cobboard.right_spickle.on = 1; - cobboard.shovel.on = 1; + /* set them on (or not) !! */ + cobboard.left_spickle.on = 0; + cobboard.right_spickle.on = 0; + cobboard.shovel.on = 0; scheduler_add_periodical_event_priority(do_cs, NULL, CS_PERIOD / SCHEDULER_UNIT, diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 418f5e1..d0526a5 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -57,7 +57,7 @@ #include "sensor.h" #include "state.h" #include "actuator.h" -#include "arm_xy.h" +#include "spickle.h" #include "cs.h" #include "i2c_protocol.h" @@ -166,7 +166,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_COBBOARD) { @@ -200,12 +200,11 @@ 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, - &PORTD, 4); + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | + PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED, &PORTD, 5); - PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED | - PWM_NG_MODE_SIGN_INVERTED, + PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, &PORTD, 6); PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, @@ -246,10 +245,13 @@ int main(void) sei(); - /* finger + other actuators */ + /* actuators */ actuator_init(); - state_init(); + /* spickle */ + spickle_init(); + +/* state_init(); */ printf_P(PSTR("\r\n")); printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); @@ -259,7 +261,7 @@ int main(void) gen.log_level = 5; cobboard.flags |= DO_CS; - state_machine(); +/* state_machine(); */ cmdline_interact(); return 0; diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 33fb833..d50b610 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -48,11 +48,11 @@ #define LEFT_SPICKLE_ENCODER ((void *)0) #define RIGHT_SPICKLE_ENCODER ((void *)1) -#define SHOVEL_ENCODER ((void *)1) +#define SHOVEL_ENCODER ((void *)2) #define LEFT_SPICKLE_PWM ((void *)&gen.pwm1_4A) #define RIGHT_SPICKLE_PWM ((void *)&gen.pwm2_4B) -#define SHOVEL_PWM ((void *)&gen.pwm2_4B) +#define SHOVEL_PWM ((void *)&gen.pwm3_1A) /** ERROR NUMS */ #define E_USER_I2C_PROTO 195 @@ -64,6 +64,7 @@ #define LED_PRIO 170 #define TIME_PRIO 160 +#define SPICKLE_PRIO 130 #define ADC_PRIO 120 #define CS_PRIO 100 #define I2C_POLL_PRIO 20 diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index a80201f..0889320 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -146,8 +146,8 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_FRONT] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ [S_CAP3] = { 10, 0, 3, 7, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_COL_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 5 */ + [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ + [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 27460db..6f47742 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -32,7 +32,7 @@ #define S_FRONT 1 #define S_CAP3 2 #define S_CAP4 3 -#define S_COL_LEFT 4 +#define S_LCOB 4 #define S_LEFT 5 #define S_RIGHT 6 #define S_COL_RIGHT 7 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c new file mode 100644 index 0000000..db954ed --- /dev/null +++ b/projects/microb2010/cobboard/spickle.c @@ -0,0 +1,200 @@ +/* + * Copyright Droids Corporation (2009) + * + * 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: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "sensor.h" +#include "../common/i2c_commands.h" +#include "actuator.h" +#include "main.h" + + +#define OFF 0 +#define WAIT_SENSOR 1 +#define SENSOR_OK 2 +#define WAIT_DOWN 3 + +static volatile uint8_t spickle_state = OFF; +static volatile uint32_t spickle_pos_up = 35000; +static volatile uint32_t spickle_pos_down = 0; +static volatile uint32_t spickle_delay_up = 500; +static volatile uint32_t spickle_delay_down = 2000; +static volatile uint32_t delay = 0; +static volatile int32_t spickle_k1 = 500, spickle_k2 = 20; +static volatile int32_t spickle_cmd = 0; + +/* init spickle position at beginning */ +static void spickle_autopos(void) +{ + pwm_ng_set(LEFT_SPICKLE_PWM, -500); + wait_ms(3000); + pwm_ng_set(LEFT_SPICKLE_PWM, 0); + encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0); +} + +/* set CS command for spickle */ +void spickle_set(void *mot, int32_t cmd) +{ + static int32_t oldpos_left, oldpos_right; + int32_t oldpos, pos, maxcmd, speed; + + if (mot == LEFT_SPICKLE_PWM) { + pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER); + oldpos = oldpos_left; + } + else { + pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER); + oldpos = oldpos_right; + } + + speed = pos - oldpos; + if (speed > 0 && cmd < 0) + maxcmd = spickle_k1; + else if (speed < 0 && cmd > 0) + maxcmd = spickle_k1; + else { + speed = ABS(speed); + maxcmd = spickle_k1 + spickle_k2 * speed; + } + if (cmd > maxcmd) + cmd = maxcmd; + else if (cmd < -maxcmd) + cmd = -maxcmd; + + pwm_ng_set(mot, cmd); + + if (mot == LEFT_SPICKLE_PWM) + oldpos_left = pos; + else + oldpos_right = pos; +} + +void spickle_set_coefs(uint32_t k1, uint32_t k2) +{ + spickle_k1 = k1; + spickle_k2 = k2; +} + +void spickle_set_delays(uint32_t delay_up, uint32_t delay_down) +{ + spickle_delay_up = delay_up; + spickle_delay_down = delay_down; +} + +void spickle_set_pos(uint32_t pos_up, uint32_t pos_down) +{ + spickle_pos_up = pos_up; + spickle_pos_down = pos_down; +} + +void spickle_dump_params(void) +{ + printf_P(PSTR("coef %ld %ld\r\n"), spickle_k1, spickle_k2); + printf_P(PSTR("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down); + printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down); +} + +void spickle_up(void) +{ + spickle_state = 0; + cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); +} + +void spickle_down(void) +{ + spickle_state = 0; + cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); +} + +void spickle_stop(void) +{ + spickle_state = 0; +} + +void spickle_auto(void) +{ + spickle_state = WAIT_SENSOR; + cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); +} + +/* for spickle auto mode */ +static void spickle_cb(__attribute__((unused)) void *dummy) +{ + static uint8_t prev = 0; + uint8_t val; + + val = sensor_get(S_LCOB); + + switch (spickle_state) { + case OFF: + break; + case WAIT_SENSOR: + if (val && !prev) { + delay = spickle_delay_up; + spickle_state = SENSOR_OK; + } + break; + case SENSOR_OK: + if (delay-- == 0) { + cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); + spickle_state = WAIT_DOWN; + delay = spickle_delay_down; + } + break; + case WAIT_DOWN: + if (delay-- == 0) { + cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); + spickle_state = WAIT_SENSOR; + } + break; + default: + break; + } + prev = val; +} + +void spickle_init(void) +{ + spickle_autopos(); + + scheduler_add_periodical_event_priority(spickle_cb, NULL, + 1000L / SCHEDULER_UNIT, + SPICKLE_PRIO); +} diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h new file mode 100644 index 0000000..c2ffcf0 --- /dev/null +++ b/projects/microb2010/cobboard/spickle.h @@ -0,0 +1,37 @@ +/* + * 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: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +#ifndef _SPICKLE_H_ +#define _SPICKLE_H_ + +void spickle_set(void *dummy, int32_t cmd); +void spickle_set_coefs(uint32_t k1, uint32_t k2); +void spickle_set_delays(uint32_t delay_up, uint32_t delay_down); +void spickle_set_pos(uint32_t pos_up, uint32_t pos_down); +void spickle_dump_params(void); +void spickle_left_manage(void); +void spickle_up(void); +void spickle_down(void); +void spickle_stop(void); +void spickle_auto(void); +void spickle_init(void); + +#endif diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 35605b8..9672d6a 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -22,6 +22,8 @@ #ifndef _I2C_COMMANDS_H_ #define _I2C_COMMANDS_H_ +#define I2C_OPPONENT_NOT_THERE -1000 + #define I2C_MAINBOARD_ADDR 1 #define I2C_COBBOARD_ADDR 2 #define I2C_BALLBOARD_ADDR 3 diff --git a/projects/microb2010/mainboard/.config b/projects/microb2010/mainboard/.config index 21ce9e5..e4e1ace 100644 --- a/projects/microb2010/mainboard/.config +++ b/projects/microb2010/mainboard/.config @@ -83,6 +83,7 @@ CONFIG_MODULE_CIRBUF=y CONFIG_MODULE_FIXED_POINT=y CONFIG_MODULE_VECT2=y CONFIG_MODULE_GEOMETRY=y +# CONFIG_MODULE_HOSTSIM is not set CONFIG_MODULE_SCHEDULER=y CONFIG_MODULE_SCHEDULER_STATS=y # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 5867b4e..2b18f20 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -6,15 +6,11 @@ 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_avoid.c strat.c -SRC += strat_static_columns.c strat_lintel.c -SRC += strat_column_disp.c strat_building.c strat_scan.c +SRC += strat_utils.c strat_base.c strat.c ASRC = CFLAGS += -Wall -Werror -#CFLAGS += -DHOMOLOGATION -CFLAGS += -DTEST_BEACON LDFLAGS = -T ../common/avr6.x ######################################## diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c index 19d2fc7..2088874 100644 --- a/projects/microb2010/mainboard/actuator.c +++ b/projects/microb2010/mainboard/actuator.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -65,13 +65,3 @@ void pwm_set_and_save(void *pwm, int32_t val) pwm_ng_set(pwm, val); } -void pickup_wheels_on(void) -{ - mainboard.enable_pickup_wheels = 1; -} - -void pickup_wheels_off(void) -{ - mainboard.enable_pickup_wheels = 0; -} - diff --git a/projects/microb2010/mainboard/actuator.h b/projects/microb2010/mainboard/actuator.h index 9c7174a..22d10b7 100644 --- a/projects/microb2010/mainboard/actuator.h +++ b/projects/microb2010/mainboard/actuator.h @@ -20,6 +20,4 @@ */ void pwm_set_and_save(void *pwm, int32_t val); -void pickup_wheels_on(void); -void pickup_wheels_off(void); diff --git a/projects/microb2010/mainboard/ax12_user.c b/projects/microb2010/mainboard/ax12_user.c index 575f5bc..de5bb88 100644 --- a/projects/microb2010/mainboard/ax12_user.c +++ b/projects/microb2010/mainboard/ax12_user.c @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/mainboard/cmdline.c b/projects/microb2010/mainboard/cmdline.c index 7c3ec9e..eeeede6 100644 --- a/projects/microb2010/mainboard/cmdline.c +++ b/projects/microb2010/mainboard/cmdline.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index 26060d6..5d49e78 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -70,28 +70,11 @@ extern parse_pgm_inst_t cmd_interact; extern parse_pgm_inst_t cmd_color; extern parse_pgm_inst_t cmd_rs; extern parse_pgm_inst_t cmd_i2cdebug; -extern parse_pgm_inst_t cmd_mechboard_show; -extern parse_pgm_inst_t cmd_mechboard_setmode1; -extern parse_pgm_inst_t cmd_mechboard_setmode2; -extern parse_pgm_inst_t cmd_mechboard_setmode3; -extern parse_pgm_inst_t cmd_mechboard_setmode4; -extern parse_pgm_inst_t cmd_mechboard_setmode5; -extern parse_pgm_inst_t cmd_pickup_wheels; +extern parse_pgm_inst_t cmd_cobboard_show; +extern parse_pgm_inst_t cmd_cobboard_setmode1; +extern parse_pgm_inst_t cmd_cobboard_setmode2; +extern parse_pgm_inst_t cmd_cobboard_setmode3; extern parse_pgm_inst_t cmd_beacon_start; -extern parse_pgm_inst_t cmd_pump_current; -extern parse_pgm_inst_t cmd_build_test; -extern parse_pgm_inst_t cmd_column_test; -extern parse_pgm_inst_t cmd_column_test2; -extern parse_pgm_inst_t cmd_lintel_test; -extern parse_pgm_inst_t cmd_pickup_test; -extern parse_pgm_inst_t cmd_scan_test; -extern parse_pgm_inst_t cmd_scan_test2; -extern parse_pgm_inst_t cmd_time_monitor; -extern parse_pgm_inst_t cmd_scanner; -extern parse_pgm_inst_t cmd_build_z1; -#ifdef TEST_BEACON -extern parse_pgm_inst_t cmd_beacon_opp_dump; -#endif extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -170,28 +153,10 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_color, (parse_pgm_inst_t *)&cmd_rs, (parse_pgm_inst_t *)&cmd_i2cdebug, - (parse_pgm_inst_t *)&cmd_mechboard_show, - (parse_pgm_inst_t *)&cmd_mechboard_setmode1, - (parse_pgm_inst_t *)&cmd_mechboard_setmode2, - (parse_pgm_inst_t *)&cmd_mechboard_setmode3, - (parse_pgm_inst_t *)&cmd_mechboard_setmode4, - (parse_pgm_inst_t *)&cmd_mechboard_setmode5, - (parse_pgm_inst_t *)&cmd_pickup_wheels, - (parse_pgm_inst_t *)&cmd_beacon_start, - (parse_pgm_inst_t *)&cmd_pump_current, - (parse_pgm_inst_t *)&cmd_build_test, - (parse_pgm_inst_t *)&cmd_column_test, - (parse_pgm_inst_t *)&cmd_column_test2, - (parse_pgm_inst_t *)&cmd_lintel_test, - (parse_pgm_inst_t *)&cmd_pickup_test, - (parse_pgm_inst_t *)&cmd_scan_test, - (parse_pgm_inst_t *)&cmd_scan_test2, - (parse_pgm_inst_t *)&cmd_time_monitor, - (parse_pgm_inst_t *)&cmd_scanner, - (parse_pgm_inst_t *)&cmd_build_z1, -#ifdef TEST_BEACON - (parse_pgm_inst_t *)&cmd_beacon_opp_dump, -#endif + (parse_pgm_inst_t *)&cmd_cobboard_show, + (parse_pgm_inst_t *)&cmd_cobboard_setmode1, + (parse_pgm_inst_t *)&cmd_cobboard_setmode2, + (parse_pgm_inst_t *)&cmd_cobboard_setmode3, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ @@ -217,7 +182,6 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_strat_conf, (parse_pgm_inst_t *)&cmd_strat_conf2, (parse_pgm_inst_t *)&cmd_strat_conf3, - (parse_pgm_inst_t *)&cmd_strat_conf4, (parse_pgm_inst_t *)&cmd_subtraj, NULL, }; diff --git a/projects/microb2010/mainboard/commands_ax12.c b/projects/microb2010/mainboard/commands_ax12.c index 804e084..75616f7 100644 --- a/projects/microb2010/mainboard/commands_ax12.c +++ b/projects/microb2010/mainboard/commands_ax12.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index a39ab07..e6850f3 100644 --- a/projects/microb2010/mainboard/commands_cs.c +++ b/projects/microb2010/mainboard/commands_cs.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -60,9 +60,13 @@ struct csb_list { prog_char csb_angle_str[] = "angle"; prog_char csb_distance_str[] = "distance"; +prog_char csb_left_cobroller_str[] = "left_cobroller"; +prog_char csb_right_cobroller_str[] = "right_cobroller"; struct csb_list csb_list[] = { { .name = csb_angle_str, .csb = &mainboard.angle }, { .name = csb_distance_str, .csb = &mainboard.distance }, + { .name = csb_left_cobroller_str, .csb = &mainboard.left_cobroller }, + { .name = csb_right_cobroller_str, .csb = &mainboard.right_cobroller }, }; struct cmd_cs_result { @@ -71,7 +75,7 @@ struct cmd_cs_result { }; /* token to be used for all cs-related commands */ -prog_char str_csb_name[] = "angle#distance"; +prog_char str_csb_name[] = "angle#distance#left_cobroller#right_cobroller"; parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name); struct cs_block *cs_from_name(const char *name) diff --git a/projects/microb2010/mainboard/commands_gen.c b/projects/microb2010/mainboard/commands_gen.c index b20dcc8..fb9024d 100644 --- a/projects/microb2010/mainboard/commands_gen.c +++ b/projects/microb2010/mainboard/commands_gen.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 7c222b3..6349ad7 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -293,23 +293,15 @@ static void cmd_start_parsed(void *parsed_result, void *data) if (!strcmp_P(res->color, PSTR("red"))) { mainboard.our_color = I2C_COLOR_RED; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED); } else if (!strcmp_P(res->color, PSTR("green"))) { mainboard.our_color = I2C_COLOR_GREEN; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN); } - printf_P(PSTR("Check that lintel is loaded\r\n")); - while(!cmdline_keypressed()); - - printf_P(PSTR("Press a key when beacon ready\r\n")); - i2c_sensorboard_set_beacon(0); - while(!cmdline_keypressed()); - i2c_sensorboard_set_beacon(1); - strat_start(); gen.logs[NB_LOGS] = 0; @@ -553,13 +545,13 @@ static void cmd_color_parsed(void *parsed_result, void *data) struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; if (!strcmp_P(res->color, PSTR("red"))) { mainboard.our_color = I2C_COLOR_RED; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED); } else if (!strcmp_P(res->color, PSTR("green"))) { mainboard.our_color = I2C_COLOR_GREEN; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN); } printf_P(PSTR("Done\r\n")); } @@ -657,1464 +649,161 @@ parse_pgm_inst_t cmd_i2cdebug = { }; /**********************************************************/ -/* Mechboard_Show */ +/* Cobboard_Show */ -/* this structure is filled when cmd_mechboard_show is parsed successfully */ -struct cmd_mechboard_show_result { +/* this structure is filled when cmd_cobboard_show is parsed successfully */ +struct cmd_cobboard_show_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_mechboard_show is parsed successfully */ -static void cmd_mechboard_show_parsed(void * parsed_result, void * data) +/* function called when cmd_cobboard_show is parsed successfully */ +static void cmd_cobboard_show_parsed(void * parsed_result, void * data) { - printf_P(PSTR("mode = %x\r\n"), mechboard.mode); - printf_P(PSTR("status = %x\r\n"), mechboard.status); - printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count); - - printf_P(PSTR("column_count = %d\r\n"), get_column_count()); - printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"), - pump_left1_is_full(), pump_left2_is_full(), - pump_right1_is_full(), pump_right2_is_full()); - - printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1); - printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2); - printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1); - printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2); - - printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left); - printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right); - + printf_P(PSTR("mode = %x\r\n"), cobboard.mode); + printf_P(PSTR("status = %x\r\n"), cobboard.status); } -prog_char str_mechboard_show_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0); -prog_char str_mechboard_show_arg1[] = "show"; -parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1); +prog_char str_cobboard_show_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0); +prog_char str_cobboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1); -prog_char help_mechboard_show[] = "show mechboard status"; -parse_pgm_inst_t cmd_mechboard_show = { - .f = cmd_mechboard_show_parsed, /* function to call */ +prog_char help_cobboard_show[] = "show cobboard status"; +parse_pgm_inst_t cmd_cobboard_show = { + .f = cmd_cobboard_show_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_show, + .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_show_arg0, - (prog_void *)&cmd_mechboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode1 */ +/* Cobboard_Setmode1 */ -/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */ -struct cmd_mechboard_setmode1_result { +/* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */ +struct cmd_cobboard_setmode1_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_mechboard_setmode1 is parsed successfully */ -static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data) +/* function called when cmd_cobboard_setmode1 is parsed successfully */ +static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { - struct cmd_mechboard_setmode1_result *res = parsed_result; + struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_mechboard_mode_init(); + i2c_cobboard_mode_init(); else if (!strcmp_P(res->arg1, PSTR("manual"))) - i2c_mechboard_mode_manual(); - else if (!strcmp_P(res->arg1, PSTR("pickup"))) - i2c_mechboard_mode_pickup(); - else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) - i2c_mechboard_mode_lazy_harvest(); + i2c_cobboard_mode_manual(); else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_mechboard_mode_harvest(); - else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) - i2c_mechboard_mode_prepare_get_lintel(); - else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) - i2c_mechboard_mode_get_lintel(); - else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) - i2c_mechboard_mode_put_lintel(); - else if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_mechboard_mode_init(); - else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_mechboard_mode_init(); - else if (!strcmp_P(res->arg1, PSTR("clear"))) - i2c_mechboard_mode_clear(); - else if (!strcmp_P(res->arg1, PSTR("loaded"))) - i2c_mechboard_mode_loaded(); - else if (!strcmp_P(res->arg1, PSTR("store"))) - i2c_mechboard_mode_store(); - else if (!strcmp_P(res->arg1, PSTR("manivelle"))) - i2c_mechboard_mode_manivelle(); - else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) - i2c_mechboard_mode_lazy_pickup(); + i2c_cobboard_mode_harvest(); } -prog_char str_mechboard_setmode1_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0); -prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup"; -parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1); +prog_char str_cobboard_setmode1_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0); +prog_char str_cobboard_setmode1_arg1[] = "init#manual#harvest"; +parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1); -prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)"; -parse_pgm_inst_t cmd_mechboard_setmode1 = { - .f = cmd_mechboard_setmode1_parsed, /* function to call */ +prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)"; +parse_pgm_inst_t cmd_cobboard_setmode1 = { + .f = cmd_cobboard_setmode1_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode1, + .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode1_arg0, - (prog_void *)&cmd_mechboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode2 */ +/* Cobboard_Setmode2 */ -/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */ -struct cmd_mechboard_setmode2_result { +/* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */ +struct cmd_cobboard_setmode2_result { fixed_string_t arg0; fixed_string_t arg1; fixed_string_t arg2; }; -/* function called when cmd_mechboard_setmode2 is parsed successfully */ -static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data) +/* function called when cmd_cobboard_setmode2 is parsed successfully */ +static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) { - struct cmd_mechboard_setmode2_result *res = parsed_result; + struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; if (!strcmp_P(res->arg2, PSTR("left"))) side = I2C_LEFT_SIDE; else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else if (!strcmp_P(res->arg2, PSTR("auto"))) - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) - i2c_mechboard_mode_prepare_pickup(side); - else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) - i2c_mechboard_mode_push_temple_disc(side); -} - -prog_char str_mechboard_setmode2_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0); -prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1); -prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2); - -prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)"; -parse_pgm_inst_t cmd_mechboard_setmode2 = { - .f = cmd_mechboard_setmode2_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode2, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode2_arg0, - (prog_void *)&cmd_mechboard_setmode2_arg1, - (prog_void *)&cmd_mechboard_setmode2_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* Mechboard_Setmode3 */ - -/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */ -struct cmd_mechboard_setmode3_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint8_t level; -}; - -/* function called when cmd_mechboard_setmode3 is parsed successfully */ -static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data) -{ - struct cmd_mechboard_setmode3_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("autobuild"))) - i2c_mechboard_mode_simple_autobuild(res->level); - else if (!strcmp_P(res->arg1, PSTR("prepare_build"))) - i2c_mechboard_mode_prepare_build_both(res->level); - else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) - i2c_mechboard_mode_prepare_inside_both(res->level); - else if (!strcmp_P(res->arg1, PSTR("push_temple"))) - i2c_mechboard_mode_push_temple(res->level); -} - -prog_char str_mechboard_setmode3_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0); -prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple"; -parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1); -parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8); - -prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)"; -parse_pgm_inst_t cmd_mechboard_setmode3 = { - .f = cmd_mechboard_setmode3_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode3, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode3_arg0, - (prog_void *)&cmd_mechboard_setmode3_arg1, - (prog_void *)&cmd_mechboard_setmode3_arg2, - NULL, - }, -}; - -/**********************************************************/ -/* Mechboard_Setmode4 */ - -/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */ -struct cmd_mechboard_setmode4_result { - fixed_string_t arg0; - fixed_string_t arg1; - uint8_t level_l; - uint8_t count_l; - uint8_t dist_l; - uint8_t level_r; - uint8_t count_r; - uint8_t dist_r; - uint8_t do_lintel; -}; - -/* function called when cmd_mechboard_setmode4 is parsed successfully */ -static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data) -{ - struct cmd_mechboard_setmode4_result *res = parsed_result; - i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l, - res->level_r, res->count_r, res->dist_r, - res->do_lintel); -} - -prog_char str_mechboard_setmode4_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0); -prog_char str_mechboard_setmode4_arg1[] = "autobuild"; -parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8); - -prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; -parse_pgm_inst_t cmd_mechboard_setmode4 = { - .f = cmd_mechboard_setmode4_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode4, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode4_arg0, - (prog_void *)&cmd_mechboard_setmode4_arg1, - (prog_void *)&cmd_mechboard_setmode4_arg2, - (prog_void *)&cmd_mechboard_setmode4_arg3, - (prog_void *)&cmd_mechboard_setmode4_arg4, - (prog_void *)&cmd_mechboard_setmode4_arg5, - (prog_void *)&cmd_mechboard_setmode4_arg6, - (prog_void *)&cmd_mechboard_setmode4_arg7, - (prog_void *)&cmd_mechboard_setmode4_arg8, - NULL, - }, -}; - -/**********************************************************/ -/* Mechboard_Setmode5 */ - -/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */ -struct cmd_mechboard_setmode5_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_mechboard_setmode5 is parsed successfully */ -static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data) -{ - struct cmd_mechboard_setmode5_result *res = parsed_result; - uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - - if (!strcmp_P(res->arg2, PSTR("left"))) - side = I2C_LEFT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("right"))) - side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else if (!strcmp_P(res->arg2, PSTR("auto"))) - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg3, PSTR("harvest"))) - next_mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) - next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("pickup"))) - next_mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg3, PSTR("clear"))) - next_mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg3, PSTR("store"))) - next_mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) - next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) - i2c_mechboard_mode_prepare_pickup_next(side, next_mode); -} - -prog_char str_mechboard_setmode5_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0); -prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1); -prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2); -prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3); - -prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)"; -parse_pgm_inst_t cmd_mechboard_setmode5 = { - .f = cmd_mechboard_setmode5_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode5, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode5_arg0, - (prog_void *)&cmd_mechboard_setmode5_arg1, - (prog_void *)&cmd_mechboard_setmode5_arg2, - (prog_void *)&cmd_mechboard_setmode5_arg3, - NULL, - }, -}; - -/**********************************************************/ -/* pickup wheels */ - -/* this structure is filled when cmd_pickup_wheels is parsed successfully */ -struct cmd_pickup_wheels_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_pickup_wheels is parsed successfully */ -static void cmd_pickup_wheels_parsed(void *parsed_result, void *data) -{ - struct cmd_pickup_wheels_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("on"))) - pickup_wheels_on(); - else - pickup_wheels_off(); -} - -prog_char str_pickup_wheels_arg0[] = "pickup_wheels"; -parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0); -prog_char str_pickup_wheels_arg1[] = "on#off"; -parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1); - -prog_char help_pickup_wheels[] = "Enable/disable pickup wheels"; -parse_pgm_inst_t cmd_pickup_wheels = { - .f = cmd_pickup_wheels_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pickup_wheels, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pickup_wheels_arg0, - (prog_void *)&cmd_pickup_wheels_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Beacon_Start */ - -/* this structure is filled when cmd_beacon_start is parsed successfully */ -struct cmd_beacon_start_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_beacon_start is parsed successfully */ -static void cmd_beacon_start_parsed(void *parsed_result, void *data) -{ - struct cmd_beacon_start_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("start"))) - i2c_sensorboard_set_beacon(1); - else - i2c_sensorboard_set_beacon(0); -} - -prog_char str_beacon_start_arg0[] = "beacon"; -parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0); -prog_char str_beacon_start_arg1[] = "start#stop"; -parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1); - -prog_char help_beacon_start[] = "Beacon enabled/disable"; -parse_pgm_inst_t cmd_beacon_start = { - .f = cmd_beacon_start_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_beacon_start, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_beacon_start_arg0, - (prog_void *)&cmd_beacon_start_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Pump_Current */ - -/* this structure is filled when cmd_pump_current is parsed successfully */ -struct cmd_pump_current_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_pump_current is parsed successfully */ -static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), - sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4), - mechboard.pump_right1_current, mechboard.pump_right2_current); -} - -prog_char str_pump_current_arg0[] = "pump_current"; -parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); -prog_char str_pump_current_arg1[] = "show"; -parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); - -prog_char help_pump_current[] = "dump pump current"; -parse_pgm_inst_t cmd_pump_current = { - .f = cmd_pump_current_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump_current, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_current_arg0, - (prog_void *)&cmd_pump_current_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Build_Test */ - -/* this structure is filled when cmd_build_test is parsed successfully */ -struct cmd_build_test_result { - fixed_string_t arg0; -}; - -/* function called when cmd_build_test is parsed successfully */ -static void cmd_build_test_parsed(void *parsed_result, void *data) -{ - //struct cmd_build_test_result *res = parsed_result; - - printf_P(PSTR("lintel must be there\r\n")); - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - wait_ms(500); - - printf_P(PSTR("Insert 4 colums\r\n")); - while (get_column_count() != 4); - - i2c_mechboard_mode_prepare_build_both(0); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(500); - - i2c_mechboard_mode_simple_autobuild(0); - wait_ms(100); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - - while (get_column_count() != 3); - - i2c_mechboard_mode_prepare_build_both(3); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(500); - - i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST, - 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0); - i2cproto_wait_update(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE); - wait_ms(500); - - i2c_mechboard_mode_harvest(); - while (get_column_count() != 3); - - i2c_mechboard_mode_prepare_build_both(5); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(1000); - - i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST, - 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0); - i2cproto_wait_update(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); -} - -prog_char str_build_test_arg0[] = "build_test"; -parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0); - -prog_char help_build_test[] = "Build_Test function"; -parse_pgm_inst_t cmd_build_test = { - .f = cmd_build_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_build_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_build_test_arg0, - NULL, - }, -}; - - -/**********************************************************/ -/* Column_Test */ - -/* this structure is filled when cmd_column_test is parsed successfully */ -struct cmd_column_test_result { - fixed_string_t arg0; - uint8_t level; - int16_t dist; - int8_t a1; - int8_t a2; - int8_t a3; - int16_t arm_dist; - int8_t nb_col; -}; - -/* function called when cmd_column_test is parsed successfully */ -static void cmd_column_test_parsed(void *parsed_result, void *data) -{ - struct cmd_column_test_result *res = parsed_result; - uint8_t level = res->level, debug = 0; - uint8_t c, push = 0; - - /* default conf */ - if (data) { - res->dist = 70; - res->a1 = -20; - res->a2 = 40; - res->a3 = -20; - res->arm_dist = 220; - res->nb_col = 2; - } - - if (!strcmp_P(res->arg0, PSTR("column_test_debug"))) - debug = 1; - if (!strcmp_P(res->arg0, PSTR("column_test_push"))) - push = 1; - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - - /* Go to disc */ - - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - /* go back, insert colums */ - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - printf_P(PSTR("Insert 4 colums\r\n")); - while (get_column_count() != 4); - - /* build with left arm */ - - i2c_mechboard_mode_prepare_inside_both(level); - trajectory_d_rel(&mainboard.traj, 200-(res->dist)); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - - trajectory_a_rel(&mainboard.traj, res->a1); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - - i2c_mechboard_mode_prepare_build_select(level, -1); - time_wait_ms(200); - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist, - 0, 0, res->arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1); - - if (debug) - c = cmdline_getchar_wait(); - /* build with right arm */ - - trajectory_a_rel(&mainboard.traj, res->a2); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - /* only ok for nb_col == 2 */ - if ((level + res->nb_col) >= 7) - i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1); - else - i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col); - time_wait_ms(200); - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_autobuild(0, 0, res->arm_dist, - level + res->nb_col, res->nb_col, - res->arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - - if (push) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); - time_wait_ms(500); - trajectory_d_rel(&mainboard.traj, 100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - else if (level == 1 || level == 0) { - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(level); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2); - - if (debug) - c = cmdline_getchar_wait(); - - trajectory_a_rel(&mainboard.traj, res->a3); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - /* go back, insert colums */ - - trajectory_d_rel(&mainboard.traj, -100); - - return; -} - -prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push"; -parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0); -parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8); - -prog_char help_column_test[] = "Column_Test function (level)"; -parse_pgm_inst_t cmd_column_test = { - .f = cmd_column_test_parsed, /* function to call */ - .data = (void *)1, /* 2nd arg of func */ - .help_str = help_column_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_column_test_arg0, - (prog_void *)&cmd_column_test_arg1, - NULL, - }, -}; - -parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16); -parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8); -parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8); -parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8); -parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16); -parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8); - -prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)"; -parse_pgm_inst_t cmd_column_test2 = { - .f = cmd_column_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_column_test2, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_column_test_arg0, - (prog_void *)&cmd_column_test_arg1, - (prog_void *)&cmd_column_test_arg2, - (prog_void *)&cmd_column_test_arg3, - (prog_void *)&cmd_column_test_arg4, - (prog_void *)&cmd_column_test_arg5, - (prog_void *)&cmd_column_test_arg6, - (prog_void *)&cmd_column_test_arg7, - NULL, - }, -}; - - -/**********************************************************/ -/* Pickup_Test */ - -/* this structure is filled when cmd_pickup_test is parsed successfully */ -struct cmd_pickup_test_result { - fixed_string_t arg0; - fixed_string_t arg1; - int16_t dist; -}; - -/* return red or green sensor */ -#define COLOR_IR_SENSOR() \ - ({ \ - uint8_t __ret = 0; \ - if (side == I2C_RIGHT_SIDE) \ - __ret = sensor_get(S_DISP_RIGHT); \ - else \ - __ret = sensor_get(S_DISP_LEFT); \ - __ret; \ - }) \ -/* column dispensers */ -#define COL_SCAN_MARGIN 200 -/* distance between the wheel axis and the IR sensor */ - -/* function called when cmd_pickup_test is parsed successfully */ -static void cmd_pickup_test_parsed(void *parsed_result, void *data) -{ - uint8_t err, side, first_try = 1; - int8_t cols_count_before, cols_count_after, cols; - struct cmd_pickup_test_result *res = parsed_result; - int16_t pos1, pos2, pos; - microseconds us; - int16_t dist = res->dist; - uint8_t timeout = 0; - - if (!strcmp_P(res->arg1, PSTR("left"))) - side = I2C_LEFT_SIDE; - else - side = I2C_RIGHT_SIDE; - - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - cols_count_before = get_column_count(); - position_set(&mainboard.pos, 0, 0, 0); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -1000); - err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); - if (err) /* we should not reach end */ - goto fail; - pos1 = position_get_x_s16(&mainboard.pos); - printf_P(PSTR("pos1 = %d\r\n"), pos1); - - err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); - if (err) - goto fail; - pos2 = position_get_x_s16(&mainboard.pos); - printf_P(PSTR("pos2 = %d\r\n"), pos2); - - pos = ABS(pos1 - pos2); - printf_P(PSTR("pos = %d\r\n"), pos); - - trajectory_d_rel(&mainboard.traj, -dist + pos/2); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - if (side == I2C_LEFT_SIDE) - trajectory_a_rel(&mainboard.traj, 90); - else - trajectory_a_rel(&mainboard.traj, -90); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - pickup_wheels_on(); - retry: - if (first_try) - i2c_mechboard_mode_lazy_harvest(); - else - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - first_try = 0; - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); - - trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - goto fail; - - position_set(&mainboard.pos, 0, 0, 0); - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(90, 0); - goto retry; - } - - /* start to pickup with finger / arms */ - - printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__); - i2c_mechboard_mode_pickup(); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == - I2C_MECHBOARD_MODE_PICKUP, 100); - us = time_get_us2(); - cols = get_column_count(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) { - if (get_column_count() != cols) { - cols = get_column_count(); - us = time_get_us2(); - } - if ((get_column_count() - cols_count_before) >= 4) { - printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__); - break; - } - /* 1 second timeout */ - if (time_get_us2() - us > 1500000L) { - printf_P(PSTR("%s timeout\r\n"), __FUNCTION__); - timeout = 1; - break; - } - } - - /* eject if we found a bad color column */ - - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(90, 0); - goto retry; - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -250); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - - cols_count_after = get_column_count(); - cols = cols_count_after - cols_count_before; - DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); - pickup_wheels_off(); - i2c_mechboard_mode_clear(); - - wait_ms(1000); - return; - fail: - printf_P(PSTR("failed\r\n")); - strat_hardstop(); + if (!strcmp_P(res->arg1, PSTR("yyy"))) + printf("faux\r\n"); + else if (!strcmp_P(res->arg1, PSTR("xxx"))) + printf("faux\r\n"); } -prog_char str_pickup_test_arg0[] = "pickup_test"; -parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0); -prog_char str_pickup_test_arg1[] = "left#right"; -parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1); -parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16); +prog_char str_cobboard_setmode2_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0); +prog_char str_cobboard_setmode2_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1); +prog_char str_cobboard_setmode2_arg2[] = "left#right"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2); -prog_char help_pickup_test[] = "Pickup_Test function"; -parse_pgm_inst_t cmd_pickup_test = { - .f = cmd_pickup_test_parsed, /* function to call */ +prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)"; +parse_pgm_inst_t cmd_cobboard_setmode2 = { + .f = cmd_cobboard_setmode2_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_pickup_test, + .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pickup_test_arg0, - (prog_void *)&cmd_pickup_test_arg1, - (prog_void *)&cmd_pickup_test_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; /**********************************************************/ -/* Lintel_Test */ +/* Cobboard_Setmode3 */ -/* this structure is filled when cmd_lintel_test is parsed successfully */ -struct cmd_lintel_test_result { - fixed_string_t arg0; -}; - -/* function called when cmd_lintel_test is parsed successfully */ -static void cmd_lintel_test_parsed(void *parsed_result, void *data) -{ - uint8_t err, first_try = 1, right_ok, left_ok; - int16_t left_cur, right_cur; - - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(500); - retry: - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 500); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err) && err != END_BLOCKING) - goto fail; - - i2c_mechboard_mode_get_lintel(); - time_wait_ms(500); - - left_cur = sensor_get_adc(ADC_CSENSE3); - left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); - right_cur = mechboard.pump_right1_current; - right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); - - printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"), - left_ok, left_cur, right_ok, right_cur); - if (first_try) { - if (!right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - else if (right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -150, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, -140, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - else if (!right_ok && left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -150, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, -140, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - /* else, lintel is ok */ - else { - i2c_mechboard_mode_put_lintel(); - } - } - else { - if (right_ok && left_ok) { - /* lintel is ok */ - i2c_mechboard_mode_put_lintel(); - } - else { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -250); - err = wait_traj_end(TRAJ_FLAGS_STD); - return; - -fail: - printf_P(PSTR("fail\r\n")); - return; -} - -prog_char str_lintel_test_arg0[] = "lintel_test"; -parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0); - -prog_char help_lintel_test[] = "Lintel_Test function"; -parse_pgm_inst_t cmd_lintel_test = { - .f = cmd_lintel_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_lintel_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_lintel_test_arg0, - NULL, - }, -}; - -/**********************************************************/ -/* Scan_Test */ - -/* this structure is filled when cmd_scan_test is parsed successfully */ -struct cmd_scan_test_result { +/* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */ +struct cmd_cobboard_setmode3_result { fixed_string_t arg0; fixed_string_t arg1; - int16_t start_dist; - int16_t scan_dist; - int16_t scan_speed; - int16_t center_x; - int16_t center_y; uint8_t level; }; -#define SCAN_MODE_CHECK_TEMPLE 0 -#define SCAN_MODE_SCAN_COL 1 -#define SCAN_MODE_SCAN_TEMPLE 2 -#define SCAN_MODE_TRAJ_ONLY 3 - -/* function called when cmd_scan_test is parsed successfully */ -static void cmd_scan_test_parsed(void *parsed_result, void *data) +/* function called when cmd_cobboard_setmode3 is parsed successfully */ +static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { - uint8_t err, mode=0, c; - int16_t pos1x, pos1y, dist; - struct cmd_scan_test_result *res = parsed_result; - int16_t back_mm = 0; - - int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; - - double center_abs_x, center_abs_y; - double ckpt_rel_d, ckpt_rel_a; - double ckpt_abs_x, ckpt_abs_y; - - if (!strcmp_P(res->arg1, PSTR("traj_only"))) - mode = SCAN_MODE_TRAJ_ONLY; - else if (!strcmp_P(res->arg1, PSTR("check_temple"))) - mode = SCAN_MODE_CHECK_TEMPLE; - else if (!strcmp_P(res->arg1, PSTR("scan_col"))) - mode = SCAN_MODE_SCAN_COL; - else if (!strcmp_P(res->arg1, PSTR("scan_temple"))) - mode = SCAN_MODE_SCAN_TEMPLE; - - /* go to disc */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err != END_BLOCKING) - return; - - /* save absolute position of disc */ - rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); - - /* go back and prepare to scan */ - strat_set_speed(1000, 1000); - trajectory_d_a_rel(&mainboard.traj, -140, 130); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - return; - - /* prepare scanner arm */ - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_prepare(); - time_wait_ms(250); - - strat_set_speed(res->scan_speed, 1000); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - trajectory_d_rel(&mainboard.traj, -res->scan_dist); - - while (1) { - err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err != 0) - break; - - dist = distance_from_robot(pos1x, pos1y); - - if (dist > res->start_dist) - break; - - if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { - err = END_ERROR; - break; - } - } - - if (err) { - if (TRAJ_SUCCESS(err)) - err = END_ERROR; /* should not reach end */ - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_stop(); - return; - } - - /* start the scanner */ - - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_start(); - - err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, - TRAJ_FLAGS_NO_NEAR); - if (err == 0) - err = END_ERROR; - if (!TRAJ_SUCCESS(err)) { - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_stop(); - return; - } - - if (mode == SCAN_MODE_TRAJ_ONLY) - return; - - wait_scan_done(10000); - - i2c_sensorboard_scanner_stop(); - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - i2c_sensorboard_scanner_algo_check(res->level, - res->center_x, res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1) { - printf_P(PSTR("-- try to build a temple\r\n")); - res->center_x = 15; - res->center_y = 13; - mode = SCAN_MODE_SCAN_TEMPLE; - } - } - - if (mode == SCAN_MODE_SCAN_TEMPLE) { - i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, - res->center_x, - res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - printf_P(PSTR("-- try to build a column\r\n")); - mode = SCAN_MODE_SCAN_COL; - } - } - - if (mode == SCAN_MODE_SCAN_COL) { - i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, - res->center_x, res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - return; - } - } - - if (sensorboard.dropzone_h == -1) - return; - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - ckpt_rel_x = 220; - ckpt_rel_y = 100; - } - - - printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y); - - rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); - abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); - - printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y); - - printf_P(PSTR("ok ? (y/n)\r\n")); - - c = cmdline_getchar_wait(); - - if (c != 'y') - return; - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - /* intermediate checkpoint for some positions */ - if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { - trajectory_goto_xy_rel(&mainboard.traj, 200, 100); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - } - - trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - - trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - - c = cmdline_getchar_wait(); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 200); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, - TRAJ_FLAGS_SMALL_DIST); - if (err == 0) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - if (err != END_BLOCKING) - return; - - if (back_mm) { - trajectory_d_rel(&mainboard.traj, -back_mm); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } -} - -prog_char str_scan_test_arg0[] = "scan_test"; -parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0); -prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple"; -parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1); -parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16); -parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16); -parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16); -parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16); -parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16); - -prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)"; -parse_pgm_inst_t cmd_scan_test = { - .f = cmd_scan_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_test_arg0, - (prog_void *)&cmd_scan_test_arg1, - (prog_void *)&cmd_scan_test_arg2, - (prog_void *)&cmd_scan_test_arg3, - (prog_void *)&cmd_scan_test_arg4, - (prog_void *)&cmd_scan_test_arg5, - (prog_void *)&cmd_scan_test_arg6, - NULL, - }, -}; - -prog_char str_scan_test_arg1b[] = "check_temple"; -parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b); -parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8); - -prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)"; -parse_pgm_inst_t cmd_scan_test2 = { - .f = cmd_scan_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_test_arg0, - (prog_void *)&cmd_scan_test_arg1b, - (prog_void *)&cmd_scan_test_arg2, - (prog_void *)&cmd_scan_test_arg3, - (prog_void *)&cmd_scan_test_arg4, - (prog_void *)&cmd_scan_test_arg5, - (prog_void *)&cmd_scan_test_arg6, - (prog_void *)&cmd_scan_test_arg7, - NULL, - }, -}; - -/**********************************************************/ -/* Time_Monitor */ - -/* this structure is filled when cmd_time_monitor is parsed successfully */ -struct cmd_time_monitor_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_time_monitor is parsed successfully */ -static void cmd_time_monitor_parsed(void *parsed_result, void *data) -{ - struct cmd_time_monitor_result *res = parsed_result; - uint16_t seconds; - - if (!strcmp_P(res->arg1, PSTR("reset"))) { - eeprom_write_word(EEPROM_TIME_ADDRESS, 0); - } - seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); - printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); -} - -prog_char str_time_monitor_arg0[] = "time_monitor"; -parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); -prog_char str_time_monitor_arg1[] = "show#reset"; -parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); - -prog_char help_time_monitor[] = "Show since how long we are running"; -parse_pgm_inst_t cmd_time_monitor = { - .f = cmd_time_monitor_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_time_monitor, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_time_monitor_arg0, - (prog_void *)&cmd_time_monitor_arg1, - NULL, - }, -}; - - -/**********************************************************/ -/* Scanner */ - -/* this structure is filled when cmd_scanner is parsed successfully */ -struct cmd_scanner_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_scanner is parsed successfully */ -static void cmd_scanner_parsed(void *parsed_result, void *data) -{ - struct cmd_scanner_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("prepare"))) { - i2c_sensorboard_scanner_prepare(); - } - else if (!strcmp_P(res->arg1, PSTR("stop"))) { - i2c_sensorboard_scanner_stop(); - } - else if (!strcmp_P(res->arg1, PSTR("start"))) { - i2c_sensorboard_scanner_start(); - } - else if (!strcmp_P(res->arg1, PSTR("algo_col"))) { - i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, - 15, 15); - } - else if (!strcmp_P(res->arg1, PSTR("algo_check"))) { - i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX - } - else if (!strcmp_P(res->arg1, PSTR("calib"))) { - i2c_sensorboard_scanner_calib(); - } - else if (!strcmp_P(res->arg1, PSTR("show"))) { - scanner_dump_state(); - } + struct cmd_cobboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("xxx"))) + printf("faux\r\n"); } -prog_char str_scanner_arg0[] = "scanner"; -parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0); -prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib"; -parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1); +prog_char str_cobboard_setmode3_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0); +prog_char str_cobboard_setmode3_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1); +parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8); -prog_char help_scanner[] = "send commands to scanner"; -parse_pgm_inst_t cmd_scanner = { - .f = cmd_scanner_parsed, /* function to call */ +prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)"; +parse_pgm_inst_t cmd_cobboard_setmode3 = { + .f = cmd_cobboard_setmode3_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_scanner, + .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scanner_arg0, - (prog_void *)&cmd_scanner_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; -/**********************************************************/ -/* Build_Z1 */ - -/* this structure is filled when cmd_build_z1 is parsed successfully */ -struct cmd_build_z1_result { - fixed_string_t arg0; - uint8_t level; - int16_t d1; - int16_t d2; - int16_t d3; -}; - -/* function called when cmd_build_z1 is parsed successfully */ -static void cmd_build_z1_parsed(void *parsed_result, void *data) -{ - struct cmd_build_z1_result *res = parsed_result; - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - - while (get_column_count() != 4); - - i2c_mechboard_mode_prepare_build_both(res->level); - time_wait_ms(500); - - trajectory_d_rel(&mainboard.traj, 400); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -res->d1); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, - res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, - 1); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == - I2C_MECHBOARD_MODE_AUTOBUILD, 100); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != - I2C_MECHBOARD_MODE_AUTOBUILD, 10000); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -res->d2); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(1); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, res->d3); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); -} - -prog_char str_build_z1_arg0[] = "build_z1"; -parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0); -parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8); -parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16); -parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16); -parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16); - -prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)"; -parse_pgm_inst_t cmd_build_z1 = { - .f = cmd_build_z1_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_build_z1, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_build_z1_arg0, - (prog_void *)&cmd_build_z1_arg1, - (prog_void *)&cmd_build_z1_arg2, - (prog_void *)&cmd_build_z1_arg3, - (prog_void *)&cmd_build_z1_arg4, - NULL, - }, -}; - -#ifdef TEST_BEACON -/**********************************************************/ -/* Beacon_Opp_Dump */ - -/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */ -struct cmd_beacon_opp_dump_result { - fixed_string_t arg0; -}; - -void beacon_dump_samples(void); - -/* function called when cmd_beacon_opp_dump is parsed successfully */ -static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data) -{ - beacon_dump_samples(); -} - -prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump"; -parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0); - -prog_char help_beacon_opp_dump[] = "Dump beacon samples"; -parse_pgm_inst_t cmd_beacon_opp_dump = { - .f = cmd_beacon_opp_dump_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_beacon_opp_dump, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_beacon_opp_dump_arg0, - NULL, - }, -}; -#endif - /**********************************************************/ /* Test */ @@ -2123,18 +812,10 @@ struct cmd_test_result { fixed_string_t arg0; int32_t radius; }; -void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a); + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - struct cmd_test_result *res = parsed_result; - double d,a; - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); - printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index ea124d3..0d83688 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -56,7 +56,6 @@ #include "cmdline.h" #include "strat_utils.h" #include "strat_base.h" -#include "strat_avoid.h" #include "strat.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" @@ -273,11 +272,8 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data) rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, RIGHT_ENCODER, res->right); //en augmentant on tourne à droite } - printf_P(PSTR("rs_gains set ")); - f64_print(mainboard.rs.left_ext_gain); - printf_P(PSTR(" ")); - f64_print(mainboard.rs.right_ext_gain); - printf_P(PSTR("\r\n")); + printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"), + mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain); } prog_char str_rs_gains_arg0[] = "rs_gains"; @@ -399,6 +395,11 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) struct cmd_pt_list_result * res = parsed_result; uint8_t i, why=0; + if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + printf_P(PSTR("not implemented\r\n")); + return; + } + if (!strcmp_P(res->arg1, PSTR("append"))) { res->arg2 = pt_list_len; } @@ -447,6 +448,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); why = wait_traj_end(0xFF); /* all */ } +#if 0 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { while (1) { why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); @@ -455,6 +457,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) break; } } +#endif if (why & (~(END_TRAJ | END_NEAR))) trajectory_stop(&mainboard.traj); } @@ -580,14 +583,24 @@ static void cmd_goto_parsed(void * parsed_result, void * data) trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3); } else if (!strcmp_P(res->arg1, PSTR("avoid"))) { +#if 0 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF); if (err != END_TRAJ && err != END_NEAR) strat_hardstop(); +#else + printf_P(PSTR("not implemented\r\n")); + return; +#endif } else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) { +#if 0 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF); if (err != END_TRAJ && err != END_NEAR) strat_hardstop(); +#else + printf_P(PSTR("not implemented\r\n")); + return; +#endif } else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) { trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3); @@ -732,14 +745,14 @@ static void cmd_position_parsed(void * parsed_result, void * data) } else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) { mainboard.our_color = I2C_COLOR_GREEN; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN); auto_position(); } else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { mainboard.our_color = I2C_COLOR_RED; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED); auto_position(); } @@ -840,74 +853,15 @@ struct cmd_strat_conf_result { /* function called when cmd_strat_conf is parsed successfully */ static void cmd_strat_conf_parsed(void *parsed_result, void *data) { - struct cmd_strat_conf_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("base"))) { - strat_infos.conf.flags = 0; - strat_infos.conf.scan_our_min_time = 90; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 90; - strat_infos.conf.delay_between_opp_scan = 90; - } - else if (!strcmp_P(res->arg1, PSTR("big3"))) { - strat_infos.conf.flags = - STRAT_CONF_STORE_STATIC2 | - STRAT_CONF_BIG_3_TEMPLE; - strat_infos.conf.scan_our_min_time = 90; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 90; - strat_infos.conf.delay_between_opp_scan = 90; - } - else if (!strcmp_P(res->arg1, PSTR("base_check"))) { - strat_infos.conf.flags = 0; - strat_infos.conf.scan_our_min_time = 35; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 90; - strat_infos.conf.delay_between_opp_scan = 90; - } - else if (!strcmp_P(res->arg1, PSTR("big3_check"))) { - strat_infos.conf.flags = - STRAT_CONF_STORE_STATIC2 | - STRAT_CONF_BIG_3_TEMPLE; - strat_infos.conf.scan_our_min_time = 35; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 90; - strat_infos.conf.delay_between_opp_scan = 90; - } - else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) { - strat_infos.conf.flags = - STRAT_CONF_TAKE_ONE_LINTEL | - STRAT_CONF_STORE_STATIC2 | - STRAT_CONF_EARLY_SCAN | - STRAT_CONF_PUSH_OPP_COLS; - strat_infos.conf.scan_our_min_time = 50; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 15; - strat_infos.conf.delay_between_opp_scan = 90; - strat_infos.conf.wait_opponent = 5; - } - else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) { - strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL; - strat_infos.conf.scan_our_min_time = 90; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 30; - strat_infos.conf.delay_between_opp_scan = 90; - } - else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) { - strat_infos.conf.flags = - STRAT_CONF_ONLY_ONE_ON_DISC; - strat_infos.conf.scan_our_min_time = 90; - strat_infos.conf.delay_between_our_scan = 90; - strat_infos.conf.scan_opp_min_time = 90; - strat_infos.conf.delay_between_opp_scan = 90; - } + // struct cmd_strat_conf_result *res = parsed_result; + strat_infos.dump_enabled = 1; strat_dump_conf(); } prog_char str_strat_conf_arg0[] = "strat_conf"; parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0); -prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc"; +prog_char str_strat_conf_arg1[] = "show#base"; parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1); prog_char help_strat_conf[] = "configure strat options"; @@ -943,6 +897,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) else on = 0; +#if 0 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) bit = STRAT_CONF_ONLY_ONE_ON_DISC; else if (!strcmp_P(res->arg1, PSTR("bypass_static2"))) @@ -959,6 +914,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) bit = STRAT_CONF_EARLY_SCAN; else if (!strcmp_P(res->arg1, PSTR("push_opp_cols"))) bit = STRAT_CONF_PUSH_OPP_COLS; +#endif if (on) strat_infos.conf.flags |= bit; @@ -971,7 +927,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) prog_char str_strat_conf2_arg0[] = "strat_conf"; parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0); -prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan"; +prog_char str_strat_conf2_arg1[] = "faux"; parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1); prog_char str_strat_conf2_arg2[] = "on#off"; parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2); @@ -1003,6 +959,7 @@ struct cmd_strat_conf3_result { /* function called when cmd_strat_conf3 is parsed successfully */ static void cmd_strat_conf3_parsed(void *parsed_result, void *data) { +#if 0 struct cmd_strat_conf3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { @@ -1031,13 +988,14 @@ static void cmd_strat_conf3_parsed(void *parsed_result, void *data) else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { strat_infos.conf.lintel_min_time = res->arg2; } +#endif strat_infos.dump_enabled = 1; strat_dump_conf(); } prog_char str_strat_conf3_arg0[] = "strat_conf"; parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0); -prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent"; +prog_char str_strat_conf3_arg1[] = "faux2"; parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1); parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16); @@ -1054,47 +1012,6 @@ parse_pgm_inst_t cmd_strat_conf3 = { }, }; -/**********************************************************/ -/* strat configuration */ - -/* this structure is filled when cmd_strat_conf4 is parsed successfully */ -struct cmd_strat_conf4_result { - fixed_string_t arg0; - fixed_string_t arg1; - int16_t arg2; -}; - -/* function called when cmd_strat_conf4 is parsed successfully */ -static void cmd_strat_conf4_parsed(void *parsed_result, void *data) -{ - struct cmd_strat_conf4_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) { - strat_infos.conf.scan_opp_angle = res->arg2; - } - strat_infos.dump_enabled = 1; - strat_dump_conf(); -} - -prog_char str_strat_conf4_arg0[] = "strat_conf"; -parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0); -prog_char str_strat_conf4_arg1[] = "scan_opponent_angle"; -parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1); -parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16); - -prog_char help_strat_conf4[] = "configure strat options"; -parse_pgm_inst_t cmd_strat_conf4 = { - .f = cmd_strat_conf4_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_strat_conf4, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf4_arg0, - (prog_void *)&cmd_strat_conf4_arg1, - (prog_void *)&cmd_strat_conf4_arg2, - NULL, - }, -}; - /**********************************************************/ /* Subtraj */ @@ -1112,76 +1029,15 @@ struct cmd_subtraj_result { /* function called when cmd_subtraj is parsed successfully */ static void cmd_subtraj_parsed(void *parsed_result, void *data) { - struct cmd_subtraj_result *res = parsed_result; - uint8_t err = 0; - struct column_dispenser *disp; - - if (strcmp_P(res->arg1, PSTR("static")) == 0) { - err = strat_static_columns(res->arg2); - } - else if (strcmp_P(res->arg1, PSTR("static2")) == 0) { - strat_infos.s_cols.configuration = res->arg2; - switch (res->arg2) { - case 1: - position_set(&mainboard.pos, 1398, - COLOR_Y(1297), COLOR_A(-66)); - break; - case 2: - position_set(&mainboard.pos, 1232, - COLOR_Y(1051), COLOR_A(4)); - break; - case 3: - position_set(&mainboard.pos, 1232, - COLOR_Y(1043), COLOR_A(5)); - break; - case 4: - position_set(&mainboard.pos, 1346, - COLOR_Y(852), COLOR_A(57)); - break; - default: - return; - } - if (res->arg2 == 1 && res->arg3 == 1) { - strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE; - } - if (res->arg2 == 1 && res->arg3 == 2) { - strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE; - } - err = strat_static_columns_pass2(); - } - else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) { - err = strat_goto_lintel_disp(&strat_infos.l1); - } - else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) { - err = strat_goto_lintel_disp(&strat_infos.l2); - } - else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) { - disp = &strat_infos.c1; - err = strat_goto_col_disp(&disp); - } - else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) { - disp = &strat_infos.c2; - err = strat_goto_col_disp(&disp); - } - else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) { - disp = &strat_infos.c3; - err = strat_goto_col_disp(&disp); - } - else if (strcmp_P(res->arg1, PSTR("disc")) == 0) { - if (res->arg2 == 0) { - printf_P(PSTR("bad level\r\n")); - return; - } - err = strat_goto_disc(res->arg2); - } +/* struct cmd_subtraj_result *res = parsed_result; */ - printf_P(PSTR("substrat returned %s\r\n"), get_err(err)); + printf_P(PSTR("TODO\r\n")); trajectory_hardstop(&mainboard.traj); } prog_char str_subtraj_arg0[] = "subtraj"; parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0); -prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2"; +prog_char str_subtraj_arg1[] = "faux"; parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1); parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32); parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32); diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index ec70f9a..8fe9718 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include #include @@ -54,6 +54,26 @@ #include "strat.h" #include "actuator.h" +int32_t encoders_left_cobroller_speed(void *number) +{ + static volatile int32_t roller_pos; + int32_t tmp, speed; + tmp = encoders_spi_get_value(number); + speed = tmp - roller_pos; + roller_pos = tmp; + return speed; +} + +int32_t encoders_right_cobroller_speed(void *number) +{ + static volatile int32_t roller_pos; + int32_t tmp, speed; + tmp = encoders_spi_get_value(number); + speed = tmp - roller_pos; + roller_pos = tmp; + return speed; +} + /* called every 5 ms */ static void do_cs(void *dummy) { @@ -87,6 +107,10 @@ static void do_cs(void *dummy) cs_manage(&mainboard.angle.cs); if (mainboard.distance.on) cs_manage(&mainboard.distance.cs); + if (mainboard.left_cobroller.on) + cs_manage(&mainboard.left_cobroller.cs); + if (mainboard.right_cobroller.on) + cs_manage(&mainboard.right_cobroller.cs); } if ((cpt & 1) && (mainboard.flags & DO_POS)) { /* about 1.5ms (worst case without centrifugal force @@ -96,6 +120,8 @@ static void do_cs(void *dummy) if (mainboard.flags & DO_BD) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); + bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs); + bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs); } if (mainboard.flags & DO_TIMER) { uint8_t second; @@ -228,9 +254,63 @@ void microb_cs_init(void) bd_set_speed_threshold(&mainboard.distance.bd, 60); bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + /* ---- CS left_cobroller */ + /* PID */ + pid_init(&mainboard.left_cobroller.pid); + pid_set_gains(&mainboard.left_cobroller.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.left_cobroller.pid, 0, 2000, 4095); + pid_set_out_shift(&mainboard.left_cobroller.pid, 10); + pid_set_derivate_filter(&mainboard.left_cobroller.pid, 6); + + /* QUADRAMP */ + quadramp_init(&mainboard.left_cobroller.qr); + quadramp_set_1st_order_vars(&mainboard.left_cobroller.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.left_cobroller.qr, 17, 17); /* set accel */ + + /* CS */ + cs_init(&mainboard.left_cobroller.cs); + cs_set_consign_filter(&mainboard.left_cobroller.cs, quadramp_do_filter, &mainboard.left_cobroller.qr); + cs_set_correct_filter(&mainboard.left_cobroller.cs, pid_do_filter, &mainboard.left_cobroller.pid); + cs_set_process_in(&mainboard.left_cobroller.cs, pwm_ng_set, LEFT_COBROLLER_PWM); + cs_set_process_out(&mainboard.left_cobroller.cs, encoders_left_cobroller_speed, LEFT_COBROLLER_ENCODER); + cs_set_consign(&mainboard.left_cobroller.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.left_cobroller.bd); + bd_set_speed_threshold(&mainboard.left_cobroller.bd, 60); + bd_set_current_thresholds(&mainboard.left_cobroller.bd, 500, 8000, 1000000, 50); + + /* ---- CS right_cobroller */ + /* PID */ + pid_init(&mainboard.right_cobroller.pid); + pid_set_gains(&mainboard.right_cobroller.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.right_cobroller.pid, 0, 2000, 4095); + pid_set_out_shift(&mainboard.right_cobroller.pid, 10); + pid_set_derivate_filter(&mainboard.right_cobroller.pid, 6); + + /* QUADRAMP */ + quadramp_init(&mainboard.right_cobroller.qr); + quadramp_set_1st_order_vars(&mainboard.right_cobroller.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.right_cobroller.qr, 17, 17); /* set accel */ + + /* CS */ + cs_init(&mainboard.right_cobroller.cs); + cs_set_consign_filter(&mainboard.right_cobroller.cs, quadramp_do_filter, &mainboard.right_cobroller.qr); + cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid); + cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM); + cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER); + cs_set_consign(&mainboard.right_cobroller.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.right_cobroller.bd); + bd_set_speed_threshold(&mainboard.right_cobroller.bd, 60); + bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50); + /* set them on !! */ mainboard.angle.on = 1; mainboard.distance.on = 1; + mainboard.left_cobroller.on = 1; + mainboard.right_cobroller.on = 1; scheduler_add_periodical_event_priority(do_cs, NULL, diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index a503f6d..2ceb7e8 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -77,8 +77,8 @@ uint8_t command_buf[I2C_SEND_BUFFER_SIZE]; volatile int8_t command_dest=-1; volatile uint8_t command_size=0; -static int8_t i2c_req_mechboard_status(void); -static int8_t i2c_req_sensorboard_status(void); +static int8_t i2c_req_cobboard_status(void); +static int8_t i2c_req_ballboard_status(void); #define I2C_ERROR(args...) do { \ if (error_log < I2C_MAX_LOG) { \ @@ -159,32 +159,32 @@ void i2c_poll_slaves(void *dummy) switch(i2c_state) { - /* poll status of mechboard */ -#define I2C_REQ_MECHBOARD 0 - case I2C_REQ_MECHBOARD: - if ((err = i2c_req_mechboard_status())) + /* poll status of cobboard */ +#define I2C_REQ_COBBOARD 0 + case I2C_REQ_COBBOARD: + if ((err = i2c_req_cobboard_status())) goto error; break; -#define I2C_ANS_MECHBOARD 1 - case I2C_ANS_MECHBOARD: - if ((err = i2c_recv(I2C_MECHBOARD_ADDR, - sizeof(struct i2c_ans_mechboard_status), +#define I2C_ANS_COBBOARD 1 + case I2C_ANS_COBBOARD: + if ((err = i2c_recv(I2C_COBBOARD_ADDR, + sizeof(struct i2c_ans_cobboard_status), I2C_CTRL_GENERIC))) goto error; break; - /* poll status of sensorboard */ -#define I2C_REQ_SENSORBOARD 2 - case I2C_REQ_SENSORBOARD: - if ((err = i2c_req_sensorboard_status())) + /* poll status of ballboard */ +#define I2C_REQ_BALLBOARD 2 + case I2C_REQ_BALLBOARD: + if ((err = i2c_req_ballboard_status())) goto error; break; -#define I2C_ANS_SENSORBOARD 3 - case I2C_ANS_SENSORBOARD: - if ((err = i2c_recv(I2C_SENSORBOARD_ADDR, - sizeof(struct i2c_ans_sensorboard_status), +#define I2C_ANS_BALLBOARD 3 + case I2C_ANS_BALLBOARD: + if ((err = i2c_recv(I2C_BALLBOARD_ADDR, + sizeof(struct i2c_ans_ballboard_status), I2C_CTRL_GENERIC))) goto error; break; @@ -253,53 +253,27 @@ void i2c_recvevent(uint8_t * buf, int8_t size) switch (buf[0]) { - case I2C_ANS_MECHBOARD_STATUS: { - struct i2c_ans_mechboard_status * ans = - (struct i2c_ans_mechboard_status *)buf; + case I2C_ANS_COBBOARD_STATUS: { + struct i2c_ans_cobboard_status * ans = + (struct i2c_ans_cobboard_status *)buf; if (size != sizeof (*ans)) goto error; /* status */ - mechboard.mode = ans->mode; - mechboard.status = ans->status; - mechboard.lintel_count = ans->lintel_count; - mechboard.column_flags = ans->column_flags; - /* pumps pwm */ - mechboard.pump_left1 = ans->pump_left1; - mechboard.pump_left2 = ans->pump_left2; - mechboard.pump_right1 = ans->pump_right1; - mechboard.pump_right2 = ans->pump_right2; - pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1); - pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2); - /* pumps current */ - mechboard.pump_right1_current = ans->pump_right1_current; - mechboard.pump_right2_current = ans->pump_right2_current; - /* servos */ - mechboard.servo_lintel_left = ans->servo_lintel_left; - mechboard.servo_lintel_right = ans->servo_lintel_right; - pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right); - pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left); + cobboard.mode = ans->mode; + cobboard.status = ans->status; break; } - case I2C_ANS_SENSORBOARD_STATUS: { - struct i2c_ans_sensorboard_status * ans = - (struct i2c_ans_sensorboard_status *)buf; + case I2C_ANS_BALLBOARD_STATUS: { + struct i2c_ans_ballboard_status * ans = + (struct i2c_ans_ballboard_status *)buf; if (size != sizeof (*ans)) goto error; - sensorboard.status = ans->status; - sensorboard.opponent_x = ans->opponent_x; - sensorboard.opponent_y = ans->opponent_y; - sensorboard.opponent_a = ans->opponent_a; - sensorboard.opponent_d = ans->opponent_d; - - sensorboard.scan_status = ans->scan_status; - sensorboard.dropzone_h = ans->dropzone_h; - sensorboard.dropzone_x = ans->dropzone_x; - sensorboard.dropzone_y = ans->dropzone_y; + ballboard.status = ans->status; break; } @@ -353,33 +327,24 @@ i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) return -EBUSY; } -static int8_t i2c_req_mechboard_status(void) +static int8_t i2c_req_cobboard_status(void) { - struct i2c_req_mechboard_status buf; + struct i2c_req_cobboard_status buf; int8_t err; - buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS; - buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3); - buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4); - err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, + buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS; + err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); return err; } -static int8_t i2c_req_sensorboard_status(void) +static int8_t i2c_req_ballboard_status(void) { - struct i2c_req_sensorboard_status buf; + struct i2c_req_ballboard_status buf; - buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS; - /* robot position */ - buf.x = position_get_x_s16(&mainboard.pos); - buf.y = position_get_y_s16(&mainboard.pos); - buf.a = position_get_a_deg_s16(&mainboard.pos); - /* pickup wheels */ - buf.enable_pickup_wheels = mainboard.enable_pickup_wheels; - - return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, + buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS; + return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); } @@ -387,7 +352,7 @@ int8_t i2c_set_color(uint8_t addr, uint8_t color) { struct i2c_cmd_generic_color buf; - if (addr == I2C_SENSORBOARD_ADDR) + if (addr == I2C_BALLBOARD_ADDR) return 0; /* XXX disabled for now */ buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR; buf.color = color; @@ -403,285 +368,27 @@ 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_mechboard_mode_manual(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_MANUAL; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_harvest(void) +int8_t i2c_cobboard_mode_manual(void) { - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_HARVEST; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + struct i2c_cmd_cobboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + buf.mode = I2C_COBBOARD_MODE_MANUAL; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_mechboard_mode_lazy_harvest(void) +int8_t i2c_cobboard_mode_harvest(void) { - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + struct i2c_cmd_cobboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + buf.mode = I2C_COBBOARD_MODE_HARVEST; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side) +int8_t i2c_cobboard_mode_init(void) { - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - buf.prep_pickup.side = side; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + 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)); } -int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC; - buf.prep_pickup.side = side; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - buf.prep_pickup.next_mode = next_mode; - buf.prep_pickup.side = side; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_pickup(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PICKUP; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_eject(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_EJECT; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_manivelle(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_MANIVELLE; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_push_temple(uint8_t level) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE; - buf.push_temple.level = level; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; - buf.prep_build.level_l = level; - buf.prep_build.level_r = level; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; - buf.prep_build.level_l = level_l; - buf.prep_build.level_r = level_r; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; - buf.prep_inside.level_l = level; - buf.prep_inside.level_r = level; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; - buf.prep_inside.level_l = level_l; - buf.prep_inside.level_r = level_r; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - buf.autobuild.level_left = level; - buf.autobuild.level_right = level; - buf.autobuild.count_left = 2; - buf.autobuild.count_right = 2; - buf.autobuild.do_lintel = 1; - buf.autobuild.distance_left = 210; - buf.autobuild.distance_right = 210; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l, - uint8_t dist_l, - uint8_t level_r, uint8_t count_r, - uint8_t dist_r, - uint8_t do_lintel) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD; - buf.autobuild.level_left = level_l; - buf.autobuild.level_right = level_r; - buf.autobuild.count_left = count_l; - buf.autobuild.count_right = count_r; - buf.autobuild.distance_left = dist_l; - buf.autobuild.distance_right = dist_r; - buf.autobuild.do_lintel = do_lintel; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_init(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_INIT; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_prepare_get_lintel(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_get_lintel(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_put_lintel(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_clear(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_CLEAR; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_loaded(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_LOADED; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_store(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_STORE; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_mechboard_mode_lazy_pickup(void) -{ - struct i2c_cmd_mechboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; - buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_set_beacon(uint8_t enable) -{ - struct i2c_cmd_sensorboard_start_beacon buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON; - buf.enable = enable; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_scanner_set(uint8_t mode) -{ - struct i2c_cmd_sensorboard_scanner buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER; - buf.mode = mode; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_scanner_calib(void) -{ - struct i2c_cmd_sensorboard_calib_scanner buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone, - int16_t x, int16_t y) -{ - struct i2c_cmd_sensorboard_scanner_algo buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; - buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE; - buf.drop_zone.working_zone = zone; - buf.drop_zone.center_x = x; - buf.drop_zone.center_y = y; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone, - int16_t x, int16_t y) -{ - struct i2c_cmd_sensorboard_scanner_algo buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; - buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE; - buf.drop_zone.working_zone = zone; - buf.drop_zone.center_x = x; - buf.drop_zone.center_y = y; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} - -int8_t i2c_sensorboard_scanner_algo_check(uint8_t level, - int16_t x, int16_t y) -{ - struct i2c_cmd_sensorboard_scanner_algo buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; - buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE; - buf.check_temple.level = level; - buf.check_temple.temple_x = x; - buf.check_temple.temple_y = y; - return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); -} diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h index 0d40205..1e22a4f 100644 --- a/projects/microb2010/mainboard/i2c_protocol.h +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -35,58 +35,8 @@ void i2c_sendevent(int8_t size); int8_t i2c_set_color(uint8_t addr, uint8_t color); int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state); -int8_t i2c_mechboard_mode_manual(void); -int8_t i2c_mechboard_mode_harvest(void); -int8_t i2c_mechboard_mode_lazy_harvest(void); -int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side); -int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode); -int8_t i2c_mechboard_mode_pickup(void); -int8_t i2c_mechboard_mode_eject(void); -int8_t i2c_mechboard_mode_lazy_pickup(void); - -int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level); -int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r); -int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level); -int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r); -int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level); -int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l, - uint8_t dist_l, - uint8_t level_r, uint8_t count_r, - uint8_t dist_r, - uint8_t do_lintel); -int8_t i2c_mechboard_mode_init(void); -int8_t i2c_mechboard_mode_eject(void); -int8_t i2c_mechboard_mode_prepare_get_lintel(void); -int8_t i2c_mechboard_mode_get_lintel(void); -int8_t i2c_mechboard_mode_put_lintel(void); -int8_t i2c_mechboard_mode_clear(void); -int8_t i2c_mechboard_mode_loaded(void); -int8_t i2c_mechboard_mode_store(void); -int8_t i2c_mechboard_mode_manivelle(void); -int8_t i2c_mechboard_mode_push_temple(uint8_t level); -int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side); - -int8_t i2c_sensorboard_set_beacon(uint8_t enable); - -int8_t i2c_sensorboard_scanner_set(uint8_t mode); -static inline int8_t i2c_sensorboard_scanner_stop(void) { - return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_STOP); -} -static inline int8_t i2c_sensorboard_scanner_start(void) { - return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_START); -} -static inline int8_t i2c_sensorboard_scanner_prepare(void) { - return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_PREPARE); -} - -int8_t i2c_sensorboard_scanner_calib(void); - -int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone, - int16_t x, int16_t y); -int8_t i2c_sensorboard_scanner_algo_check(uint8_t level, - int16_t x, int16_t y); -int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone, - int16_t x, int16_t y); - +int8_t i2c_cobboard_mode_manual(void); +int8_t i2c_cobboard_mode_harvest(void); +int8_t i2c_cobboard_mode_init(void); #endif diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index d69d058..d0c9d14 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -67,10 +67,6 @@ #include "cs.h" #include "i2c_protocol.h" -#if __AVR_LIBC_VERSION__ == 10602UL -#error "won't work with this version" -#endif - /* 0 means "programmed" * ---- with 16 Mhz quartz * CKSEL 3-0 : 0111 @@ -85,8 +81,8 @@ struct genboard gen; struct mainboard mainboard; -struct mechboard mechboard; -struct sensorboard sensorboard; +struct cobboard cobboard; +struct ballboard ballboard; /***********************/ @@ -172,7 +168,6 @@ int main(void) memset(&mainboard, 0, sizeof(mainboard)); mainboard.flags = DO_ENCODERS | DO_RS | DO_POS | DO_POWER | DO_BD; - sensorboard.opponent_x = I2C_OPPONENT_NOT_THERE; /* UART */ uart_init(); @@ -244,10 +239,6 @@ int main(void) NULL, 0); PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); - pwm_ng_set(&gen.servo2, 290); /* right */ - pwm_ng_set(&gen.servo3, 400); /* left */ - /* 2 lintels 180, 485 */ - /* 1 lintel 155, 520 */ /* SCHEDULER */ scheduler_init(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 78fccc4..7f21058 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -66,14 +66,15 @@ #define IMP_COEF 10. #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF) -#define LEFT_ENCODER ((void *)1) -#define RIGHT_ENCODER ((void *)0) +#define LEFT_ENCODER ((void *)1) +#define RIGHT_ENCODER ((void *)0) +#define LEFT_COBROLLER_ENCODER ((void *)2) +#define RIGHT_COBROLLER_ENCODER ((void *)3) #define LEFT_PWM ((void *)&gen.pwm1_4A) #define RIGHT_PWM ((void *)&gen.pwm2_4B) - -#define LEFT_PUMP1_PWM ((void *)&gen.pwm3_1A) -#define LEFT_PUMP2_PWM ((void *)&gen.pwm4_1B) +#define LEFT_COBROLLER_PWM ((void *)&gen.pwm3_1A) +#define RIGHT_COBROLLER_PWM ((void *)&gen.pwm4_1B) /** ERROR NUMS */ #define E_USER_STRAT 194 @@ -142,6 +143,8 @@ struct mainboard { /* control systems */ struct cs_block angle; struct cs_block distance; + struct cs_block left_cobroller; + struct cs_block right_cobroller; /* x,y positionning */ struct robot_system rs; @@ -154,54 +157,23 @@ struct mainboard { volatile int16_t speed_d; /* current dist speed */ int32_t pwm_l; /* current left pwm */ int32_t pwm_r; /* current right pwm */ - uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */ - }; -/* state of mechboard, synchronized through i2c */ -struct mechboard { +/* state of cobboard, synchronized through i2c */ +struct cobboard { uint8_t mode; uint8_t status; - int8_t lintel_count; - uint8_t column_flags; - - /* pwm */ - int16_t pump_left1; - int16_t pump_right1; - int16_t pump_left2; - int16_t pump_right2; - - /* currents (for left arm, we can just read it on adc) */ - int16_t pump_right1_current; - int16_t pump_right2_current; - - /* pwm for lintel servos */ - uint16_t servo_lintel_left; - uint16_t servo_lintel_right; }; -/* state of sensorboard, synchronized through i2c */ -struct sensorboard { +/* state of ballboard, synchronized through i2c */ +struct ballboard { uint8_t status; - /* opponent pos */ - int16_t opponent_x; - int16_t opponent_y; - int16_t opponent_a; - int16_t opponent_d; - - /* scanner */ -#define I2C_SCAN_DONE 1 - uint8_t scan_status; -#define I2C_COLUMN_NO_DROPZONE -1 - int8_t dropzone_h; - int16_t dropzone_x; - int16_t dropzone_y; }; extern struct genboard gen; extern struct mainboard mainboard; -extern struct mechboard mechboard; -extern struct sensorboard sensorboard; +extern struct cobboard cobboard; +extern struct ballboard ballboard; /* start the bootloader */ void bootloader(void); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 3f54716..864b00b 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -33,7 +33,7 @@ #include #include #include -#include +#include #include #include @@ -65,182 +65,12 @@ #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 - -#ifdef TEST_BEACON - -#define BEACON_MAX_SAMPLES 100 -struct beacon_sample { - int16_t posx; - int16_t posy; - int16_t posa; - int16_t oppx; - int16_t oppy; - uint8_t time; -}; - -static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES]; -static uint8_t beacon_prev_time = 0; -static uint8_t beacon_cur_idx = 0; - -static void beacon_update_samples(void) -{ - int16_t opp_a, opp_d, opp_x, opp_y; - int8_t err; - uint8_t time; - - time = time_get_s(); - - /* one sample per second max */ - if (time <= beacon_prev_time) - return; - /* limit max number of samples */ - if (beacon_cur_idx >= BEACON_MAX_SAMPLES) - return; - - memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx])); - beacon_prev_time = time; - beacon_sample[beacon_cur_idx].time = time; - - /* get opponent pos; if not found, just set struct to 0 */ - err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a); - if (err == -1) - return; - - beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].oppx = opp_x; - beacon_sample[beacon_cur_idx].oppy = opp_y; - beacon_cur_idx++; -} - -void beacon_dump_samples(void) -{ - uint16_t i; - - for (i=0; izone->name); - - if (temple->flags & TEMPLE_F_MONOCOL) - printf_P(PSTR("MONOCOL ")); - else - printf_P(PSTR("BICOL ")); - - if (temple->flags & TEMPLE_F_ON_DISC) - printf_P(PSTR("ON_DISC ")); - else - printf_P(PSTR("ON_ZONE_0_1 ")); - - if (temple->flags & TEMPLE_F_OPPONENT) - printf_P(PSTR("OPPONENT ")); - else - printf_P(PSTR("OURS ")); - - if (temple->flags & TEMPLE_F_LINTEL) - printf_P(PSTR("LIN_ON_TOP ")); - else - printf_P(PSTR("COL_ON_TOP ")); - - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"), - temple->x, temple->y, temple->a, - temple->checkpoint_x, temple->checkpoint_y, - temple->last_try_time); - printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"), - temple->level_l, temple->dist_l, temple->angle_l); - printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"), - temple->level_l, temple->dist_l, temple->angle_l); -} - -void strat_dump_zone(struct build_zone *zone) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR(" zone %s: "), zone->name); - - if (zone->flags & ZONE_F_DISC) - printf_P(PSTR("DISC ")); - else if (zone->flags & ZONE_F_ZONE1) - printf_P(PSTR("ZONE1 ")); - else if (zone->flags & ZONE_F_ZONE0) - printf_P(PSTR("ZONE0 ")); - - if (zone->flags & ZONE_F_BUSY) - printf_P(PSTR("BUSY ")); - else - printf_P(PSTR("FREE ")); - - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"), - zone->level, - zone->checkpoint_x, zone->checkpoint_y, - zone->last_try_time); -} - -void strat_dump_static_cols(void) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"), - strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE, - strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE, - strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE); -} - -void strat_dump_col_disp(void) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"), - strat_infos.c1.count, strat_infos.c1.last_try_time); - printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"), - strat_infos.c2.count, strat_infos.c2.last_try_time); - printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"), - strat_infos.c3.count, strat_infos.c3.last_try_time); -} - -void strat_dump_lin_disp(void) -{ - if (!strat_infos.dump_enabled) - return; - printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"), - strat_infos.l1.count, strat_infos.l1.last_try_time); - printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"), - strat_infos.l2.count, strat_infos.l2.last_try_time); - -} - -void strat_dump_all_temples(void) -{ - struct temple *temple; - uint8_t i; - - if (!strat_infos.dump_enabled) - return; - - for (i=0; iflags & TEMPLE_F_VALID)) - continue; - strat_dump_temple(temple); - } -} - -void strat_dump_all_zones(void) -{ - struct build_zone *zone; - uint8_t i; - - if (!strat_infos.dump_enabled) - return; - - for (i=0; iflags & ZONE_F_VALID)) - continue; - strat_dump_zone(zone); - } } /* display current information about the state of the game */ @@ -499,64 +129,18 @@ void strat_dump_infos(const char *caller) return; printf_P(PSTR("%s() dump strat infos:\r\n"), caller); - strat_dump_static_cols(); - strat_dump_col_disp(); - strat_dump_lin_disp(); - strat_dump_all_temples(); - strat_dump_all_zones(); } /* init current area state before a match. Dump update user conf * here */ void strat_reset_infos(void) { - uint8_t i; - - /* /!\ don't do a big memset() as there is static data */ - strat_infos.s_cols.flags = 0; - strat_infos.c1.count = 5; - strat_infos.c1.last_try_time = 0; - strat_infos.c2.count = 5; - strat_infos.c2.last_try_time = 0; - strat_infos.c3.count = 5; - strat_infos.c3.last_try_time = 0; - strat_infos.l1.count = 1; - strat_infos.l1.last_try_time = 0; - strat_infos.l2.count = 1; - strat_infos.l2.last_try_time = 0; - - strat_infos.taken_lintel = 0; - strat_infos.col_in_boobs = 0; - strat_infos.lazy_pickup_done = 0; - strat_infos.i2c_loaded_skipped = 0; - - memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list)); - - for (i=0; i= 1) && - (get_column_count_right() >= 1)) - return 0; - } - else { - if (get_column_count()) - return 0; - } - return 1; + return END_TRAJ; } /* dump state (every 5 s max) */ @@ -687,121 +196,9 @@ uint8_t need_more_elements(void) uint8_t strat_main(void) { uint8_t err; - struct temple *temple = NULL; - struct build_zone *zone = NULL; - - uint8_t last_print_cols = 0; - uint8_t last_print_lin = 0; - uint8_t last_print_temple = 0; - uint8_t last_print_zone = 0; /* do static cols + first temple */ err = strat_beginning(); - /* skip error code */ - - while (1) { - - if (err == END_TIMER) { - DEBUG(E_USER_STRAT, "End of time"); - strat_exit(); - break; - } - - /* we have at least one col on each arm, build now */ - if (need_more_elements() == 0) { - - /* try to build on opponent, will return - * END_TRAJ without doing anything if - * disabled */ - err = strat_build_on_opponent_temple(); - if (!TRAJ_SUCCESS(err)) - continue; - if (need_more_elements()) - continue; - - /* try to scan and build on our temple, will - * return END_TRAJ without doing anything if - * disabled */ - err = strat_check_temple_and_build(); - if (!TRAJ_SUCCESS(err)) - continue; - if (need_more_elements()) - continue; - - /* Else, do a simple build, as before */ - - temple = strat_get_best_temple(); - - /* one valid temple found */ - if (temple) { - DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple); - - err = strat_goto_temple(temple); - if (!TRAJ_SUCCESS(err)) - continue; - - /* can return END_ERROR or END_TIMER, - * should not happen here */ - err = strat_grow_temple(temple); - if (!TRAJ_SUCCESS(err)) - continue; - - err = strat_escape(temple->zone, TRAJ_FLAGS_STD); - if (!TRAJ_SUCCESS(err)) - continue; - - continue; - } - - zone = strat_get_best_zone(); - if (zone) { - DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone); - - DEBUG(E_USER_STRAT, "goto zone %s", zone->name); - err = strat_goto_build_zone(zone, zone->level); - if (!TRAJ_SUCCESS(err)) - continue; - DEBUG(E_USER_STRAT, "zone reached"); - - /* no error code except END_ERROR, should not happen */ - err = strat_build_new_temple(zone); - - err = strat_escape(zone, TRAJ_FLAGS_STD); - if (!TRAJ_SUCCESS(err)) - continue; - - continue; - } - - /* XXX hey what can we do here... :'( */ - DEBUG(E_USER_STRAT, "panic :)"); - time_wait_ms(1000); - continue; - } - - /* else we need some elements (lintels, then columns) */ - else { - if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0) - DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin); - - err = strat_pickup_lintels(); - /* can return an error code, but we have - * nothing to do because pickup_column() - * starts with a goto_and_avoid() */ - if (!TRAJ_SUCCESS(err)) - nop(); - - DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols); - - err = strat_pickup_columns(); - if (!TRAJ_SUCCESS(err)) - nop(); /* nothing to do */ - - /* XXX check here that we have elements, or do - * something else */ - /* if we cannot take elements, try to build */ - } - } - return END_TRAJ; + return err; } diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 2b2b96a..add640a 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -43,66 +43,24 @@ #define CORNER_Y COLOR_Y(2100) /* - * /- line 0 - * | /- line 1 - * | | /- line 2 - * | | | - * +---C1--------------------------C1---+ - * | z0a z1 z0b | - * | . | - * | . | - * C3 0 1 2 ^ C3 - * y | 3 4 5 < > | - * C2 6 7 8 v C2 - * |------ 9 10 11 . ------| - * | | . | | - * | red | . |green| - * +-----+--L1--L2-------L2--L1---+-----+ + * + * + * + * + * +------------------------------------+ + * | | + * | | + * | | + * | | + * y | | + * | | + * |------ ------| + * | | | | + * | | | | + * +-----+------------------------+-----+ * x */ -/* static columns */ -#define LINE0_X 600 -#define LINE1_X 850 -#define LINE2_X 1100 - -#define COL0_X 600 -#define COL0_Y COLOR_Y(1175) -#define COL1_X 850 -#define COL1_Y COLOR_Y(1175) -#define COL2_X 1100 -#define COL2_Y COLOR_Y(1175) - -#define COL3_X 600 -#define COL3_Y COLOR_Y(975) -#define COL4_X 850 -#define COL4_Y COLOR_Y(975) -#define COL5_X 1100 -#define COL5_Y COLOR_Y(975) - -#define COL6_X 600 -#define COL6_Y COLOR_Y(775) -#define COL7_X 850 -#define COL7_Y COLOR_Y(775) -#define COL8_X 1100 -#define COL8_Y COLOR_Y(775) - -#define COL9_X 600 -#define COL9_Y COLOR_Y(575) -#define COL10_X 850 -#define COL10_Y COLOR_Y(575) -#define COL11_X 1100 -#define COL11_Y COLOR_Y(575) - -/* distance to go backward before pickup in dispenser */ -#define DIST_BACK_DISPENSER 35 - -/* diag of the pentagon (pentacle ?) */ -#define DISC_PENTA_DIAG 530 - -#define COL_DISP_MAX_TRIES 5 -#define LIN_DISP_MAX_TRIES 3 - /* useful traj flags */ #define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR)) #define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER) @@ -129,128 +87,21 @@ struct bbox { }; struct conf { -#define STRAT_CONF_ONLY_ONE_ON_DISC 0x01 -#define STRAT_CONF_BYPASS_STATIC2 0x02 -#define STRAT_CONF_TAKE_ONE_LINTEL 0x04 -#define STRAT_CONF_SKIP_WHEN_CHECK_FAILS 0x08 -#define STRAT_CONF_STORE_STATIC2 0x10 -#define STRAT_CONF_BIG_3_TEMPLE 0x20 -#define STRAT_CONF_EARLY_SCAN 0x40 -#define STRAT_CONF_PUSH_OPP_COLS 0x80 - uint8_t flags; - uint8_t scan_opp_min_time; - uint8_t delay_between_opp_scan; - uint8_t scan_our_min_time; - uint8_t delay_between_our_scan; - uint8_t wait_opponent; - uint8_t lintel_min_time; - int16_t scan_opp_angle; -}; - -struct static_columns { -#define STATIC_COL_LINE0_DONE 0x01 -#define STATIC_COL_LINE1_DONE 0x02 -#define STATIC_COL_LINE2_DONE 0x04 - uint8_t flags; - uint8_t configuration; -}; - -struct column_dispenser { - int8_t count; - uint8_t last_try_time; - uint8_t scan_left; - int16_t checkpoint_x; - int16_t checkpoint_y; - int16_t scan_a; - int16_t eject_a; - int16_t pickup_a; - int16_t recalib_x; - int16_t recalib_y; - char *name; -}; - -struct lintel_dispenser { - int8_t count; - uint8_t last_try_time; - int16_t x; - char *name; -}; - -struct temple { -#define TEMPLE_F_VALID 0x01 /* structure is valid */ -#define TEMPLE_F_MONOCOL 0x02 /* temple has only one col */ -#define TEMPLE_F_ON_DISC 0x04 /* temple is on disc (else it's on other zone) */ -#define TEMPLE_F_OPPONENT 0x08 /* temple was originally built by opponent */ -#define TEMPLE_F_LINTEL 0x10 /* lintel on top (don't put another lintel) */ - +#define STRAT_CONF_XXX 0x01 uint8_t flags; - /* position of the robot when we built it */ - int16_t x; - int16_t y; - int16_t a; - - /* position of the robot checkpoint */ - int16_t checkpoint_x; - int16_t checkpoint_y; - - /* position and level of each col */ - uint8_t level_l; - uint8_t dist_l; - uint8_t angle_l; - - uint8_t level_r; - uint8_t dist_r; - uint8_t angle_r; - -#define TEMPLE_DISABLE_TIME 5 - uint8_t last_try_time; - - struct build_zone *zone; }; -struct build_zone { -#define ZONE_F_VALID 0x01 /* zone is valid */ -#define ZONE_F_DISC 0x02 /* specific disc zone */ -#define ZONE_F_ZONE1 0x04 /* specific zone 1 */ -#define ZONE_F_ZONE0 0x08 /* specific zone 0 */ -#define ZONE_F_BUSY 0x10 /* this zone is busy */ - uint8_t flags; - uint8_t level; - int16_t checkpoint_x; - int16_t checkpoint_y; - -#define ZONE_DISABLE_TIME 5 - uint8_t last_try_time; - char *name; -}; - -#define MAX_TEMPLE 5 -#define MAX_ZONE 5 - /* all infos related to strat */ struct strat_infos { uint8_t dump_enabled; struct conf conf; struct bbox area_bbox; - uint8_t taken_lintel; - uint8_t col_in_boobs; - uint8_t lazy_pickup_done; - uint8_t i2c_loaded_skipped; - struct static_columns s_cols; - struct column_dispenser c1; - struct column_dispenser c2; - struct column_dispenser c3; - struct lintel_dispenser l1; - struct lintel_dispenser l2; - struct build_zone zone_list[MAX_ZONE]; - struct temple temple_list[MAX_TEMPLE]; }; extern struct strat_infos strat_infos; /* in strat.c */ void strat_dump_infos(const char *caller); /* show current known state of area */ -void strat_dump_temple(struct temple *temple); void strat_dump_conf(void); void strat_reset_infos(void); /* reset current known state */ void strat_preinit(void); @@ -261,42 +112,4 @@ void strat_goto_near(int16_t x, int16_t y, uint16_t dist); uint8_t strat_main(void); void strat_event(void *dummy); -/* in strat_static_columns.c */ -uint8_t strat_static_columns(uint8_t configuration); -uint8_t strat_static_columns_pass2(void); - -/* in strat_lintel.c */ -uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp); -uint8_t strat_pickup_lintels(void); - -/* in strat_column_disp.c */ -uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a); -uint8_t strat_pickup_columns(void); -uint8_t strat_goto_col_disp(struct column_dispenser **disp); - -/* in strat_building.c */ -uint8_t strat_goto_disc(int8_t level); -uint8_t strat_goto_build_zone(struct build_zone *build_zone, uint8_t level); -uint8_t strat_build_new_temple(struct build_zone *build_zone); -uint8_t strat_goto_temple(struct temple *temple); -uint8_t strat_grow_temple(struct temple *temple); -uint8_t strat_grow_temple_column(struct temple *temple); -struct temple *strat_get_best_temple(void); -struct temple *strat_get_our_temple_on_disc(uint8_t valid); -struct build_zone *strat_get_best_zone(void); -struct temple *strat_get_free_temple(void); - -/* in strat_scan.c */ -struct scan_disc_result; -void scanner_dump_state(void); -int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x, - int16_t *ckpt_rel_y, int16_t *back_mm); -uint8_t strat_scan_disc(int16_t angle, uint8_t mode, - struct scan_disc_result *result); -uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level); -int16_t strat_get_temple_angle(struct temple *temple); -int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle); -uint8_t strat_build_on_opponent_temple(void); -uint8_t strat_check_temple_and_build(void); - #endif diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c deleted file mode 100644 index 6e75dd0..0000000 --- a/projects/microb2010/mainboard/strat_avoid.c +++ /dev/null @@ -1,535 +0,0 @@ -/* - * Copyright Droids Corporation, Microb Technology (2009) - * - * 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_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - */ - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "main.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_utils.h" -#include "sensor.h" - -#define EDGE_NUMBER 5 -void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt, - int16_t radius, int16_t x, int16_t y) -{ - - double c_a, s_a; - uint8_t i; - double px1, py1, px2, py2; - double a_rad; - - a_rad = atan2(y - robot_pt->y, x - robot_pt->x); - - /* generate pentagon */ - c_a = cos(-2*M_PI/EDGE_NUMBER); - s_a = sin(-2*M_PI/EDGE_NUMBER); - - /* - px1 = radius; - py1 = 0; - */ - px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER)); - py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER)); - - - for (i = 0; i < EDGE_NUMBER; i++){ - oa_poly_set_point(pol, x + px1, y + py1, i); - - px2 = px1*c_a + py1*s_a; - py2 = -px1*s_a + py1*c_a; - - px1 = px2; - py1 = py2; - } -} - -void set_rotated_poly(poly_t *pol, const point_t *robot_pt, - int16_t w, int16_t l, int16_t x, int16_t y) -{ - double tmp_x, tmp_y; - double a_rad; - - a_rad = atan2(y - robot_pt->y, x - robot_pt->x); - - DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f", - __FUNCTION__, x, y, a_rad); - - /* point 1 */ - tmp_x = w; - tmp_y = l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 0); - - /* point 2 */ - tmp_x = -w; - tmp_y = l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 1); - - /* point 3 */ - tmp_x = -w; - tmp_y = -l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 2); - - /* point 4 */ - tmp_x = w; - tmp_y = -l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 3); -} - -#define DISC_X CENTER_X -#define DISC_Y CENTER_Y - -void set_central_disc_poly(poly_t *pol, const point_t *robot_pt) -{ - set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG, - DISC_X, DISC_Y); -} - -#ifdef HOMOLOGATION -/* /!\ half size */ -#define O_WIDTH 400 -#define O_LENGTH 550 -#else -/* /!\ half size */ -#define O_WIDTH 360 -#define O_LENGTH 500 -#endif - -void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l) -{ - int16_t x, y; - get_opponent_xy(&x, &y); - DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y); - - /* place poly even if invalid, because it's -100 */ - set_rotated_poly(pol, robot_pt, w, l, x, y); -} - -/* don't care about polygons further than this distance for escape */ -#define ESCAPE_POLY_THRES 1000 - -/* don't reduce opp if opp is too far */ -#define REDUCE_POLY_THRES 600 - -/* has to be longer than any poly */ -#define ESCAPE_VECT_LEN 3000 - -/* - * Go in playground, loop until out of poly. The argument robot_pt is - * the pointer to the current position of the robot. - * Return 0 if there was nothing to do. - * Return 1 if we had to move. In this case, update the theorical - * position of the robot in robot_pt. - */ -static int8_t go_in_area(point_t *robot_pt) -{ - point_t poly_pts_area[4]; - poly_t poly_area; - point_t disc_pt, dst_pt; - - disc_pt.x = DISC_X; - disc_pt.y = DISC_Y; - - /* Go in playground */ - if (!is_in_boundingbox(robot_pt)){ - NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"", - robot_pt->x, robot_pt->y); - - poly_area.l = 4; - poly_area.pts = poly_pts_area; - poly_pts_area[0].x = strat_infos.area_bbox.x1; - poly_pts_area[0].y = strat_infos.area_bbox.y1; - - poly_pts_area[1].x = strat_infos.area_bbox.x2; - poly_pts_area[1].y = strat_infos.area_bbox.y1; - - poly_pts_area[2].x = strat_infos.area_bbox.x2; - poly_pts_area[2].y = strat_infos.area_bbox.y2; - - poly_pts_area[3].x = strat_infos.area_bbox.x1; - poly_pts_area[3].y = strat_infos.area_bbox.y2; - - is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area); - NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y); - - strat_goto_xy_force(dst_pt.x, dst_pt.y); - - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); - - return 1; - } - - return 0; -} - - -/* - * Escape from polygons if needed. - * robot_pt is the current position of the robot, it will be - * updated. - */ -static int8_t escape_from_poly(point_t *robot_pt, - poly_t *pol_disc, - int16_t opp_x, int16_t opp_y, - int16_t opp_w, int16_t opp_l, - poly_t *pol_opp) -{ - uint8_t in_disc = 0, in_opp = 0; - double escape_dx = 0, escape_dy = 0; - double disc_dx = 0, disc_dy = 0; - double opp_dx = 0, opp_dy = 0; - double len; - point_t opp_pt, disc_pt, dst_pt; - point_t intersect_disc_pt, intersect_opp_pt; - - opp_pt.x = opp_x; - opp_pt.y = opp_y; - disc_pt.x = DISC_X; - disc_pt.y = DISC_Y; - - /* escape from other poly if necessary */ - if (is_in_poly(robot_pt, pol_disc) == 1) - in_disc = 1; - if (is_in_poly(robot_pt, pol_opp) == 1) - in_opp = 1; - - if (in_disc == 0 && in_opp == 0) { - NOTICE(E_USER_STRAT, "no need to escape"); - return 0; - } - - NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp); - - /* process escape vector */ - - if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) { - disc_dx = robot_pt->x - DISC_X; - disc_dy = robot_pt->y - DISC_Y; - NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f", - disc_dx, disc_dy); - len = norm(disc_dx, disc_dy); - if (len != 0) { - disc_dx /= len; - disc_dy /= len; - } - else { - disc_dx = 1.0; - disc_dy = 0.0; - } - escape_dx += disc_dx; - escape_dy += disc_dy; - } - - if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) { - opp_dx = robot_pt->x - opp_x; - opp_dy = robot_pt->y - opp_y; - NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f", - opp_dx, opp_dy); - len = norm(opp_dx, opp_dy); - if (len != 0) { - opp_dx /= len; - opp_dy /= len; - } - else { - opp_dx = 1.0; - opp_dy = 0.0; - } - escape_dx += opp_dx; - escape_dy += opp_dy; - } - - /* normalize escape vector */ - len = norm(escape_dx, escape_dy); - if (len != 0) { - escape_dx /= len; - escape_dy /= len; - } - else { - if (pol_disc != NULL) { - /* rotate 90° */ - escape_dx = disc_dy; - escape_dy = disc_dx; - } - else if (pol_opp != NULL) { - /* rotate 90° */ - escape_dx = opp_dy; - escape_dy = opp_dx; - } - else { /* should not happen */ - opp_dx = 1.0; - opp_dy = 0.0; - } - } - - NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f", - escape_dx, escape_dy); - - /* process the correct len of escape vector */ - - dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN; - dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN; - - NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32, - robot_pt->x, robot_pt->y); - NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32, - dst_pt.x, dst_pt.y); - - if (in_disc) { - if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt, - pol_disc) == 1) { - /* we add 2 mm to be sure we are out of th polygon */ - dst_pt.x = intersect_disc_pt.x + escape_dx * 2; - dst_pt.y = intersect_disc_pt.y + escape_dy * 2; - if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) { - - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); - - strat_goto_xy_force(dst_pt.x, dst_pt.y); - - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; - - return 0; - } - } - } - - if (in_opp) { - if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt, - pol_opp) == 1) { - /* we add 2 cm to be sure we are out of th polygon */ - dst_pt.x = intersect_opp_pt.x + escape_dx * 2; - dst_pt.y = intersect_opp_pt.y + escape_dy * 2; - - if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) { - - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); - - strat_goto_xy_force(dst_pt.x, dst_pt.y); - - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; - - return 0; - } - } - } - - /* should not happen */ - return -1; -} - - -static int8_t __goto_and_avoid(int16_t x, int16_t y, - uint8_t flags_intermediate, - uint8_t flags_final, - uint8_t forward) -{ - int8_t len = -1, i; - point_t *p; - poly_t *pol_disc, *pol_opp; - int8_t ret; - int16_t opp_w, opp_l, opp_x, opp_y; - point_t p_dst, robot_pt; - - DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d", - __FUNCTION__, x, y, flags_intermediate, flags_final, forward); - - retry: - get_opponent_xy(&opp_x, &opp_y); - opp_w = O_WIDTH; - opp_l = O_LENGTH; - - robot_pt.x = position_get_x_s16(&mainboard.pos); - robot_pt.y = position_get_y_s16(&mainboard.pos); - - oa_init(); - pol_disc = oa_new_poly(5); - set_central_disc_poly(pol_disc, &robot_pt); - pol_opp = oa_new_poly(4); - set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH); - - /* If we are not in the limited area, try to go in it. */ - ret = go_in_area(&robot_pt); - - /* check that destination is valid */ - p_dst.x = x; - p_dst.y = y; - if (!is_in_boundingbox(&p_dst)) { - NOTICE(E_USER_STRAT, " dst is not in playground"); - return END_ERROR; - } - if (is_point_in_poly(pol_disc, x, y)) { - NOTICE(E_USER_STRAT, " dst is in disc"); - return END_ERROR; - } - if (is_point_in_poly(pol_opp, x, y)) { - NOTICE(E_USER_STRAT, " dst is in opp"); - return END_ERROR; - } - - /* now start to avoid */ - while (opp_w && opp_l) { - - /* robot_pt is not updated if it fails */ - ret = escape_from_poly(&robot_pt, - pol_disc, opp_x, opp_y, - opp_w, opp_l, pol_opp); - if (ret == 0) { - oa_reset(); - oa_start_end_points(robot_pt.x, robot_pt.y, x, y); - /* oa_dump(); */ - - len = oa_process(); - if (len >= 0) - break; - } - if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) { - if (opp_w == 0) - opp_l /= 2; - opp_w /= 2; - } - else { - NOTICE(E_USER_STRAT, "oa_process() returned %d", len); - return END_ERROR; - } - - NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l); - set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l); - } - - p = oa_get_path(); - for (i=0 ; ix, p->y); - - if (forward) - trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y); - else - trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y); - - /* no END_NEAR for the last point */ - if (i == len - 1) - ret = wait_traj_end(flags_final); - else - ret = wait_traj_end(flags_intermediate); - - if (ret == END_BLOCKING) { - DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", - __FUNCTION__, x, y); - goto retry; - } - else if (ret == END_OBSTACLE) { - /* brake and wait the speed to be slow */ - DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", - __FUNCTION__, x, y); - goto retry; - } - /* else if it is not END_TRAJ or END_NEAR, return */ - else if (!TRAJ_SUCCESS(ret)) { - return ret; - } - p++; - } - - return END_TRAJ; -} - -/* go forward to a x,y point. use current speed for that */ -uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) -{ - return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1); -} - -/* go backward to a x,y point. use current speed for that */ -uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) -{ - return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0); -} - -/* go to a x,y point. prefer backward but go forward if the point is - * near and in front of us */ -uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) -{ - double d,a; - abs_xy_to_rel_da(x, y, &d, &a); - - if (d < 300 && a < RAD(90) && a > RAD(-90)) - return __goto_and_avoid(x, y, flags_intermediate, - flags_final, 1); - else - return __goto_and_avoid(x, y, flags_intermediate, - flags_final, 0); -} diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h deleted file mode 100644 index 0978fd7..0000000 --- a/projects/microb2010/mainboard/strat_avoid.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright Droids Corporation, Microb Technology (2009) - * - * 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_avoid.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - */ - -void set_opponent_poly(poly_t *pol, int16_t w, int16_t l); -int8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final); -int8_t goto_and_avoid_backward(int16_t x, int16_t y, - uint8_t flags_intermediate, - uint8_t flags_final); -int8_t goto_and_avoid_forward(int16_t x, int16_t y, - uint8_t flags_intermediate, - uint8_t flags_final); diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 21d526a..40c6f86 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -126,27 +126,6 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) { uint8_t i, err; -#ifdef HOMOLOGATION - int8_t serr; - uint8_t hardstop = 0; - microseconds us = time_get_us2(); - int16_t opp_a, opp_d, opp_x, opp_y; - - while (1) { - serr = get_opponent_xyda(&opp_x, &opp_y, - &opp_d, &opp_a); - if (serr == -1) - break; - if (opp_d < 600) - break; - if (hardstop == 0) { - strat_hardstop(); - hardstop = 1; - } - if ((time_get_us2() - us) > 3000000L) - break; - } -#endif for (i=0; i<3; i++) { trajectory_goto_xy_abs(&mainboard.traj, x, y); err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); @@ -200,41 +179,6 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) return err; } -/* escape from zone, and don't brake, so we can continue with another - * traj */ -uint8_t strat_escape(struct build_zone *zone, uint8_t flags) -{ - uint8_t err; - uint16_t old_spdd, old_spda; - - strat_get_speed(&old_spdd, &old_spda); - - err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000); - if (err == 0) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -150); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - strat_set_speed(old_spdd, old_spda); - return err; - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - if (zone->flags & ZONE_F_DISC) { - trajectory_d_rel(&mainboard.traj, -350); - err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags); - } - else { - trajectory_d_rel(&mainboard.traj, -300); - err = wait_traj_end(flags); - } - - strat_set_speed(old_spdd, old_spda); - if (err == 0) - return END_NEAR; - return err; -} - static void strat_update_traj_speed(void) { uint16_t d, a; @@ -290,12 +234,6 @@ void strat_limit_speed(void) if (get_opponent_da(&opp_d, &opp_a) != 0) goto update; -#ifdef HOMOLOGATION - if (opp_d < 600) { - lim_d = 150; - lim_a = 200; - } -#else if (opp_d < 500) { if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { lim_d = SPEED_DIST_VERY_SLOW; @@ -310,7 +248,6 @@ void strat_limit_speed(void) lim_a = SPEED_ANGLE_VERY_SLOW; } } -#endif else if (opp_d < 800) { if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { lim_d = SPEED_DIST_SLOW; diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h index 27e1497..6ff0666 100644 --- a/projects/microb2010/mainboard/strat_base.h +++ b/projects/microb2010/mainboard/strat_base.h @@ -59,10 +59,6 @@ void strat_start(void); * times. */ uint8_t strat_goto_xy_force(int16_t x, int16_t y); -/* escape from disc polygon or another zone */ -struct build_zone; -uint8_t strat_escape(struct build_zone *zone, uint8_t flags); - /* return true if we have to brake due to an obstacle */ uint8_t strat_obstacle(void); diff --git a/projects/microb2010/mainboard/strat_building.c b/projects/microb2010/mainboard/strat_building.c deleted file mode 100644 index 9abdb5a..0000000 --- a/projects/microb2010/mainboard/strat_building.c +++ /dev/null @@ -1,907 +0,0 @@ -/* - * Copyright Droids, Microb Technology (2008) - * - * 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_building.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - -#include "../common/i2c_commands.h" -#include "main.h" -#include "cmdline.h" -#include "i2c_protocol.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_utils.h" -#include "strat_avoid.h" -#include "sensor.h" - - -#define DISC_DIST_NEED_GOTO_AVOID 1000 -#define DISC_DIST_PREPARE_BUILD 700 -#define DISC_DIST_SLOW 500 - -#define ERROUT(e) do { \ - err = e; \ - goto end; \ - } while(0) - -static uint8_t is_ready_for_prepare_build(void) -{ - double d, a; - if (distance_from_robot(CENTER_X, CENTER_Y) > - DISC_DIST_PREPARE_BUILD) - return 0; - abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); - if (a < RAD(-30)) - return 0; - if (a > RAD(30)) - return 0; - return 1; -} - -/* go to the nearest place on the disc. Also prepare the arms for - * building at the correct level. If level==-1, don't move the - * arms. */ -uint8_t strat_goto_disc(int8_t level) -{ - uint8_t err; - uint16_t old_spdd, old_spda; - double d, a, x, y; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - /* workaround for some static cols configurations */ - if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) { - if (time_get_s() > 15) - i2c_mechboard_mode_loaded(); - } - - /* if we are far from the disc, goto backward faster */ - abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); - if (d > DISC_DIST_NEED_GOTO_AVOID) { - rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y); - err = goto_and_avoid(x, y, TRAJ_FLAGS_STD, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - } - -#ifdef HOMOLOGATION - { - int16_t opp_d, opp_a; - trajectory_turnto_xy(&mainboard.traj, - CENTER_X, CENTER_Y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - time_wait_ms(500); - - err = get_opponent_da(&opp_d, &opp_a); - if (err == 0 && opp_d < 600 && - (opp_a > 325 || opp_a < 35)) - return END_ERROR; - } -#endif - - trajectory_goto_forward_xy_abs(&mainboard.traj, - CENTER_X, CENTER_Y); - err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(), - TRAJ_FLAGS_NO_NEAR); - - if (err == END_BLOCKING) - ERROUT(END_BLOCKING); - if (TRAJ_SUCCESS(err)) /* should not reach dest */ - ERROUT(END_ERROR); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - if (level != -1) - i2c_mechboard_mode_prepare_build_both(level); - - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X, - CENTER_Y) < DISC_DIST_SLOW, - TRAJ_FLAGS_NO_NEAR); - - if (err == END_BLOCKING) - ERROUT(END_BLOCKING); - if (TRAJ_SUCCESS(err)) /* should not reach dest */ - ERROUT(END_ERROR); - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - if (err == END_BLOCKING) - ERROUT(END_TRAJ); - if (TRAJ_SUCCESS(err)) /* should not reach dest */ - ERROUT(END_ERROR); - - ERROUT(err); - end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* must be called from the checkpoint before zone 1. */ -static uint8_t strat_goto_build_zone1_near(uint8_t level) -{ - uint8_t err; - - /* turn to build zone */ - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - return err; - - /* move forward to reach the build zone */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); - i2c_mechboard_mode_prepare_build_both(level); - err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); - if (err == END_BLOCKING) { - err = END_TRAJ; - } - - DEBUG(E_USER_STRAT, "build zone reached"); - return err; -} - -/* must be called from the checkpoint before zone 0 */ -static uint8_t strat_goto_build_zone0_near(uint8_t level) -{ - uint8_t err; -#ifdef OLD_STYLE - int16_t cur_y, diff_y, dst_y; -#endif - - /* turn to build zone */ - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - return err; - -#ifdef OLD_STYLE - cur_y = position_get_y_s16(&mainboard.pos); - dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100); - diff_y = ABS(cur_y - dst_y); - - /* move forward to reach the build zone */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); - i2c_mechboard_mode_prepare_build_both(level); - trajectory_d_rel(&mainboard.traj, diff_y); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err == END_BLOCKING) { /* not very good for z0 but... */ - err = END_TRAJ; - } -#else - /* move forward to reach the build zone */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); - i2c_mechboard_mode_prepare_build_both(level); - err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); - if (err == END_BLOCKING) { - err = END_TRAJ; - } -#endif - - DEBUG(E_USER_STRAT, "build zone reached"); - return err; -} - -/* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for - * now... */ -uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level) -{ - uint8_t err = END_TRAJ; - uint16_t old_spdd, old_spda; - int16_t checkpoint_x, checkpoint_y; - int16_t errx; - - zone->last_try_time = time_get_s(); - - if (zone->flags & ZONE_F_DISC) - return strat_goto_disc(level); - - DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x); - - /* workaround for some static cols configurations */ - if (time_get_s() > 15) - i2c_mechboard_mode_loaded(); - - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - checkpoint_x = zone->checkpoint_x; - checkpoint_y = COLOR_Y(zone->checkpoint_y); - errx = position_get_x_s16(&mainboard.pos) - checkpoint_x; - - /* goto checkpoint if we are too far from it, or if error on x - * is too big. */ - if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 || - ABS(errx) > 15) { - err = goto_and_avoid(checkpoint_x, checkpoint_y, - TRAJ_FLAGS_STD, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - } - - if (zone->flags & ZONE_F_ZONE1) - err = strat_goto_build_zone1_near(level); - else if (zone->flags & ZONE_F_ZONE0) - err = strat_goto_build_zone0_near(level); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* return a free temple structure */ -struct temple *strat_get_free_temple(void) -{ - uint8_t i; - - for (i=0; i col_r ? col_l : col_r); - lintel = (get_lintel_count() > 0); - - if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { - if ((temple->level_r > 5) && - (temple->flags & TEMPLE_F_ON_DISC)) - return 0; - } - - /* return symetric temples only */ - if (temple->level_l != temple->level_r) - return 0; - - if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME) - return 0; - - /* we could do better to work on non-symetric temples */ - if (temple->level_l + max_col + lintel > 9) - return 0; - - if (temple->flags & TEMPLE_F_MONOCOL) - return 0; - - /* XXX don't allow to build on opponent temple. For that we - * must support the little back_mm. */ - if (temple->flags & TEMPLE_F_OPPONENT) - return 0; - - return 1; -} - - -/* return the best existing temple for building */ -struct temple *strat_get_best_temple(void) -{ - uint8_t i; - struct temple *best = NULL; - struct temple *temple = NULL; - - for (i=0; iflags & TEMPLE_F_VALID)) - continue; - - if (strat_can_build_on_temple(temple) == 0) - continue; - - if (best == NULL) { - best = temple; - continue; - } - - /* take the higher temple between 'best' and 'temple' */ - if (best->level_l < temple->level_l) - best = temple; - } - - DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best); - return best; -} - -/* return the temple we built on the disc if any. If valid == 1, the - * temple must be buildable. */ -struct temple *strat_get_our_temple_on_disc(uint8_t valid) -{ - uint8_t i; - struct temple *temple = NULL; - - if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { - return NULL; - } - - for (i=0; iflags & TEMPLE_F_VALID)) - continue; - - if (valid == 1 && strat_can_build_on_temple(temple) == 0) - continue; - - if (temple->flags & TEMPLE_F_ON_DISC) - return temple; - } - return NULL; -} - -#define COL_MAX(x,y) (((x)>(y)) ? (x) : (y)) - -#define TIME_FOR_LINTEL 3000L -#define TIME_FOR_BUILD 0L -#define TIME_FOR_COL 800L -#define TIME_MARGIN 2000L - -#define CHECKPOINT_DISC_DIST 380 -#define CHECKPOINT_OTHER_DIST 200 -/* Grow a temple. It will update temple list. */ -uint8_t strat_grow_temple(struct temple *temple) -{ - double checkpoint_x, checkpoint_y; - uint8_t add_level = 0; - uint8_t do_lintel = 1; - uint8_t col_l, col_r, col_max; - uint8_t err; - uint16_t timeout; - - /* XXX temple must be symetric */ - uint8_t level = temple->level_l; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - if (temple->level_l >= 9) - return END_ERROR; - - if ( (temple->zone->flags & ZONE_F_ZONE1) || - (temple->zone->flags & ZONE_F_ZONE0) ) { - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -17); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - col_l = get_column_count_left(); - col_r = get_column_count_right(); - - if (time_get_s() < 75) { - /* make temple symetric: if we have 1 col on left and 2 cols - * on right, only build one on both sides. */ - if (col_l > col_r) { - col_r = col_l; - do_lintel = 0; - } - if (col_r > col_l) { - col_r = col_l; - do_lintel = 0; - } - if (get_lintel_count() == 0) - do_lintel = 0; - } - else if (col_l != col_r) - do_lintel = 0; - - if (col_l == 0 || col_r == 0) { - if (temple->flags & TEMPLE_F_LINTEL) - do_lintel = 0; - } - - if (col_l == 0 && col_r == 0 && do_lintel == 0) { - DEBUG(E_USER_STRAT, "nothing to do"); - return END_ERROR; - } - - add_level = do_lintel + col_l; - while (level + add_level > 9) { - if (do_lintel) { - do_lintel = 0; - add_level = do_lintel + col_l; - continue; - } - /* we know col_r and col_l are > 0 */ - col_r--; - col_l--; - } - - col_max = COL_MAX(col_r, col_l); - - /* Reduce nb elts if we don't have time */ - timeout = (!!col_max) * TIME_FOR_BUILD; - timeout += col_max * TIME_FOR_COL; - timeout += do_lintel * TIME_FOR_LINTEL; - if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) { - do_lintel = 0; - timeout -= TIME_FOR_LINTEL; - } - if ((timeout / 1000L) + time_get_s() > 89 && col_max) { - if (col_r > 0) - col_r--; - if (col_l > 0) - col_l--; - col_max--; - timeout -= TIME_FOR_COL; - } - - /* take a margin for timeout */ - timeout += (!!col_max) * TIME_MARGIN; - - if (col_l == 0 && col_r == 0 && do_lintel == 0) { - DEBUG(E_USER_STRAT, "nothing to do (2)"); - return END_ERROR; - } - - DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d", - level, col_l, level, col_r, do_lintel); - - i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST, - level, col_r, I2C_AUTOBUILD_DEFAULT_DIST, - do_lintel); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == - I2C_MECHBOARD_MODE_AUTOBUILD, 100); - err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != - I2C_MECHBOARD_MODE_AUTOBUILD, timeout); - if (err == 0) { - DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout); - temple->flags = 0; /* remove temple from list */ - return END_TRAJ; - } - else - DEBUG(E_USER_STRAT, "temple built"); - - /* position of the robot when build the new temple */ - temple->x = position_get_x_s16(&mainboard.pos); - temple->y = position_get_y_s16(&mainboard.pos); - temple->a = position_get_a_deg_s16(&mainboard.pos); - - /* checkpoint is a bit behind us */ - if (temple->zone->flags & ZONE_F_DISC) { - rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, - &checkpoint_x, &checkpoint_y); - } - else { - rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, - &checkpoint_x, &checkpoint_y); - } - temple->checkpoint_x = checkpoint_x; - temple->checkpoint_y = checkpoint_y; - - temple->level_l = level + add_level; - temple->dist_l = 0; - temple->angle_l = 0; - - temple->level_r = level + add_level; - temple->dist_r = 0; - temple->angle_r = 0; - - temple->flags = TEMPLE_F_VALID; - - if (distance_from_robot(CENTER_X, CENTER_Y) < 400) - temple->flags |= TEMPLE_F_ON_DISC; - - if (do_lintel) - temple->flags |= TEMPLE_F_LINTEL; - - /* we must push the temple */ - if ( ((temple->zone->flags & ZONE_F_ZONE1) || - (temple->zone->flags & ZONE_F_ZONE0)) && - level <= 1) { - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(level); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - /* Special case for big 3 */ - if (strat_infos.col_in_boobs) { - uint16_t old_spdd, old_spda; - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW); - DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__); - strat_infos.col_in_boobs = 0; - trajectory_d_rel(&mainboard.traj, -120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, - I2C_MECHBOARD_MODE_CLEAR); - WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L); - i2c_mechboard_mode_prepare_build_both(level + add_level); - time_wait_ms(800); - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - err = strat_grow_temple(temple); - strat_set_speed(old_spdd, old_spda); - return err; - } - - return END_TRAJ; -} - -#define COL_BACK_DIST 70 -#define COL_ANGLE 20 -#define COL_ARM_DIST 220 - -#define COL_BACK_DIST_ZONE1 35 -#define COL_ARM_DIST_ZONE1 230 -#define COL_ANGLE_ZONE1 19 - -static uint8_t try_build_col(uint8_t l, uint8_t r, - uint8_t lp, uint8_t rp, - uint8_t lvl) -{ - uint8_t max_lvl = lvl + r + l; - - if (l == 0 && r == 0) - return 0; - if (lp - l == 2 && rp - r == 0) - return 0; - if (lp - l == 0 && rp - r == 2) - return 0; - if (max_lvl > 9) - return 0; - if (max_lvl == 9 && rp == 2 && r == 1) - return 0; - return max_lvl; -} - -/* Grow a temple by building a column on it. It will update temple - * list. */ -uint8_t strat_grow_temple_column(struct temple *temple) -{ - uint16_t old_spdd, old_spda; - double checkpoint_x, checkpoint_y; - uint8_t add_level = 0; - uint8_t col_l, col_r; - uint8_t col_l_before, col_r_before; - uint8_t err; - int16_t a_abs, a; - uint8_t level = temple->level_l; - uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0; - uint8_t tmp_lvl; - int16_t col_arm_dist = COL_ARM_DIST; - int16_t col_back_dist = COL_BACK_DIST; - int16_t col_angle = COL_ANGLE; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - if (level >= 9) - return END_ERROR; - - strat_get_speed(&old_spdd, &old_spda); - - if ( (temple->zone->flags & ZONE_F_ZONE1) || - (temple->zone->flags & ZONE_F_ZONE0) ) { - if (level == 1) - col_arm_dist = COL_ARM_DIST_ZONE1; - col_back_dist = COL_BACK_DIST_ZONE1; - col_angle = COL_ANGLE_ZONE1; - } - - a_abs = position_get_a_deg_s16(&mainboard.pos); - - col_l_before = get_column_count_left(); - col_r_before = get_column_count_right(); - col_l = col_l_before; - col_r = col_r_before; - - /* check number of cols */ - for (col_l = 0; col_l < col_l_before + 1; col_l++) { - for (col_r = 0; col_r < col_r_before + 1; col_r++) { - tmp_lvl = try_build_col(col_l, col_r, - col_l_before, - col_r_before, level); - if (tmp_lvl > lvl_ok) { - lvl_ok = tmp_lvl; - col_l_ok = col_l; - col_r_ok = col_r; - } - } - } - - col_l = col_l_ok; - col_r = col_r_ok; - add_level = col_l + col_r; - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - - if (col_l == 0 && col_r == 0) - ERROUT(END_ERROR); - - DEBUG(E_USER_STRAT, "Build col: left=%d right=%d", - col_l, col_r); - - i2c_mechboard_mode_prepare_inside_both(level); - trajectory_d_rel(&mainboard.traj, -col_back_dist); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* build with left arm */ - if (col_l) { - a = a_abs - col_angle; - if (a < -180) - a += 360; - trajectory_a_abs(&mainboard.traj, a); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - if (time_get_s() > 88) - return END_TIMER; - - if (level >= 7 && get_column_count_left() == 2) - i2c_mechboard_mode_prepare_build_select(level+1, -1); - else - i2c_mechboard_mode_prepare_build_select(level, -1); - time_wait_ms(200); - i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist, - 0, 0, col_arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && - (col_r == 0) && - (temple->flags & TEMPLE_F_OPPONENT)) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE); - time_wait_ms(500); - trajectory_d_rel(&mainboard.traj, 100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - else if ((level == 1 || level == 0) && (col_r == 0)) { - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(level); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - i2c_mechboard_mode_prepare_inside_select(level+col_l, -1); - } - - /* build with right arm */ - if (col_r) { - a = a_abs + col_angle; - if (a > 180) - a -= 360; - trajectory_a_abs(&mainboard.traj, a); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - if (time_get_s() > 88) - return END_TIMER; - - if ((level+col_l) >= 7 && get_column_count_right() == 2) - i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1); - else - i2c_mechboard_mode_prepare_build_select(-1, level + col_l); - time_wait_ms(200); - i2c_mechboard_mode_autobuild(0, 0, col_arm_dist, - level + col_l, col_r, - col_arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && - (temple->flags & TEMPLE_F_OPPONENT)) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); - time_wait_ms(500); - trajectory_d_rel(&mainboard.traj, 100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - else if (level == 1 || level == 0) { - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(level); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r); - - } - - trajectory_a_abs(&mainboard.traj, a_abs); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* position of the robot when build the new temple */ - temple->x = position_get_x_s16(&mainboard.pos); - temple->y = position_get_y_s16(&mainboard.pos); - temple->a = position_get_a_deg_s16(&mainboard.pos); - - /* checkpoint is a bit behind us */ - if (temple->zone->flags | ZONE_F_DISC) { - rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, - &checkpoint_x, &checkpoint_y); - } - else { - rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, - &checkpoint_x, &checkpoint_y); - } - temple->checkpoint_x = checkpoint_x; - temple->checkpoint_y = checkpoint_y; - - temple->level_l = level + add_level; - temple->dist_l = 0; /* XXX */ - temple->angle_l = 0; - - temple->level_r = level + add_level; - temple->dist_r = 0; - temple->angle_r = 0; - - temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL; - - if (distance_from_robot(CENTER_X, CENTER_Y) < 400) - temple->flags |= TEMPLE_F_ON_DISC; - - if ( (temple->zone->flags & ZONE_F_ZONE1) || - (temple->zone->flags & ZONE_F_ZONE0) ) { - - } - return END_TRAJ; - end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -uint8_t strat_build_new_temple(struct build_zone *zone) -{ - struct temple *temple; - uint8_t level = zone->level; - uint8_t err; - - /* create a dummy temple */ - temple = strat_get_free_temple(); - if (!temple) - return END_ERROR; - - memset(temple, 0, sizeof(*temple)); - temple->level_l = level; - temple->level_r = level; - temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL; - temple->zone = zone; - - zone->flags |= ZONE_F_BUSY; - - if (time_get_s() > 50 && time_get_s() < 85 && - get_lintel_count() == 0) - err = strat_grow_temple_column(temple); - else - err = strat_grow_temple(temple); - - if (!TRAJ_SUCCESS(err)) - temple->flags = 0; - return err; -} - -uint8_t strat_goto_temple(struct temple *temple) -{ - uint16_t old_spdd, old_spda; - uint8_t err; - - DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d", - temple, temple->checkpoint_x, temple->checkpoint_y); - - temple->last_try_time = time_get_s(); - - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - i2c_mechboard_mode_loaded(); - - err = goto_and_avoid(temple->checkpoint_x, - temple->checkpoint_y, - TRAJ_FLAGS_STD, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - err = strat_goto_build_zone(temple->zone, temple->level_r); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - DEBUG(E_USER_STRAT, "zone reached"); - ERROUT(END_TRAJ); - - end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* return the best existing temple for building */ -struct build_zone *strat_get_best_zone(void) -{ - uint8_t i; - struct build_zone *zone = NULL; - - for (i=0; iflags & ZONE_F_BUSY) - continue; - if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME) - continue; - - return zone; - } - return NULL; -} diff --git a/projects/microb2010/mainboard/strat_column_disp.c b/projects/microb2010/mainboard/strat_column_disp.c deleted file mode 100644 index e0b481e..0000000 --- a/projects/microb2010/mainboard/strat_column_disp.c +++ /dev/null @@ -1,485 +0,0 @@ -/* - * Copyright Droids, Microb Technology (2009) - * - * 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_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "actuator.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_avoid.h" -#include "strat_utils.h" -#include "sensor.h" -#include "i2c_protocol.h" - -#define ERROUT(e) do { \ - err = e; \ - goto end; \ - } while(0) - -/* distance between the wheel axis and the IR sensor */ -#define IR_SHIFT_DISTANCE_RIGHT 85 -#define IR_SHIFT_DISTANCE_LEFT 95 - -/* return red or green sensor */ -#define COLOR_IR_SENSOR(left) \ - ({ \ - uint8_t __ret = 0; \ - if (left) \ - __ret = sensor_get(S_DISP_LEFT); \ - else \ - __ret = sensor_get(S_DISP_RIGHT); \ - \ - __ret; \ - }) \ - -/* eject one col, some error codes are ignored here: we want to be - * sure that the column is correctly ejected. */ -uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a) -{ - uint8_t err; - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -300); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - i2c_mechboard_mode_eject(); - time_wait_ms(600); - trajectory_a_abs(&mainboard.traj, eject_a); - - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - i2c_mechboard_mode_clear(); - time_wait_ms(1000); - trajectory_a_abs(&mainboard.traj, pickup_a); - - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - return err; -} - -/* get columns from dispenser. Must be called when the robot is in - * front of the dispenser. */ -static uint8_t strat_pickup_col_disp(struct column_dispenser *disp) -{ - uint16_t old_spdd, old_spda; - int16_t recalib_x, recalib_y; - int16_t eject_a, pickup_a; - uint8_t err, timeout = 0; - int8_t cols_count_before, cols_count_after, cols; - microseconds us; - uint8_t first_try = 1; - uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP; - - /* XXX set lazy pickup mode */ - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - strat_get_speed(&old_spdd, &old_spda); - - cols_count_before = get_column_count(); - pickup_a = COLOR_A(disp->pickup_a); - eject_a = COLOR_A(disp->eject_a); - recalib_x = disp->recalib_x; - recalib_y = COLOR_Y(disp->recalib_y); - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - - /* turn to dispenser */ - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - trajectory_a_abs(&mainboard.traj, pickup_a); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* go forward until blocking, then go back ~30mm */ - - pickup_wheels_on(); - retry: - if (time_get_s() > 86) { - DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); - return END_TIMER; - } - - if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) && - strat_infos.col_in_boobs == 0 && - strat_infos.lazy_pickup_done == 0) { - DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__); - pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - strat_infos.col_in_boobs = 1; - strat_infos.lazy_pickup_done = 1; - } - else { - pickup_mode = I2C_MECHBOARD_MODE_PICKUP; - strat_infos.col_in_boobs = 0; - } - - if (first_try) - i2c_mechboard_mode_lazy_harvest(); - else - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - first_try = 0; - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 120); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - - err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); - - trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(eject_a, pickup_a); - goto retry; - } - - /* start to pickup with finger / arms */ - - DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__); - - if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) - i2c_mechboard_mode_pickup(); - else - i2c_mechboard_mode_lazy_pickup(); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100); - us = time_get_us2(); - cols = get_column_count(); - while (get_mechboard_mode() == pickup_mode) { - if (get_column_count() != cols) { - cols = get_column_count(); - us = time_get_us2(); - } - if ((get_column_count() - cols_count_before) >= disp->count) { - DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__); - break; - } - /* 1 second timeout */ - if (time_get_us2() - us > 1000000L) { - DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__); - timeout = 1; - break; - } - } - - /* eject if we found a bad color column */ - - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(eject_a, pickup_a); - goto retry; - } - - /* only recalib if it was not a timeout or if we got at least - * 2 cols. */ - if (timeout == 0 || (get_column_count() - cols_count_before >= 2)) - strat_reset_pos(recalib_x, recalib_y, pickup_a); - else { - /* else just update x or y depending on disp */ - if (disp == &strat_infos.c1) - strat_reset_pos(recalib_x, DO_NOT_SET_POS, - DO_NOT_SET_POS); - else - strat_reset_pos(recalib_x, DO_NOT_SET_POS, - DO_NOT_SET_POS); - } - - /* go back */ - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -300); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - - /* update dispenser count */ - - cols_count_after = get_column_count(); - cols = cols_count_after - cols_count_before; - if (cols > 0) { - DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); - disp->count -= cols; - if (disp->count < 0) - disp->count = 0; - } - - pickup_wheels_off(); - if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) - i2c_mechboard_mode_clear(); - else - disp->count -= 2; - - ERROUT(END_TRAJ); - -end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* - * Go in front of a dispenser. It will update the dispenser if it is - * c2 or c3 if we detect that this dispenser does not exist. - */ -uint8_t strat_goto_col_disp(struct column_dispenser **pdisp) -{ - uint8_t err; - int16_t checkpoint_x, checkpoint_y; - int16_t scan_a; - uint16_t old_spdd, old_spda, scan_left; - int16_t pos1x, pos1y, pos2x, pos2y, pos, dist; - int16_t margin_col2, margin_col3; - struct column_dispenser *disp = *pdisp; - - if (disp->count <= 0) - return END_ERROR; - - if (disp->last_try_time >= time_get_s()) - return END_ERROR; - - disp->last_try_time = time_get_s(); - - strat_get_speed(&old_spdd, &old_spda); - - i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, - I2C_MECHBOARD_MODE_CLEAR); - - /* set some useful variables */ - checkpoint_x = disp->checkpoint_x; - checkpoint_y = COLOR_Y(disp->checkpoint_y); - scan_a = COLOR_A(disp->scan_a); - scan_left = COLOR_INVERT(disp->scan_left); - - /* goto checkpoint */ - DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d", - __FUNCTION__, disp->name, checkpoint_x, - checkpoint_y, scan_left); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - -#if 0 - /* we have an intermediate checkpoint if we are on our - * side. If goto_and_avoid() returns END_ERROR, skip - * this checkpoint. */ - if (position_get_x_s16(&mainboard.pos) < 1500) { - err = goto_and_avoid(1000, COLOR_Y(1500), - TRAJ_FLAGS_STD, - TRAJ_FLAGS_STD); - if (!TRAJ_SUCCESS(err) && err != END_ERROR) - ERROUT(err); - } -#endif - /* go to checkpoint near the dispenser */ - err = goto_and_avoid(checkpoint_x, checkpoint_y, - TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* turn to correct angle to prepare scanning */ - - trajectory_a_abs(&mainboard.traj, scan_a); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* scan now */ - - DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -1000); - err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left), - TRAJ_FLAGS_NO_NEAR); - if (err) /* we should not reach end */ - ERROUT(END_ERROR); - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - - err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left), - TRAJ_FLAGS_NO_NEAR); - if (err) - ERROUT(END_ERROR); - pos2x = position_get_x_s16(&mainboard.pos); - pos2y = position_get_y_s16(&mainboard.pos); - - dist = distance_between(pos1x, pos1y, pos2x, pos2y); - DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist); - - if (scan_left) - trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2); - else - trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - if (disp == &strat_infos.c1) - ERROUT(END_TRAJ); - - /* mark c2 or c3 as empty... */ - if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0) - ERROUT(END_TRAJ); - - pos = (pos2y + pos1y) / 2; - if (scan_a == 90) /* y is decreasing when scanning */ - pos -= 80; - else if (scan_a == -90) /* y is increasing when scanning */ - pos += 80; - - margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y)); - margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y)); - - if (margin_col3 > margin_col2) { - DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos); - strat_infos.c3.count = 0; - *pdisp = &strat_infos.c2; - if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time) - strat_infos.c2.last_try_time = strat_infos.c3.last_try_time; - } - else { - DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos); - strat_infos.c2.count = 0; - *pdisp = &strat_infos.c3; - if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time) - strat_infos.c3.last_try_time = strat_infos.c2.last_try_time; - } - ERROUT(END_TRAJ); - -end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* return the best dispenser between the 2 */ -static struct column_dispenser * -strat_disp_compare(struct column_dispenser *a, - struct column_dispenser *b) -{ - uint8_t want_cols = 4 - get_column_count(); - - DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols); - - /* an empty dispenser is not valid */ - if (a->count == 0) - return b; - if (b->count == 0) - return a; - - /* try to do a round robin: this is not optimal, but at least - * we will try another dispenser when one fails. */ - if (a->last_try_time < b->last_try_time) { - return a; - } - if (b->last_try_time < a->last_try_time) { - return b; - } - - /* take the one with the most columns */ - if (a->count >= want_cols && b->count < want_cols) - return a; - - /* take the one with the most columns */ - if (b->count >= want_cols && a->count < want_cols) - return b; - - /* the closer is the better */ - if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) < - distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) { - return a; - } - return b; -} - -/* choose the best dispenser */ -static struct column_dispenser *strat_get_best_col_disp(void) -{ - struct column_dispenser *disp; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - /* for the first call, use c3 */ - if (strat_infos.c1.last_try_time == 0 && - strat_infos.c2.last_try_time == 0 && - strat_infos.c3.last_try_time == 0) - return &strat_infos.c2; // XXX c3 - - DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__); - - /* else compare with standard conditions */ - disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2); - disp = strat_disp_compare(disp, &strat_infos.c3); - - if (disp->count == 0) - return NULL; - - return disp; -} - -/* choose the best dispenser, depending on disp count, distance, - * tries, ... and go pickup on it. */ -uint8_t strat_pickup_columns(void) -{ - struct column_dispenser *disp; - uint8_t err; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - disp = strat_get_best_col_disp(); - - if (disp == NULL) { - DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__); - return END_ERROR; - } - - err = strat_goto_col_disp(&disp); - if (!TRAJ_SUCCESS(err)) - return err; - - err = strat_pickup_col_disp(disp); - if (!TRAJ_SUCCESS(err)) - return err; - - return END_TRAJ; -} diff --git a/projects/microb2010/mainboard/strat_lintel.c b/projects/microb2010/mainboard/strat_lintel.c deleted file mode 100644 index 72ab299..0000000 --- a/projects/microb2010/mainboard/strat_lintel.c +++ /dev/null @@ -1,247 +0,0 @@ -/* - * Copyright Droids, Microb Technology (2008) - * - * 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_lintel.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_avoid.h" -#include "strat_utils.h" -#include "sensor.h" -#include "i2c_protocol.h" - -#define ERROUT(e) do { \ - err = e; \ - goto end; \ - } while(0) - -#define X_PRE_MARGIN 20 -#define X_POST_MARGIN 10 - -/* - * goto lintel disp. Return END_TRAJ if success or if there is nothing - * to do. Return END_ERROR if dest cannot be reached, else, it may - * return END_OBSTACLE or END_BLOCKING. - */ -uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp) -{ - uint8_t err, first_try = 1, right_ok, left_ok; - uint16_t old_spdd, old_spda; - int16_t left_cur, right_cur, a; - - if (disp->count == 0) - return END_ERROR; - - if (get_lintel_count() == 2) - return END_ERROR; - - if (disp->last_try_time >= time_get_s()) - return END_ERROR; - - disp->last_try_time = time_get_s(); - - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name); - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - - err = goto_and_avoid_backward(disp->x, COLOR_Y(400), - TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - trajectory_a_abs(&mainboard.traj, COLOR_A(-90)); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - if (time_get_s() > 86) { - DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); - return END_TIMER; - } - - i2c_mechboard_mode_prepare_get_lintel(); - retry: - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); - if (err == END_BLOCKING) { - a = position_get_a_deg_s16(&mainboard.pos); - /* only reset pos if angle is not too different */ - if (ABS(a - COLOR_A(-90)) < 5) - strat_reset_pos(DO_NOT_SET_POS, - COLOR_Y(ROBOT_LENGTH/2), - COLOR_A(-90)); - } - else if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - i2c_mechboard_mode_get_lintel(); - time_wait_ms(500); - - left_cur = sensor_get_adc(ADC_CSENSE3); - left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); - right_cur = mechboard.pump_right1_current; - right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); - - DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__, - left_ok, left_cur, right_ok, right_cur); - if (first_try) { - if (!right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - /* XXX recalib x ? */ - else if (right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -200, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, 190, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - else if (!right_ok && left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -200, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, 190, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - /* else, lintel is ok */ - else { - strat_infos.taken_lintel ++; - i2c_mechboard_mode_put_lintel(); - } - } - else { - if (right_ok && left_ok) { - /* lintel is ok */ - strat_infos.taken_lintel ++; - i2c_mechboard_mode_put_lintel(); - } - else { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - } - disp->count--; - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -250); - err = wait_traj_end(TRAJ_FLAGS_STD); - - ERROUT(err); - -end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* go pickup lintels on dispensers. Return END_TRAJ on success or if - - * there is nothing to do, else return the error status. */ -uint8_t strat_pickup_lintels(void) -{ - uint8_t err; - - if (get_column_count() != 0) - return END_ERROR; - - if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0) - return END_TRAJ; - - /* skip if it's too early */ - if (time_get_s() < strat_infos.conf.lintel_min_time) - return END_TRAJ; - - /* skip next lintel if we want only one */ - if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { - if (strat_infos.taken_lintel) - return END_TRAJ; - } - - /* don't take lintel now if we already have one and if there - * is not much time */ - if (get_lintel_count() && time_get_s() > 75) - return END_TRAJ; - - /* take lintel 1 */ - err = strat_goto_lintel_disp(&strat_infos.l1); - if (!TRAJ_SUCCESS(err) && err != END_ERROR) - return err; - - /* skip next lintel if we want only one */ - if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { - if (strat_infos.taken_lintel) - return END_TRAJ; - } - - /* don't take lintel now if we already have one and if there - * is not much time */ - if (get_lintel_count() && time_get_s() > 75) - return END_TRAJ; - - /* take lintel 2 */ - err = strat_goto_lintel_disp(&strat_infos.l2); - if (!TRAJ_SUCCESS(err) && err != END_ERROR) - return err; - - return END_TRAJ; -} diff --git a/projects/microb2010/mainboard/strat_scan.c b/projects/microb2010/mainboard/strat_scan.c deleted file mode 100644 index 4140226..0000000 --- a/projects/microb2010/mainboard/strat_scan.c +++ /dev/null @@ -1,689 +0,0 @@ -/* - * Copyright Droids, Microb Technology (2009) - * - * 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_scan.c,v 1.2 2009-11-08 17:24:33 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - -#include "../common/i2c_commands.h" -#include "main.h" -#include "cmdline.h" -#include "i2c_protocol.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_utils.h" -#include "strat_avoid.h" -#include "sensor.h" - -#define ERROUT(e) do { \ - err = e; \ - goto end; \ - } while(0) - - -void scanner_dump_state(void) -{ - uint8_t status; - - printf_P(PSTR("scanner state:\r\n")); - status = sensorboard.scan_status; - - printf_P(PSTR(" status=%x: "), sensorboard.scan_status); - - if (status & I2C_SCAN_DONE) - printf_P(PSTR("DONE ")); - else - printf_P(PSTR("RUNNING ")); - if (status & I2C_SCAN_MAX_COLUMN) - printf_P(PSTR("OBSTACLE ")); - - printf_P(PSTR("\r\n")); - - if (sensorboard.dropzone_h == -1) { - printf_P(PSTR("No zone found\r\n")); - return; - } - - printf_P(PSTR(" column_h=%d\r\n"), sensorboard.dropzone_h); - printf_P(PSTR(" column_x=%d\r\n"), sensorboard.dropzone_x); - printf_P(PSTR(" column_y=%d\r\n"), sensorboard.dropzone_y); -} - -/* must be larger than the disc poly */ -#define CHECKPOINT_DIST 600 - -/* go to a specific angle on disc, if level == -1, don't move arms */ -uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level) -{ - uint8_t err; - uint16_t old_spdd, old_spda; - double x, y; - uint8_t need_clear = 0; - - DEBUG(E_USER_STRAT, "%s(a_deg=%d, level=%d)", __FUNCTION__, - a_deg, level); - - strat_get_speed(&old_spdd, &old_spda); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - /* workaround for some static cols configurations */ - if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) { - if (time_get_s() > 15) - i2c_mechboard_mode_loaded(); - } - /* another workaround for offensive configuration */ - else { - if (strat_infos.i2c_loaded_skipped == 0) { - DEBUG(E_USER_STRAT, "%s() need clear"); - strat_infos.i2c_loaded_skipped = 1; - i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, - I2C_MECHBOARD_MODE_CLEAR); - need_clear = 1; - } - else - i2c_mechboard_mode_loaded(); - } - - - /* calculate the checkpoint */ - x = CHECKPOINT_DIST; - y = 0; - rotate(&x, &y, RAD(a_deg)); - x += CENTER_X; - y += CENTER_Y; - - err = goto_and_avoid(x, y, TRAJ_FLAGS_STD, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* early offensive conf only */ - if (need_clear) { - err = WAIT_COND_OR_TIMEOUT(get_column_count() == 2, - 3000L); - DEBUG(E_USER_STRAT, "%s() offensive: err=%d", err); - if (err == 0) /* timeout */ - return END_ERROR; - } - err = strat_goto_disc(level); - - end: - strat_set_speed(old_spdd, old_spda); - return err; - -} - -/* only valid for temple on disc */ -int16_t strat_get_temple_angle(struct temple *temple) -{ - int16_t x, y; - double a; - - x = temple->x; - y = temple->y; - x -= CENTER_X; - y -= CENTER_Y; - a = atan2(y, x); - return DEG(a); -} - -#define SCAN_ANGLE_OFFSET (-40) -int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle) -{ - return temple_angle + SCAN_ANGLE_OFFSET; -} - -/* start to scan after this distance */ -#define DIST_START_SCAN 50 - -/* scan during this distance (includes DIST_START_SCAN) */ -#define DIST_SCAN 430 - -/* speed of the scan */ -#define SPEED_SCAN 450 - -/* from scanner point of view */ -#define DISC_CENTER_X 15 -#define DISC_CENTER_Y 13 - -/* distance of the checkpoint */ -#define CKPT_DST 550. - -/* to convert in robot coordinates */ -#define SIDE_OFFSET (ROBOT_WIDTH/2) -#define DIST_OFFSET (DIST_SCAN - DIST_START_SCAN) - -/* center of the disc in robot coordinates */ -#define CENTER_X_SCANNER 166 -#define CENTER_Y_SCANNER 174 - -/* center of the disc in scanner millimeters coordinates */ -#define CENTER_X_SCANNER2 120 -#define CENTER_Y_SCANNER2 155 - -/* structure filled by strat_scan_disc() */ -struct scan_disc_result { -#define SCAN_FAILED 0 -#define SCAN_VALID 1 - uint8_t status; - -#define SCAN_ACTION_BUILD_TEMPLE 0 -#define SCAN_ACTION_BUILD_COL 1 - uint8_t action; - - uint8_t level; -}; - -#define SCAN_MODE_CHECK_TEMPLE 0 -#define SCAN_MODE_SCAN_COL 1 -#define SCAN_MODE_SCAN_TEMPLE 2 - -int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x, - int16_t *ckpt_rel_y, int16_t *back_mm) -{ - int16_t center_rel_x, center_rel_y; - int16_t col_rel_x, col_rel_y; - int16_t col_vect_x, col_vect_y; - double col_vect_norm; - int16_t ckpt_vect_x, ckpt_vect_y; - - /* do some filtering */ - if (mode == SCAN_MODE_SCAN_TEMPLE && - sensorboard.dropzone_x > CENTER_X_SCANNER) { - DEBUG(E_USER_STRAT, "x too big"); - return -1; - } - - /* process relative pos from robot point of view */ - center_rel_x = DIST_OFFSET - CENTER_Y_SCANNER; - center_rel_y = -(SIDE_OFFSET + CENTER_X_SCANNER); - - col_rel_x = DIST_OFFSET - sensorboard.dropzone_y; - col_rel_y = -(SIDE_OFFSET + sensorboard.dropzone_x); - DEBUG(E_USER_STRAT, "col_rel = %d,%d", col_rel_x, col_rel_y); - - /* vector from center to column */ - col_vect_x = col_rel_x - center_rel_x; - col_vect_y = col_rel_y - center_rel_y; - col_vect_norm = norm(col_vect_x, col_vect_y); - - /* vector from center to ckpt */ - ckpt_vect_x = (double)(col_vect_x) * CKPT_DST / col_vect_norm; - ckpt_vect_y = (double)(col_vect_y) * CKPT_DST / col_vect_norm; - - /* rel pos of ckpt */ - *ckpt_rel_x = center_rel_x + ckpt_vect_x; - *ckpt_rel_y = center_rel_y + ckpt_vect_y; - - /* do some filtering */ - if (col_vect_norm > 150 || col_vect_norm < 30) { - DEBUG(E_USER_STRAT, "bad norm"); - return -1; - } - - if (mode == SCAN_MODE_SCAN_TEMPLE) { - if (col_vect_norm > 50) { - *back_mm = ABS(col_vect_norm-50); - } - } - return 0; -} - -/* - * scan the disc: return END_TRAJ on success (and status in result is - * set to SCAN_VALID). In this case, all the scan_disc_result - * structure is filled with appropriate parameters. mode can be - * 'check' or 'scan_col'. Note that if we do a check_temple, the level - * field in structure must be filled first by the caller. - */ -uint8_t strat_scan_disc(int16_t angle, uint8_t mode, - struct scan_disc_result *result) -{ - uint16_t old_spdd, old_spda; - uint8_t err, stop_scanner = 0; - uint8_t original_mode = mode; - int16_t pos1x, pos1y, dist; - int16_t back_mm = 0; - - int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; - - double center_abs_x, center_abs_y; - double ckpt_rel_d, ckpt_rel_a; - double ckpt_abs_x, ckpt_abs_y; - - /* mark status as failed for now */ - result->status = SCAN_FAILED; - - DEBUG(E_USER_STRAT, "%s(angle=%d)", __FUNCTION__, angle); - - strat_get_speed(&old_spdd, &old_spda); - - /* go on disc */ - err = strat_goto_disc_angle(angle, -1); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* wait opponent before scanning */ - if (strat_infos.conf.wait_opponent > 0) { - int16_t opp_x, opp_y, opp_d, opp_a; - int8_t err; - microseconds us; - - us = time_get_us2(); - while ((err = get_opponent_xyda(&opp_x, &opp_y, - &opp_d, &opp_a)) == 0) { - if (opp_d > 600) - break; - if (opp_a < 180) - break; - - if (time_get_us2() - us >= (uint32_t)strat_infos.conf.wait_opponent * 1000000L) - return END_ERROR; - } - } - - /* save absolute position of disc */ - rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); - - strat_limit_speed_disable(); - - /* go back and prepare to scan */ - strat_set_speed(1000, 1000); - trajectory_d_a_rel(&mainboard.traj, -140, 130); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - /* XXX check that opp is not behind us */ - - /* prepare scanner */ - - stop_scanner = 1; - i2c_sensorboard_scanner_prepare(); - time_wait_ms(250); /* XXX to remove ? */ - - strat_set_speed(SPEED_SCAN, 1000); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - trajectory_d_rel(&mainboard.traj, -DIST_SCAN); - - while (1) { - err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err != 0) - break; - - dist = distance_from_robot(pos1x, pos1y); - - if (dist > DIST_START_SCAN) - break; - - if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { - err = END_ERROR; - break; - } - } - - if (err) { - if (TRAJ_SUCCESS(err)) - err = END_ERROR; /* should not reach end */ - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - ERROUT(err); - } - - /* start the scanner */ - - i2c_sensorboard_scanner_start(); - - err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, - TRAJ_FLAGS_NO_NEAR); - if (err == 0) - err = END_ERROR; - if (!TRAJ_SUCCESS(err)) { - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - ERROUT(err); - } - - wait_scan_done(1000); - - i2c_sensorboard_scanner_stop(); - stop_scanner = 0; - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - i2c_sensorboard_scanner_algo_check(result->level, - CENTER_X_SCANNER2, - CENTER_Y_SCANNER2); - i2cproto_wait_update(); - wait_scan_done(1000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 && - !(strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)) { - DEBUG(E_USER_STRAT, "-- try to build a temple"); - mode = SCAN_MODE_SCAN_TEMPLE; - } - else { - result->action = SCAN_ACTION_BUILD_TEMPLE; - /* level is already set by caller */ - } - } - - if (mode == SCAN_MODE_SCAN_TEMPLE) { - i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, - DISC_CENTER_X, - DISC_CENTER_Y); - i2cproto_wait_update(); - wait_scan_done(1000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - if (original_mode != SCAN_MODE_CHECK_TEMPLE) { - DEBUG(E_USER_STRAT, "-- try to build a column"); - mode = SCAN_MODE_SCAN_COL; - } - else { - DEBUG(E_USER_STRAT, "-- check failed"); - } - } - else { - result->action = SCAN_ACTION_BUILD_TEMPLE; - result->level = sensorboard.dropzone_h; - } - } - - if (mode == SCAN_MODE_SCAN_COL) { - i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, - DISC_CENTER_X, - DISC_CENTER_Y); - i2cproto_wait_update(); - wait_scan_done(1000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - ERROUT(END_ERROR); - } - else { - result->action = SCAN_ACTION_BUILD_COL; - result->level = sensorboard.dropzone_h; - } - } - - if (sensorboard.dropzone_h == -1) { - ERROUT(END_ERROR); - } - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - ckpt_rel_x = 220; - ckpt_rel_y = 100; - } - - DEBUG(E_USER_STRAT, "rel xy for ckpt is %d,%d", ckpt_rel_x, ckpt_rel_y); - - rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); - abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); - - DEBUG(E_USER_STRAT, "abs ckpt is %2.2f,%2.2f", ckpt_abs_x, ckpt_abs_y); - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - /* intermediate checkpoint for some positions */ - if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { - trajectory_goto_xy_rel(&mainboard.traj, 200, 100); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - } - - trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - if (result->action == SCAN_ACTION_BUILD_TEMPLE) { - i2c_mechboard_mode_prepare_build_both(result->level); - } - - trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, - TRAJ_FLAGS_SMALL_DIST); - if (err == 0) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - if (TRAJ_SUCCESS(err)) - err = END_ERROR; /* should not reach end */ - if (err != END_BLOCKING && !TRAJ_SUCCESS(err)) - ERROUT(err); - - if (back_mm) { - trajectory_d_rel(&mainboard.traj, -back_mm); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - - result->status = SCAN_VALID; - - strat_limit_speed_enable(); - return END_TRAJ; - - end: - if (stop_scanner) - i2c_sensorboard_scanner_stop(); - strat_limit_speed_enable(); - strat_set_speed(old_spdd, old_spda); - return err; - -} - -/* do action according to scanner result. temple argument can be NULL - * if it's a new one (from opponent) or it can be our previous - * temple. */ -uint8_t strat_scan_do_action(struct scan_disc_result *scan_result, - struct temple *temple, struct build_zone *zone) -{ - uint8_t err; - - /* remove the temple from the list */ - if (scan_result->status != SCAN_VALID) - return END_ERROR; - - if (temple) { - /* we were scanning a temple, remove it */ - if (scan_result->level != temple->level_l) { - temple->flags = 0; - temple = NULL; - } - } - - if (temple == NULL) { - temple = strat_get_free_temple(); - if (temple == NULL) - return END_ERROR; - memset(temple, 0, sizeof(*temple)); - temple->level_l = scan_result->level; - temple->level_r = scan_result->level; - temple->flags = TEMPLE_F_OPPONENT | - TEMPLE_F_VALID | TEMPLE_F_LINTEL; - temple->zone = zone; - } - zone->flags |= ZONE_F_BUSY; - - switch (scan_result->action) { - - case SCAN_ACTION_BUILD_COL: - err = strat_grow_temple_column(temple); - break; - - case SCAN_ACTION_BUILD_TEMPLE: - err = strat_grow_temple(temple); - break; - default: - err = END_TRAJ; - break; - } - if (!TRAJ_SUCCESS(err)) - temple->flags = 0; - return err; -} - -uint8_t strat_build_on_opponent_temple(void) -{ - struct temple *temple; - uint8_t err; - struct scan_disc_result scan_result; - int16_t temple_angle; - - if (time_get_s() < strat_infos.conf.scan_opp_min_time) - return END_TRAJ; - - strat_infos.conf.scan_opp_min_time = - time_get_s() + strat_infos.conf.delay_between_opp_scan; - - /* scan on disc only */ - if (strat_infos.conf.scan_opp_angle == -1) { - temple = strat_get_our_temple_on_disc(0); - - /* scan the opposite of our temple if we found - * one on disc */ - if (temple) { - temple_angle = strat_get_temple_angle(temple); - temple_angle += 180; - if (temple_angle > 180) - temple_angle -= 360; - } - /* else scan at 0 deg (opponent side) */ - else { - temple_angle = 0; - } - } - else { - /* user specified scan position */ - temple_angle = strat_infos.conf.delay_between_opp_scan; - if (temple_angle > 180) - temple_angle -= 360; - } - temple_angle = strat_temple_angle_to_scan_angle(temple_angle); - - - err = strat_scan_disc(temple_angle, SCAN_MODE_SCAN_TEMPLE, - &scan_result); - if (!TRAJ_SUCCESS(err)) - return err; - - /* XXX on disc only */ - err = strat_scan_do_action(&scan_result, NULL, - &strat_infos.zone_list[0]); - - if (!TRAJ_SUCCESS(err)) - return err; - - err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD); - return err; -} - -uint8_t strat_check_temple_and_build(void) -{ - struct temple *temple; - uint8_t err; - struct scan_disc_result scan_result; - int16_t temple_angle; - - if (time_get_s() < strat_infos.conf.scan_our_min_time) - return END_TRAJ; - strat_infos.conf.scan_our_min_time = - time_get_s() + strat_infos.conf.delay_between_our_scan; - - /* on disc only, symetric only */ - temple = strat_get_our_temple_on_disc(1); - if (temple == NULL) - return END_TRAJ; - - temple_angle = strat_get_temple_angle(temple); - temple_angle = strat_temple_angle_to_scan_angle(temple_angle); - - scan_result.level = temple->level_l; - err = strat_scan_disc(temple_angle, SCAN_MODE_CHECK_TEMPLE, - &scan_result); - if (scan_result.status != SCAN_VALID) { - temple->flags = 0; - temple = NULL; - } - /* no column after a temple check */ - else if (scan_result.action == SCAN_ACTION_BUILD_COL && - time_get_s() < 70) - err = END_ERROR; - if (!TRAJ_SUCCESS(err)) - return err; - - err = strat_scan_do_action(&scan_result, temple, - temple->zone); - if (!TRAJ_SUCCESS(err)) - return err; - - err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD); - return err; -} diff --git a/projects/microb2010/mainboard/strat_static_columns.c b/projects/microb2010/mainboard/strat_static_columns.c deleted file mode 100644 index 7af6043..0000000 --- a/projects/microb2010/mainboard/strat_static_columns.c +++ /dev/null @@ -1,416 +0,0 @@ -/* - * Copyright Droids, Microb Technology (2008) - * - * 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_static_columns.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ - * - * Olivier MATZ - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "../common/i2c_commands.h" -#include "main.h" -#include "strat.h" -#include "strat_base.h" -#include "strat_utils.h" -#include "strat_avoid.h" -#include "sensor.h" -#include "i2c_protocol.h" - -#define ERROUT(e) do { \ - err = e; \ - goto end; \ - } while(0) - -#define BIG_DIST 5000 - -/* - * must be called from start area. - * get 4 static columns and build a temple on the disc - */ -uint8_t strat_static_columns(uint8_t configuration) -{ - uint8_t err; - uint8_t col1_present = 0, col4_present = 0; - uint16_t old_spdd, old_spda; - - DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration); - - strat_get_speed(&old_spdd, &old_spda); - - /* calibrate scanner */ - i2c_sensorboard_scanner_calib(); - - i2c_mechboard_mode_harvest(); - - /* go straight. total distance is less than 5 meters */ - strat_set_speed(1000, 1000); - trajectory_d_rel(&mainboard.traj, BIG_DIST); - - /* when y > 50, break */ - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - - /* turn to 90° abs while going forward */ - DEBUG(E_USER_STRAT, "turn now"); - strat_set_speed(1000, 350); - trajectory_only_a_abs(&mainboard.traj, COLOR_A(90)); - - /* when y > 100, check the presence of column 4 */ - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) - col4_present = 1; - if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) - col4_present = 1; - - /* when y > 120, check the presence of column 1 */ - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) - col1_present = 1; - if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) - col1_present = 1; - - /* when y > 130, break */ - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - - strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE; - - DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present); - DEBUG(E_USER_STRAT, "have %d cols", get_column_count()); - - if (configuration == 0) { - if (get_column_count() > 2) { - configuration = 1; - if (col4_present || col1_present) { - strat_infos.s_cols.flags |= - STATIC_COL_LINE2_DONE; - } - else { - strat_infos.s_cols.flags |= - STATIC_COL_LINE1_DONE; - } - } - - /* only 2 colums on the first line */ - else { - /* all other colums are on line 1 */ - if (col4_present && col1_present) { - configuration = 2; - strat_infos.s_cols.flags |= - STATIC_COL_LINE2_DONE; - } - - /* only 2 columns on line 1, so there are also - * 2 on line 2 */ - else if (col4_present || col1_present) { - configuration = 4; - strat_infos.s_cols.flags |= - STATIC_COL_LINE2_DONE; - } - - /* all other columns are on line 2 */ - else { - configuration = 3; - strat_infos.s_cols.flags |= - STATIC_COL_LINE1_DONE; - } - } - } - - strat_infos.s_cols.configuration = configuration; - DEBUG(E_USER_STRAT, "use configuration %d", configuration); - - if (configuration == 1) { - /* we already got 4 columns, go to the disc directly */ - - strat_set_speed(1500, 900); - trajectory_only_a_abs(&mainboard.traj, COLOR_A(0)); - err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD); - - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - } - else if (configuration == 2 /* go from line 0 to line 1 */) { - strat_set_speed(800, 1000); - /* relative is needed here */ - trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); - err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - strat_set_speed(1000, 600); - err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100), - TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - } - else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns - on line 2*/) { - strat_set_speed(1000, 600); - /* relative is needed here */ - trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); - err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - } - else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns - on line 2 */) { - strat_set_speed(1000, 600); - /* relative is needed here */ - trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); - err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - } - else { - trajectory_stop(&mainboard.traj); - } - - ERROUT(END_TRAJ); - - end: - strat_set_speed(old_spdd, old_spda); - return err; -} - -/* - * get last 2 columns - * must be called after the first temple building - */ -uint8_t strat_static_columns_pass2(void) -{ - uint16_t old_spdd, old_spda; - uint8_t side, err, next_mode; - - DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); - - strat_get_speed(&old_spdd, &old_spda); - - if (get_color() == I2C_COLOR_RED) - side = I2C_RIGHT_SIDE; - else - side = I2C_LEFT_SIDE; - - if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2) - next_mode = I2C_MECHBOARD_MODE_STORE; - else - next_mode = I2C_MECHBOARD_MODE_HARVEST; - - switch (strat_infos.s_cols.configuration) { - - /* configuration 1: 4 cols on line 0 */ - case 1: - if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) { - /* go on line 2 */ - - strat_set_speed(2000, 700); - trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - i2c_mechboard_mode_prepare_pickup_next(side, - next_mode); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_goto_forward_xy_abs(&mainboard.traj, - LINE2_X, - COLOR_Y(400)); - err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - } - else { - /* go on line 1 */ - strat_set_speed(2000, 700); - trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - i2c_mechboard_mode_prepare_pickup_next(side, - next_mode); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - - err = goto_and_avoid_forward(LINE1_X, - COLOR_Y(400), - TRAJ_FLAGS_NO_NEAR, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - } - - ERROUT(END_TRAJ); - break; - - /* configuration 2: 2 cols on line 0, - all other colums are on line 1 */ - case 2: - /* go on line 1 */ - strat_set_speed(2000, 700); - trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - i2c_mechboard_mode_prepare_pickup_next(side, - next_mode); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - - err = goto_and_avoid_forward(COL10_X, COLOR_Y(400), - TRAJ_FLAGS_NO_NEAR, - TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - ERROUT(END_TRAJ); - break; - - /* configuration 3: 2 cols on line 0, - all other colums are on line 2 */ - case 3: - /* go on line 2 */ - strat_set_speed(2000, 700); - trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - i2c_mechboard_mode_prepare_pickup_next(side, - next_mode); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - - trajectory_goto_forward_xy_abs(&mainboard.traj, - LINE2_X, - COLOR_Y(400)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - ERROUT(END_TRAJ); - break; - - /* configuration 4: 2 cols on line 0, - 2 on line 1, 2 on line 2 */ - case 4: - /* go on line 1 */ - strat_set_speed(600, 2000); - trajectory_d_a_rel(&mainboard.traj, -BIG_DIST, - COLOR_A(-135)); - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900), - TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - i2c_mechboard_mode_prepare_pickup_next(side, - next_mode); - - strat_set_speed(2000, 2000); - trajectory_d_rel(&mainboard.traj, -BIG_DIST); - err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100), - TRAJ_FLAGS_STD); - if (TRAJ_SUCCESS(err)) /* we should not reach end */ - ERROUT(END_ERROR); - else if (err) - ERROUT(err); - - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - ERROUT(err); - - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - err = goto_and_avoid_forward(LINE1_X, - COLOR_Y(400), - TRAJ_FLAGS_NO_NEAR, - TRAJ_FLAGS_NO_NEAR); - ERROUT(END_TRAJ); - break; - - default: - break; - } - - /* should not reach this point */ - ERROUT(END_ERROR); - - end: - strat_set_speed(old_spdd, old_spda); - return err; -} diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 81a12d6..b1bf942 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -86,18 +86,6 @@ int16_t simple_modulo_360(int16_t a) return a; } -/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ -double simple_modulo_2pi(double a) -{ - if (a < -M_PI) { - a += M_2PI; - } - else if (a > M_PI) { - a -= M_2PI; - } - return a; -} - /* return the distance to a point in the area */ int16_t angle_abs_to_rel(int16_t a_abs) { @@ -182,14 +170,6 @@ uint8_t robot_is_in_area(int16_t margin) margin); } -/* return true if we are near the disc */ -uint8_t robot_is_near_disc(void) -{ - if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) - return 1; - return 0; -} - /* return 1 or 0 depending on which side of a line (y=cste) is the * robot. works in red or green color. */ uint8_t y_is_more_than(int16_t y) @@ -286,9 +266,10 @@ uint8_t get_opponent_color(void) int8_t get_opponent_xy(int16_t *x, int16_t *y) { uint8_t flags; + return -1; // XXX IRQ_LOCK(flags); - *x = sensorboard.opponent_x; - *y = sensorboard.opponent_y; +/* *x = ballboard.opponent_x; */ +/* *y = ballboard.opponent_y; */ IRQ_UNLOCK(flags); if (*x == I2C_OPPONENT_NOT_THERE) return -1; @@ -300,10 +281,11 @@ int8_t get_opponent_da(int16_t *d, int16_t *a) { uint8_t flags; int16_t x_tmp; + return -1; // XXX IRQ_LOCK(flags); - x_tmp = sensorboard.opponent_x; - *d = sensorboard.opponent_d; - *a = sensorboard.opponent_a; +/* x_tmp = ballboard.opponent_x; */ +/* *d = ballboard.opponent_d; */ +/* *a = ballboard.opponent_a; */ IRQ_UNLOCK(flags); if (x_tmp == I2C_OPPONENT_NOT_THERE) return -1; @@ -314,100 +296,24 @@ 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) { uint8_t flags; + return -1; // XXX IRQ_LOCK(flags); - *x = sensorboard.opponent_x; - *y = sensorboard.opponent_y; - *d = sensorboard.opponent_d; - *a = sensorboard.opponent_a; +/* *x = ballboard.opponent_x; */ +/* *y = ballboard.opponent_y; */ +/* *d = ballboard.opponent_d; */ +/* *a = ballboard.opponent_a; */ IRQ_UNLOCK(flags); if (*x == I2C_OPPONENT_NOT_THERE) return -1; return 0; } - -uint8_t pump_left1_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && - (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_left2_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && - (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_right1_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && - (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_right2_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && - (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -/* number of column owned by the robot */ -uint8_t get_column_count_left(void) -{ - uint8_t ret = 0; - ret += pump_left1_is_full(); - ret += pump_left2_is_full(); - return ret; -} - -/* number of column owned by the robot */ -uint8_t get_column_count_right(void) -{ - uint8_t ret = 0; - ret += pump_right1_is_full(); - ret += pump_right2_is_full(); - return ret; -} - -/* number of column owned by the robot */ -uint8_t get_column_count(void) -{ - uint8_t ret = 0; - ret += pump_left1_is_full(); - ret += pump_left2_is_full(); - ret += pump_right1_is_full(); - ret += pump_right2_is_full(); - return ret; -} - -uint8_t get_lintel_count(void) -{ - return mechboard.lintel_count; -} - -uint8_t get_mechboard_mode(void) -{ - return mechboard.mode; -} - -uint8_t get_scanner_status(void) -{ - return sensorboard.scan_status; -} - -/* return 0 if timeout, or 1 if cond is true */ -uint8_t wait_scan_done(uint16_t timeout) -{ - uint8_t err; - err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); - return err; -} - uint8_t opponent_is_behind(void) { - int8_t opp_there; - int16_t opp_d, opp_a; +/* 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; +/* 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; } diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index 986b601..85997c2 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -43,7 +43,6 @@ struct xy_point { int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2); int16_t distance_from_robot(int16_t x, int16_t y); int16_t simple_modulo_360(int16_t a); -double simple_modulo_2pi(double a); int16_t angle_abs_to_rel(int16_t a_abs); void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs); double norm(double x, double y); @@ -62,15 +61,4 @@ uint8_t get_opponent_color(void); int8_t get_opponent_xy(int16_t *x, int16_t *y); 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); -uint8_t get_column_count(void); -uint8_t get_column_count_right(void); -uint8_t get_column_count_left(void); -uint8_t get_lintel_count(void); -uint8_t get_mechboard_mode(void); -uint8_t pump_left1_is_full(void); -uint8_t pump_left2_is_full(void); -uint8_t pump_right1_is_full(void); -uint8_t pump_right2_is_full(void); -uint8_t get_scanner_status(void); -uint8_t wait_scan_done(uint16_t timeout); uint8_t opponent_is_behind(void); diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py index 5080927..ff99e81 100755 --- a/projects/microb2010/microb_cmd/microbcmd.py +++ b/projects/microb2010/microb_cmd/microbcmd.py @@ -862,7 +862,7 @@ class Interp(cmd.Cmd): buf = f.read() addr = 0 while addr < len(buf): - time.sleep(0.1) + #time.sleep(0.1) if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0: sys.stdout.write("*") sys.stdout.flush() diff --git a/projects/microb2010/tests/beacon_tsop/.config b/projects/microb2010/tests/beacon_tsop/.config index 53256cb..f3cfa6e 100644 --- a/projects/microb2010/tests/beacon_tsop/.config +++ b/projects/microb2010/tests/beacon_tsop/.config @@ -217,8 +217,8 @@ CONFIG_MODULE_PID_CREATE_CONFIG=y # # Programmer options # -# CONFIG_AVRDUDE is not set -CONFIG_AVARICE=y +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set # # Avrdude diff --git a/projects/microb2010/tests/beacon_tsop/Makefile b/projects/microb2010/tests/beacon_tsop/Makefile index d8d5b40..1596ca3 100755 --- a/projects/microb2010/tests/beacon_tsop/Makefile +++ b/projects/microb2010/tests/beacon_tsop/Makefile @@ -3,7 +3,7 @@ TARGET = beacon_tsop AVERSIVE_DIR = ../../../.. # List C source files here. (C dependencies are automatically generated.) -SRC = main.c commands.c cmdline.c trigo.c uart_proto.c +SRC = main.c commands.c commands_cs.c cmdline.c trigo.c uart_proto.c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s @@ -14,7 +14,7 @@ SRC = main.c commands.c cmdline.c trigo.c uart_proto.c # care about how the name is spelled on its command-line. ASRC = -AVRDUDE_DELAY=30 +AVRDUDE_DELAY=15 CFLAGS += -W -Wall -Werror diff --git a/projects/microb2010/tests/beacon_tsop/board2006.h b/projects/microb2010/tests/beacon_tsop/board2006.h new file mode 100644 index 0000000..2e926bb --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/board2006.h @@ -0,0 +1,122 @@ +/* + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +/* LEDs */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_PORT PORTE +#define LED1_DDR DDRE +#define LED1_BIT 2 +#define LED2_PORT PORTE +#define LED2_DDR DDRE +#define LED2_BIT 3 +#define LED3_PORT PORTB +#define LED3_DDR DDRB +#define LED3_BIT 3 +#define LED_DDR_INIT() do { \ + LED1_DDR |= _BV(LED1_BIT); \ + LED2_DDR |= _BV(LED2_BIT); \ + LED3_DDR |= _BV(LED3_BIT); \ + } while(0) +#define LED1_ON() sbi(LED1_PORT, LED1_BIT) +#define LED1_OFF() cbi(LED1_PORT, LED1_BIT) +#define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT) +#define LED2_ON() sbi(LED2_PORT, LED2_BIT) +#define LED2_OFF() cbi(LED2_PORT, LED2_BIT) +#define LED2_TOGGLE() LED_TOGGLE(LED2_PORT, LED2_BIT) +#define LED3_ON() sbi(LED3_PORT, LED3_BIT) +#define LED3_OFF() cbi(LED3_PORT, LED3_BIT) +#define LED3_TOGGLE() LED_TOGGLE(LED3_PORT, LED3_BIT) + +/* TSOP */ + +#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ + +#define INTx_TSOP_OPP INT6 +#define ISCx0_TSOP_OPP ISC60 +#define ISCx1_TSOP_OPP ISC61 +#define SIG_TSOP_OPP SIG_INTERRUPT6 +#define TSOP_OPP_READ() (!(PINE & 0x40)) + +#define INTx_TSOP_STA INT7 +#define ISCx0_TSOP_STA ISC70 +#define ISCx1_TSOP_STA ISC71 +#define SIG_TSOP_STA SIG_INTERRUPT7 +#define TSOP_STA_READ() (!(PINE & 0x80)) + +#define TSOP_FREQ_455_MHZ 0.455 +#define N_PERIODS_455 10. +#define TSOP_FREQ_38_MHZ 0.038 +#define N_PERIODS_38 15. +#define TSOP_FREQ_30_MHZ 0.030 +#define N_PERIODS_30 15. +#define TSOP_FREQ_56_MHZ 0.056 +#define N_PERIODS_56 15. + +/* TSOP STATIC */ + +#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_MHZ) +#define TSOP_STA_N_PERIODS (N_PERIODS_38) + +#define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) +#define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) + +#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2)) +#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2)) + +#define TSOP_STA_FRAME_LEN 16 +#define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1) + +#define TSOP_STA_BEACON_ID_MASK 0x7 +#define TSOP_STA_BEACON_ID_SHIFT 0 +#define TSOP_STA_FRAME_DATA_MASK 0xFF8 +#define TSOP_STA_FRAME_DATA_SHIFT 3 + +#define TSOP_STA_BEACON_ID0 0x03 +#define TSOP_STA_BEACON_ID1 0x05 + +/* TSOP OPP */ + +#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_455_MHZ) +#define TSOP_OPP_N_PERIODS (N_PERIODS_455) + +#define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) +#define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) + +#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2)) +#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2)) + +#define TSOP_OPP_FRAME_LEN 16 +#define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1) + +#define TSOP_OPP_BEACON_ID_MASK 0x7 +#define TSOP_OPP_BEACON_ID_SHIFT 0 +#define TSOP_OPP_FRAME_DATA_MASK 0xFF8 +#define TSOP_OPP_FRAME_DATA_SHIFT 3 + +#define TSOP_OPP_BEACON_ID 0x01 diff --git a/projects/microb2010/tests/beacon_tsop/board2010.h b/projects/microb2010/tests/beacon_tsop/board2010.h new file mode 100644 index 0000000..669d590 --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/board2010.h @@ -0,0 +1,129 @@ +/* + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +/* LEDs */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED_PORT PORTA +#define LED_DDR DDRA +#define LED1_BIT 0 +#define LED2_BIT 1 +#define LED3_BIT 2 +#define LED4_BIT 3 +#define LED5_BIT 4 +#define LED6_BIT 5 +#define LED_DDR_INIT() do { \ + LED_DDR |= (_BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT) | \ + _BV(LED4_BIT) | _BV(LED5_BIT) | _BV(LED6_BIT)); \ + } while(0) +#define LED1_ON() sbi(LED_PORT, LED1_BIT) +#define LED1_OFF() cbi(LED_PORT, LED1_BIT) +#define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT) +#define LED2_ON() sbi(LED_PORT, LED2_BIT) +#define LED2_OFF() cbi(LED_PORT, LED2_BIT) +#define LED2_TOGGLE() LED_TOGGLE(LED_PORT, LED2_BIT) +#define LED3_ON() sbi(LED_PORT, LED3_BIT) +#define LED3_OFF() cbi(LED_PORT, LED3_BIT) +#define LED3_TOGGLE() LED_TOGGLE(LED_PORT, LED3_BIT) +#define LED4_ON() sbi(LED_PORT, LED4_BIT) +#define LED4_OFF() cbi(LED_PORT, LED4_BIT) +#define LED4_TOGGLE() LED_TOGGLE(LED_PORT, LED4_BIT) +#define LED5_ON() sbi(LED_PORT, LED5_BIT) +#define LED5_OFF() cbi(LED_PORT, LED5_BIT) +#define LED5_TOGGLE() LED_TOGGLE(LED_PORT, LED5_BIT) +#define LED6_ON() sbi(LED_PORT, LED6_BIT) +#define LED6_OFF() cbi(LED_PORT, LED6_BIT) +#define LED6_TOGGLE() LED_TOGGLE(LED_PORT, LED6_BIT) + +/* TSOP */ + +#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ + +#define INTx_TSOP_OPP INT5 +#define ISCx0_TSOP_OPP ISC50 +#define ISCx1_TSOP_OPP ISC(1 +#define SIG_TSOP_OPP SIG_INTERRUPT5 +#define TSOP_OPP_READ() (!(PINE & 0x20)) + +#define INTx_TSOP_STA INT6 +#define ISCx0_TSOP_STA ISC60 +#define ISCx1_TSOP_STA ISC61 +#define SIG_TSOP_STA SIG_INTERRUPT6 +#define TSOP_STA_READ() (!(PINE & 0x40)) + +#define TSOP_FREQ_455_MHZ 0.455 +#define N_PERIODS_455 10. +#define TSOP_FREQ_38_MHZ 0.038 +#define N_PERIODS_38 15. +#define TSOP_FREQ_30_MHZ 0.030 +#define N_PERIODS_30 15. +#define TSOP_FREQ_56_MHZ 0.056 +#define N_PERIODS_56 15. + +/* TSOP STATIC */ + +#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_MHZ) +#define TSOP_STA_N_PERIODS (N_PERIODS_38) + +#define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) +#define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) + +#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2)) +#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2)) + +#define TSOP_STA_FRAME_LEN 16 +#define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1) + +#define TSOP_STA_BEACON_ID_MASK 0x7 +#define TSOP_STA_BEACON_ID_SHIFT 0 +#define TSOP_STA_FRAME_DATA_MASK 0xFF8 +#define TSOP_STA_FRAME_DATA_SHIFT 3 + +#define TSOP_STA_BEACON_ID0 0x03 +#define TSOP_STA_BEACON_ID1 0x05 + +/* TSOP OPP */ + +#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_455_MHZ) +#define TSOP_OPP_N_PERIODS (N_PERIODS_455) + +#define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) +#define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) + +#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2)) +#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2)) + +#define TSOP_OPP_FRAME_LEN 16 +#define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1) + +#define TSOP_OPP_BEACON_ID_MASK 0x7 +#define TSOP_OPP_BEACON_ID_SHIFT 0 +#define TSOP_OPP_FRAME_DATA_MASK 0xFF8 +#define TSOP_OPP_FRAME_DATA_SHIFT 3 + +#define TSOP_OPP_BEACON_ID 0x01 diff --git a/projects/microb2010/tests/beacon_tsop/cmdline.h b/projects/microb2010/tests/beacon_tsop/cmdline.h index 19f673e..00190a9 100644 --- a/projects/microb2010/tests/beacon_tsop/cmdline.h +++ b/projects/microb2010/tests/beacon_tsop/cmdline.h @@ -20,7 +20,7 @@ * */ -#define CMDLINE_UART 0 +#define CMDLINE_UART 1 int cmdline_process(void); diff --git a/projects/microb2010/tests/beacon_tsop/commands.c b/projects/microb2010/tests/beacon_tsop/commands.c index 253c886..7b489fd 100644 --- a/projects/microb2010/tests/beacon_tsop/commands.c +++ b/projects/microb2010/tests/beacon_tsop/commands.c @@ -198,6 +198,15 @@ parse_pgm_inst_t cmd_test = { /**********************************************************/ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_consign_show; + /* in progmem */ parse_pgm_ctx_t main_ctx[] = { @@ -206,5 +215,15 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_debug_frame, (parse_pgm_inst_t *)&cmd_debug_speed, (parse_pgm_inst_t *)&cmd_test, + + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_consign_show, + NULL, }; diff --git a/projects/microb2010/tests/beacon_tsop/hfuse b/projects/microb2010/tests/beacon_tsop/hfuse index 06e504c..59ff393 100644 --- a/projects/microb2010/tests/beacon_tsop/hfuse +++ b/projects/microb2010/tests/beacon_tsop/hfuse @@ -1 +1 @@ -0xD9 +0x43 diff --git a/projects/microb2010/tests/beacon_tsop/lfuse b/projects/microb2010/tests/beacon_tsop/lfuse index 8541fd9..863c00d 100644 --- a/projects/microb2010/tests/beacon_tsop/lfuse +++ b/projects/microb2010/tests/beacon_tsop/lfuse @@ -1 +1 @@ -0xE1 +0xFB diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index 855eb05..b1d0db7 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -38,8 +38,15 @@ #include "uart_proto.h" #include "trigo.h" #include "main.h" + +#define BOARD2010 +//#define BOARD2006 + +#ifdef BOARD2010 +#include "board2010.h" +#else #include "board2006.h" -//#include "board2010.h" +#endif /******************* TSOP */ @@ -86,12 +93,12 @@ static uint16_t tick = 0; /* 8ms, easier if it's a pow of 2 */ #define CS_PERIOD_US (8192) -#define CS_PERIOD ((uint16_t)(CS_PERIOD_US*2)) +#define CS_PERIOD ((uint16_t)(CS_PERIOD_US/4)) #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */ #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US)) /* too fast = 100 tr/s */ /* in tr / 1000s */ -#define CS_CONSIGN (10 * 1000L) +#define CS_CONSIGN (15 * 1000L) /* 5% tolerance to validate captures, period is in */ #define TIM3_UNIT 250000000L @@ -106,6 +113,7 @@ static uint16_t tick = 0; #define LASER_OFF() do { TCCR0 = 0; } while (0) struct beacon_tsop beacon_tsop; +uint32_t cs_consign = CS_CONSIGN; static uint32_t current_motor_period; @@ -242,9 +250,9 @@ SIGNAL(SIG_TSOP_STA) { sei(); if (cur_tsop) - LED2_ON(); + LED5_ON(); else - LED2_OFF(); + LED5_OFF(); decode_frame(&static_beacon, ref_time, cur_time, cur_tsop); @@ -270,10 +278,10 @@ SIGNAL(SIG_TSOP_OPP) { running = 1; sei(); -/* if (cur_tsop) */ -/* LED2_ON(); */ -/* else */ -/* LED2_OFF(); */ + if (cur_tsop) + LED6_ON(); + else + LED6_OFF(); decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); @@ -295,6 +303,7 @@ static inline int32_t AbS(int32_t x) * (modulo 65536 obviously) */ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) { +#if 0 int32_t best_diff = 65536L; int8_t best_cpt = -2; int32_t diff; @@ -327,7 +336,21 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) /* real time difference in timer unit (resolution 4us) */ diff = (best_cpt * 16384L) + (icr_diff & 0x3fff); current_motor_period = diff; /* save it in global var */ - return 250000000L/diff; +#endif + + /* too slow (less than 1 tr/s) */ + if (icr_cpt >= CPT_ICR_MAX) + return 1000L; + + /* too fast (more than 100 tr/s) */ + if (icr_cpt <= CPT_ICR_MIN) + return 100000L; + + /* XXX test */ + if (icr_cpt > 25) + return icr_cpt * 8192UL; + + return TIM3_UNIT/icr_diff; } /* process the received frame ring */ @@ -520,9 +543,9 @@ int main(void) uint16_t diff_icr = 0; uint8_t cpt_icr = 0; uint8_t cpt = 0; - int32_t speed, out, err; + int32_t speed = 0, out, err; uint16_t tcnt3; - uint8_t x = 0; + uint8_t x = 0; /* debug display counter */ opp_beacon.frame_len = TSOP_OPP_FRAME_LEN; opp_beacon.time_long = TSOP_OPP_TIME_LONG; @@ -533,20 +556,22 @@ int main(void) static_beacon.time_short = TSOP_STA_TIME_SHORT; /* LEDS */ - LED1_DDR |= _BV(LED1_BIT); - LED2_DDR |= _BV(LED2_BIT); - LED3_DDR |= _BV(LED3_BIT); + LED_DDR_INIT(); DDRB |= 0x10; /* OC0 (laser pwm) */ /* PID init */ pid_init(&beacon_tsop.pid); - pid_set_gains(&beacon_tsop.pid, 500, 0, 0); - pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095); + pid_set_gains(&beacon_tsop.pid, 700, 10, 0); + pid_set_maximums(&beacon_tsop.pid, 0, 200000, 4095); pid_set_out_shift(&beacon_tsop.pid, 10); pid_set_derivate_filter(&beacon_tsop.pid, 4); uart_init(); +#if CMDLINE_UART == 0 fdevopen(uart0_dev_send, uart0_dev_recv); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); +#endif rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer); snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > "); @@ -562,19 +587,23 @@ int main(void) /* pwm for motor */ PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_1); +#ifdef BOARD2010 + PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, C, 10, 0, NULL, 0); +#else PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0); +#endif /* pwm for laser: * - clear on timer compare (CTC) * - Toggle OC0 on compare match * - prescaler = 1 */ TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); - OCR0 = 80; /* f = 100 khz at 16 Mhz */ + OCR0 = 18; /* f ~= 420 khz at 16 Mhz */ /* configure timer 3: CLK/64 * it is used as a reference time * enable noise canceller for ICP3 */ - TCCR3B = _BV(ICNC3) | _BV(CS11) | _BV(CS10); + TCCR3B = _BV(CS11) | _BV(CS10); sei(); @@ -593,7 +622,7 @@ int main(void) sei(); ETIFR = _BV(ICF3); - //LED2_TOGGLE(); + LED2_TOGGLE(); diff_icr = (icr - prev_icr); cpt_icr = cpt; prev_icr = icr; @@ -619,10 +648,9 @@ int main(void) /* process CS... maybe we don't need to use * control_system_manager, just PID is enough */ + if (cpt == CPT_ICR_MAX) speed = 0; - else - speed = get_speed(cpt_icr, diff_icr); /* enabled laser when rotation speed if at least 5tr/s */ if (speed > 5000) @@ -630,15 +658,17 @@ int main(void) else LASER_OFF(); - err = CS_CONSIGN - speed; + err = cs_consign - speed; out = pid_do_filter(&beacon_tsop.pid, err); - if (x == 0 && beacon_tsop.debug_speed) - printf("%ld %ld\n", speed, out); if (out < 0) out = 0; /* XXX */ - if (out > 2000) - out = 2000; + if (out > 3000) + out = 3000; + + if (x == 0 && beacon_tsop.debug_speed) + printf("%ld %ld %u %u / %u\n", + speed, out, diff_icr, cpt_icr, cpt); pwm_ng_set(&beacon_tsop.pwm_motor, out); diff --git a/projects/microb2010/tests/beacon_tsop/uart_config.h b/projects/microb2010/tests/beacon_tsop/uart_config.h index 13e1af7..b6d70b3 100644 --- a/projects/microb2010/tests/beacon_tsop/uart_config.h +++ b/projects/microb2010/tests/beacon_tsop/uart_config.h @@ -64,6 +64,44 @@ //#define UART0_STOP_BIT UART_STOP_BITS_2 +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 0 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 +//#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 4 +#define UART1_TX_FIFO_SIZE 1 +//#define UART1_NBITS 5 +//#define UART1_NBITS 6 +//#define UART1_NBITS 7 +#define UART1_NBITS 8 +//#define UART1_NBITS 9 + +#define UART1_PARITY UART_PARTITY_NONE +//#define UART1_PARITY UART_PARTITY_ODD +//#define UART1_PARITY UART_PARTITY_EVEN + +#define UART1_STOP_BIT UART_STOP_BITS_1 +//#define UART1_STOP_BIT UART_STOP_BITS_2 + + /* .... same for uart 1, 2, 3 ... */ diff --git a/projects/microb2010/tests/beacon_tsop/uart_proto.c b/projects/microb2010/tests/beacon_tsop/uart_proto.c new file mode 100644 index 0000000..81c315c --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/uart_proto.c @@ -0,0 +1,117 @@ +/* + * Copyright Droids Corporation (2010) + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include "cmdline.h" +#include "main.h" +#include "board2006.h" +//#include "board2010.h" + +#define UART_NUM 0 + +#if UART_NUM == 0 + +#define UCSRxA UCSR0A +#define UCSRxB UCSR0B +#define UCSRxC UCSR0C +#define RXCx RXC0 +#define UDRx UDR0 +#define UDREx UDRE0 +#define U2Xx U2X0 +#define RXENx RXEN0 +#define TXENx TXEN0 +#define UCSZx0 UCSZ00 +#define UCSZx1 UCSZ01 +#define UBRRxL UBRR0L +#define UBRRxH UBRR0H + +#elif UART_NUM == 1 + +#define UCSRxA UCSR1A +#define UCSRxB UCSR1B +#define UCSRxC UCSR1C +#define RXCx RXC1 +#define UDRx UDR1 +#define UDREx UDRE1 +#define U2Xx U2X1 +#define RXENx RXEN1 +#define TXENx TXEN1 +#define UCSZx0 UCSZ10 +#define UCSZx1 UCSZ11 +#define UBRRxL UBRR1L +#define UBRRxH UBRR1H + +#endif + +void uart_proto_init(void) +{ + UCSRxA = _BV(U2Xx); + UCSRxB = _BV(RXENx) | _BV(TXENx); + UCSRxC = _BV(UCSZx1) | _BV(UCSZx0); /* 8 bits no parity 1 stop */ + UBRRxL = 34; /* 57600 at 16 Mhz */ + UBRRxH = 0; +} + +static void uart_proto_send(char c) +{ + while ( !( UCSRxA & (1<>= 7; + c = x & 0x7f; + c |= 0x80; + uart_proto_send(c); +} + +void xmit_opp(uint16_t d, uint16_t a) +{ + uart_proto_send(0); + xmit_int14(d); + xmit_int14(a); +} + +void xmit_static(uint16_t x, uint16_t y, uint16_t a) +{ + uart_proto_send(1); + xmit_int14(x); + xmit_int14(y); + xmit_int14(a); +} diff --git a/projects/microb2010/tests/beacon_tsop/uart_proto.h b/projects/microb2010/tests/beacon_tsop/uart_proto.h new file mode 100644 index 0000000..bd24c7d --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/uart_proto.h @@ -0,0 +1,25 @@ +/* + * Copyright Droids Corporation (2010) + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +void uart_proto_init(void); +void xmit_opp(uint16_t d, uint16_t a); +void xmit_static(uint16_t x, uint16_t y, uint16_t a);