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
#include <rdline.h>
+#include "sensor.h"
#include "../common/i2c_commands.h"
#include "actuator.h"
-#include "ax12_user.h"
#include "main.h"
void servo_carry_open(void)
*
*/
+#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
+
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;
(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,
#include "state.h"
#include "i2c_protocol.h"
#include "actuator.h"
-#include "actuator.h"
+#include "spickle.h"
extern uint16_t state_debug;
},
};
+/**********************************************************/
+/* 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 */
#include "main.h"
#include "actuator.h"
+#include "spickle.h"
/* called every 5 ms */
static void do_cs(__attribute__((unused)) void *dummy)
/* ---- 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);
/* ---- 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);
/* ---- 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);
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,
#include "sensor.h"
#include "state.h"
#include "actuator.h"
-#include "arm_xy.h"
+#include "spickle.h"
#include "cs.h"
#include "i2c_protocol.h"
# 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) {
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,
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"));
gen.log_level = 5;
cobboard.flags |= DO_CS;
- state_machine();
+/* state_machine(); */
cmdline_interact();
return 0;
#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
#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
[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 */
#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
--- /dev/null
+/*
+ * 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 <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#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);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: 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
#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
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
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
########################################
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
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;
-}
-
*/
void pwm_set_and_save(void *pwm, int32_t val);
-void pickup_wheels_on(void);
-void pickup_wheels_off(void);
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
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 */
(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 */
(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,
};
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
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 {
};
/* 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)
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <encoders_spi.h>
#include <adc.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <i2c.h>
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;
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"));
}
};
/**********************************************************/
-/* 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 */
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";
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <encoders_spi.h>
#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"
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";
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;
}
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);
break;
}
}
+#endif
if (why & (~(END_TRAJ | END_NEAR)))
trajectory_stop(&mainboard.traj);
}
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);
}
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();
}
/* 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";
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")))
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;
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);
/* 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"))) {
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);
},
};
-/**********************************************************/
-/* 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 */
/* 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);
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <adc.h>
#include <pid.h>
#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)
{
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
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;
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,
#include <uart.h>
#include <pwm_ng.h>
#include <i2c.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
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) { \
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;
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;
}
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);
}
{
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;
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));
-}
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
#include <pwm_ng.h>
#include <timer.h>
#include <scheduler.h>
-#include <time.h>
+#include <clock_time.h>
#include <adc.h>
#include <pid.h>
#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
struct genboard gen;
struct mainboard mainboard;
-struct mechboard mechboard;
-struct sensorboard sensorboard;
+struct cobboard cobboard;
+struct ballboard ballboard;
/***********************/
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();
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();
#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
/* 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;
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);
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
#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; i<BEACON_MAX_SAMPLES; i++) {
- printf_P(PSTR("%d: pos=(%d,%d,%d) opp=(%d,%d) time=%d\r\n"),
- i,
- beacon_sample[i].posx,
- beacon_sample[i].posy,
- beacon_sample[i].posa,
- beacon_sample[i].oppx,
- beacon_sample[i].oppy,
- beacon_sample[i].time);
- }
-}
-#endif
-
struct strat_infos strat_infos = {
/* conf */
.conf = {
.flags = 0,
- /* scanner disabled by default */
- .scan_opp_min_time = 90,
- .delay_between_opp_scan = 90,
- .scan_our_min_time = 90,
- .delay_between_our_scan = 90,
- .wait_opponent = 0,
- .lintel_min_time = 0,
- .scan_opp_angle = -1,
- },
-
- /* static columns */
- .s_cols = {
- .flags = 0,
- .configuration = 0,
- },
-
- /* column dispensers ; be carreful, positions are
- * color-dependent, so COLOR_Y() and COLOR_A() should be
- * used. All angles here are _absolute_ */
- .c1 = {
- .checkpoint_x = 2711 - COL_SCAN_PRE_MARGIN,
- .checkpoint_y = AREA_Y - COL_DISP_MARGIN,
- .scan_left = 0,
- .scan_a = 180,
- .eject_a = 180,
- .recalib_x = 2711,
- .recalib_y = AREA_Y - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
- .pickup_a = 90,
- .name = "col_disp1",
- },
- .c2 = {
- .checkpoint_x = AREA_X - COL_DISP_MARGIN,
- .checkpoint_y = 800 - COL_SCAN_PRE_MARGIN,
- .scan_left = 1,
- .scan_a = -90,
- .eject_a = -90,
- .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
- .recalib_y = 800,
- .pickup_a = 0,
- .name = "col_disp2",
- },
- .c3 = {
- .checkpoint_x = AREA_X-COL_DISP_MARGIN,
- .checkpoint_y = 1300 + COL_SCAN_PRE_MARGIN,
- .scan_a = 90,
- .scan_left = 0,
- .eject_a = -90,
- .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
- .recalib_y = 1300,
- .pickup_a = 0,
- .name = "col_disp3",
- },
-
- /* lintel dispensers */
- .l1 = {
- .x = 912, /* XXX for red only */
- .name = "lin_disp1",
- },
- .l2 = {
- .x = 1312, /* XXX for red only */
- .name = "lin_disp2",
},
- /* build zones */
- .zone_list = {
-#define ZONE_DISC_NUM 0
- {
- .flags = ZONE_F_VALID | ZONE_F_DISC,
- .level = 2,
- .checkpoint_x = 0,
- .checkpoint_x = 0,
- .name = "disc",
- },
-#define ZONE_1A_NUM 1
- {
- .flags = ZONE_F_VALID,
- .level = 1,
- .checkpoint_x = 1385,
- .checkpoint_y = 1700,
- .name = "z1a",
- },
-#define ZONE_1B_NUM 2
- {
- .flags = ZONE_F_VALID,
- .level = 1,
- .checkpoint_x = 1615,
- .checkpoint_y = 1700,
- .name = "z1b",
- },
-#define ZONE_0B_NUM 3
- {
- .flags = ZONE_F_VALID,
- .level = 0,
- .checkpoint_x = 2100,
- .checkpoint_y = 1700,
- .name = "z0b",
- },
-#define ZONE_0A_NUM 4
- {
- .flags = ZONE_F_VALID,
- .level = 0,
- .checkpoint_x = 900,
- .checkpoint_y = 1700,
- .name = "z0a",
- },
- }
};
/*************************************************************/
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_BD | DO_POWER;
- i2c_mechboard_mode_init();
- if (get_color() == I2C_COLOR_RED)
- i2c_mechboard_mode_prepare_pickup(I2C_LEFT_SIDE);
- else
- i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
-
+ i2c_cobboard_mode_init();
strat_dump_conf();
strat_dump_infos(__FUNCTION__);
}
printf_P(PSTR("-- conf --\r\n"));
- printf_P(PSTR(" one build on disc: "));
- if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" bypass static2: "));
- if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" take one lintel: "));
- if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" skip this temple when temple check fails: "));
- if (strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" store static2: "));
- if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" (big3) try to build a temple with 3 lintels: "));
- if (strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" early opponent scan: "));
- if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" push opponent columns: "));
- if (strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS)
- printf_P(PSTR("on\r\n"));
- else
- printf_P(PSTR("off\r\n"));
-
- printf_P(PSTR(" scan opponent min time: %d\r\n"),
- strat_infos.conf.scan_opp_min_time);
- printf_P(PSTR(" delay between oppnent scan: %d\r\n"),
- strat_infos.conf.delay_between_opp_scan);
- printf_P(PSTR(" scan our min time: %d\r\n"),
- strat_infos.conf.scan_our_min_time);
- printf_P(PSTR(" delay between our scan: %d\r\n"),
- strat_infos.conf.delay_between_our_scan);
- printf_P(PSTR(" wait opponent gone before scan: %d\r\n"),
- strat_infos.conf.wait_opponent);
- printf_P(PSTR(" lintel min time: %d\r\n"),
- strat_infos.conf.lintel_min_time);
- printf_P(PSTR(" scan_opp_angle: %d\r\n"),
- strat_infos.conf.scan_opp_angle);
-}
-
-void strat_dump_temple(struct temple *temple)
-{
- if (!strat_infos.dump_enabled)
- return;
-
- printf_P(PSTR(" temple %p (%s): "), temple, temple->zone->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; i<MAX_TEMPLE; i++) {
- temple = &strat_infos.temple_list[i];
- if (!(temple->flags & 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; i<MAX_ZONE; i++) {
- zone = &strat_infos.zone_list[i];
- if (!(zone->flags & ZONE_F_VALID))
- continue;
- strat_dump_zone(zone);
- }
}
/* display current information about the state of the game */
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<MAX_ZONE; i++)
- strat_infos.zone_list[i].flags = ZONE_F_VALID;
- strat_infos.zone_list[ZONE_DISC_NUM].flags |= ZONE_F_DISC;
- strat_infos.zone_list[ZONE_1A_NUM].flags |= ZONE_F_ZONE1;
- strat_infos.zone_list[ZONE_1B_NUM].flags |= ZONE_F_ZONE1;
- strat_infos.zone_list[ZONE_0A_NUM].flags |= ZONE_F_ZONE0;
- strat_infos.zone_list[ZONE_0B_NUM].flags |= ZONE_F_ZONE0;
-
- strat_set_bounding_box();
-
- /* set lintel position, depending on color */
- if (mainboard.our_color == I2C_COLOR_RED) {
- strat_infos.l1.x = 912;
- strat_infos.l2.x = 1312;
- }
- else {
- strat_infos.l1.x = 888;
- strat_infos.l2.x = 1288;
- }
}
/* call it just before launching the strat */
void strat_init(void)
{
- pickup_wheels_on();
+ /* XXX init rollers, .. */
strat_reset_infos();
/* we consider that the color is correctly set */
/* used in strat_base for END_TIMER */
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_BD | DO_TIMER | DO_POWER;
-
-#ifdef TEST_BEACON
- beacon_prev_time = 0;
- beacon_cur_idx = 0;
-#endif
}
{
uint8_t flags;
- pickup_wheels_off();
mainboard.flags &= ~(DO_TIMER);
strat_hardstop();
time_reset();
{
/* limit speed when opponent is close */
strat_limit_speed();
-
-#ifdef TEST_BEACON
- beacon_update_samples();
-#endif
}
-/* do static cols + first temples */
static uint8_t strat_beginning(void)
{
- uint8_t err;
-
- /* don't limit the speed when opponent is near: it can change
- * the radius of the curves */
- strat_limit_speed_disable();
-
- err = strat_static_columns(0);
-
- strat_limit_speed_enable();
-
- if (!TRAJ_SUCCESS(err))
- return err;
-
- /* go to disc to build the first temple */
-
- /* XXX if opponent is near disc, go to zone1 */
- err = strat_goto_disc(2);
- if (!TRAJ_SUCCESS(err))
- return err;
- DEBUG(E_USER_STRAT, "disc reached");
-
- /* can return END_ERROR or END_TIMER, should not happen
- * here */
- err = strat_build_new_temple(&strat_infos.zone_list[0]);
- if (!TRAJ_SUCCESS(err))
- return err;
-
- /* bypass static2 if specified */
- if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2) {
- err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
- return err;
- }
-
- /* get the last 2 columns, and build them on previous temple */
- err = strat_static_columns_pass2();
- if (!TRAJ_SUCCESS(err))
- return err;
-
- /* early opponent scan, for offensive strategy */
- if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) {
- err = strat_pickup_lintels();
- /* ignore code */
-
- /* try to build on opponent (scan must be enabled) */
- err = strat_build_on_opponent_temple();
- /* ignore code */
- }
-
- return err;
-}
-
-/* return true if we need to grab some more elements (lintel/cols) */
-uint8_t need_more_elements(void)
-{
- if (time_get_s() <= 75) {
- /* we have at least one col on each arm, build now */
- if ((get_column_count_left() >= 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) */
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;
}
#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)
};
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);
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
+++ /dev/null
-/*
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-#include "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 ; i<len ; i++) {
- DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, 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);
-}
+++ /dev/null
-/*
- * 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);
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
{
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);
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;
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;
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;
* 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);
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/queue.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-
-#include "../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<MAX_TEMPLE; i++) {
- if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
- return &strat_infos.temple_list[i];
- }
- return NULL;
-}
-
-uint8_t strat_can_build_on_temple(struct temple *temple)
-{
- uint8_t col_l, col_r, max_col, lintel;
-
- col_l = get_column_count_left();
- col_r = get_column_count_right();
- max_col = (col_l > 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; i<MAX_TEMPLE; i++) {
- temple = &strat_infos.temple_list[i];
-
- if (!(temple->flags & 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; i<MAX_TEMPLE; i++) {
- temple = &strat_infos.temple_list[i];
-
- if (!(temple->flags & 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; i<MAX_ZONE; i++) {
- zone = &strat_infos.zone_list[i];
-
- if (zone->flags & ZONE_F_BUSY)
- continue;
- if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)
- continue;
-
- return zone;
- }
- return NULL;
-}
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/queue.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-#include "../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;
-}
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/queue.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-#include "../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;
-}
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/queue.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-
-#include "../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;
-}
+++ /dev/null
-/*
- * 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 <zer0@droids-corp.org>
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/queue.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <time.h>
-#include <spi.h>
-
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-
-#include "../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;
-}
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
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)
{
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)
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;
{
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;
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;
}
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);
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);
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()
#
# Programmer options
#
-# CONFIG_AVRDUDE is not set
-CONFIG_AVARICE=y
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
#
# Avrdude
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
# care about how the name is spelled on its command-line.
ASRC =
-AVRDUDE_DELAY=30
+AVRDUDE_DELAY=15
CFLAGS += -W -Wall -Werror
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (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
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (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
*
*/
-#define CMDLINE_UART 0
+#define CMDLINE_UART 1
int cmdline_process(void);
/**********************************************************/
+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[] = {
(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,
};
#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 */
/* 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
#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;
sei();
if (cur_tsop)
- LED2_ON();
+ LED5_ON();
else
- LED2_OFF();
+ LED5_OFF();
decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
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);
* (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;
/* 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 */
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;
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 > ");
/* 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();
sei();
ETIFR = _BV(ICF3);
- //LED2_TOGGLE();
+ LED2_TOGGLE();
diff_icr = (icr - prev_icr);
cpt_icr = cpt;
prev_icr = icr;
/* 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)
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);
//#define UART0_STOP_BIT UART_STOP_BITS_2\r
\r
\r
+/*\r
+ * UART1 definitions \r
+ */\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE \r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED 1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED 0\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+//#define UART1_USE_DOUBLE_SPEED 1\r
+\r
+#define UART1_RX_FIFO_SIZE 4\r
+#define UART1_TX_FIFO_SIZE 1\r
+//#define UART1_NBITS 5\r
+//#define UART1_NBITS 6\r
+//#define UART1_NBITS 7\r
+#define UART1_NBITS 8\r
+//#define UART1_NBITS 9\r
+\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+//#define UART1_PARITY UART_PARTITY_ODD\r
+//#define UART1_PARITY UART_PARTITY_EVEN\r
+\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+//#define UART1_STOP_BIT UART_STOP_BITS_2\r
+\r
+\r
\r
\r
/* .... same for uart 1, 2, 3 ... */\r
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (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 <aversive.h>
+#include <aversive/wait.h>
+
+#include <pid.h>
+#include <pwm_ng.h>
+#include <parse.h>
+#include <rdline.h>
+#include <uart.h>
+
+#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<<UDREx)) ) ;
+ UDRx = c;
+}
+
+
+/* transmit an integer between 0 and 16384 */
+static void xmit_int14(uint16_t x)
+{
+ uint8_t c;
+
+ c = x & 0x7f;
+ c |= 0x80;
+ uart_proto_send(c);
+
+ x >>= 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);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (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);
git.droids-corp.org / aversive.git / commitdiff