From 1e34f924a476750c0951329a897ba5c42c5963dc Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Mon, 5 Apr 2010 14:19:28 +0200 Subject: [PATCH 01/16] .keepme for binaries --- projects/microb2010/microb_cmd/binaries/.keepme | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 projects/microb2010/microb_cmd/binaries/.keepme diff --git a/projects/microb2010/microb_cmd/binaries/.keepme b/projects/microb2010/microb_cmd/binaries/.keepme new file mode 100644 index 0000000..e69de29 -- 2.20.1 From 99768092f81d0e183b61682e832abd0263472d2d Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Mon, 5 Apr 2010 14:33:03 +0200 Subject: [PATCH 02/16] microb cmdline for fast bootloader --- projects/microb2010/microb_cmd/microbcmd.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py index 555141f..da53115 100755 --- a/projects/microb2010/microb_cmd/microbcmd.py +++ b/projects/microb2010/microb_cmd/microbcmd.py @@ -107,7 +107,7 @@ def prog_page(ser, addr, buf): def prog_metadata(ser, addr, buf): length = len(buf) crc = do_crc(buf) - page = struct.pack("LL", length, crc) filename = os.path.join(MICROB_PATH, "binaries/%x_%x.bin"%(length, crc)) print "saving in %s"%(filename) f = open(filename, "w") @@ -116,13 +116,8 @@ def prog_metadata(ser, addr, buf): return prog_page(ser, addr, page) def get_same_bin_file(ser): - # hack because dump32 does not work - l1 = read32(ser, METADATA_ADDR) & 0xFFFF - l2 = read32(ser, METADATA_ADDR + 2) & 0xFFFF - c1 = read32(ser, METADATA_ADDR + 4) & 0xFFFF - c2 = read32(ser, METADATA_ADDR + 6) & 0xFFFF - l = l1 + l2 << 16 - c = c1 + c2 << 16 + l = read32(ser, METADATA_ADDR) + c = read32(ser, METADATA_ADDR + 4) filename = os.path.join(MICROB_PATH, "binaries/%x_%x.bin"%(l, c)) print filename @@ -334,8 +329,8 @@ class Interp(cmd.Cmd): addr = 0 - #old_buf = get_same_bin_file(self.ser) - old_buf = None + old_buf = get_same_bin_file(self.ser) + #old_buf = None while addr < len(buf): if addr > METADATA_ADDR and old_buf != None and \ -- 2.20.1 From ebfaaedd491e61696cc93b353471be15408d23e4 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 16:30:39 +0200 Subject: [PATCH 03/16] merge hostsim in main --- modules/base/scheduler/scheduler_host.c | 6 + modules/comm/i2c/Makefile | 4 + modules/comm/i2c/i2c.c | 130 ++++---- modules/comm/i2c/i2c_host.c | 1 + modules/debug/diagnostic/Makefile | 4 + modules/debug/diagnostic/diag_host.c | 1 + modules/hardware/adc/Makefile | 4 + modules/hardware/adc/adc.c | 6 +- modules/hardware/adc/adc_host.c | 1 + projects/microb2010/mainboard/.config | 32 +- projects/microb2010/mainboard/Makefile | 9 +- projects/microb2010/mainboard/actuator.c | 9 + projects/microb2010/mainboard/ax12_user.c | 8 +- projects/microb2010/mainboard/cmdline.c | 46 +-- projects/microb2010/mainboard/cmdline.h | 4 + projects/microb2010/mainboard/commands_ax12.c | 4 + projects/microb2010/mainboard/commands_cs.c | 9 +- projects/microb2010/mainboard/commands_gen.c | 37 ++- .../microb2010/mainboard/commands_mainboard.c | 69 ++++- projects/microb2010/mainboard/commands_traj.c | 33 +- projects/microb2010/mainboard/cs.c | 51 +++- projects/microb2010/mainboard/display.py | 282 ++++++++++++++++++ projects/microb2010/mainboard/i2c_protocol.c | 53 ++-- projects/microb2010/mainboard/main.c | 44 ++- projects/microb2010/mainboard/main.h | 24 ++ projects/microb2010/mainboard/robotsim.c | 186 ++++++++++++ projects/microb2010/mainboard/robotsim.h | 26 ++ .../microb2010/mainboard/scheduler_config.h | 6 +- projects/microb2010/mainboard/sensor.c | 40 ++- 29 files changed, 937 insertions(+), 192 deletions(-) create mode 100644 modules/comm/i2c/i2c_host.c create mode 100644 modules/debug/diagnostic/diag_host.c create mode 100644 modules/hardware/adc/adc_host.c create mode 100644 projects/microb2010/mainboard/display.py create mode 100644 projects/microb2010/mainboard/robotsim.c create mode 100644 projects/microb2010/mainboard/robotsim.h diff --git a/modules/base/scheduler/scheduler_host.c b/modules/base/scheduler/scheduler_host.c index 05f6593..965007f 100644 --- a/modules/base/scheduler/scheduler_host.c +++ b/modules/base/scheduler/scheduler_host.c @@ -19,17 +19,23 @@ * */ +#include #include #include #include #include +#include /* this file is compiled for host version only */ /** declared in scheduler.c in case of AVR version */ struct event_t g_tab_event[SCHEDULER_NB_MAX_EVENT]; +#ifdef CONFIG_MODULE_SCHEDULER_STATS +struct scheduler_stats sched_stats; +#endif + /** init all global data */ void scheduler_init(void) { diff --git a/modules/comm/i2c/Makefile b/modules/comm/i2c/Makefile index 5cab808..6fa6efb 100644 --- a/modules/comm/i2c/Makefile +++ b/modules/comm/i2c/Makefile @@ -1,6 +1,10 @@ TARGET = i2c # List C source files here. (C dependencies are automatically generated.) +ifeq ($(HOST),avr) SRC = i2c.c +else +SRC = i2c_host.c +endif include $(AVERSIVE_DIR)/mk/aversive_module.mk diff --git a/modules/comm/i2c/i2c.c b/modules/comm/i2c/i2c.c index d1832a3..680655a 100644 --- a/modules/comm/i2c/i2c.c +++ b/modules/comm/i2c/i2c.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2007) - * + * * 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 @@ -50,7 +50,7 @@ static void (*g_recv_event)(uint8_t *, int8_t) = NULL; * params are : hwstatus, index of byte in frame, byte value */ static void (*g_recv_byte_event)(uint8_t, uint8_t, uint8_t) = NULL; -/** send event, called when transmit is complete +/** send event, called when transmit is complete * param is error code : 0 if success */ static void (*g_send_event)(int8_t) = NULL; @@ -81,17 +81,17 @@ static volatile uint8_t g_command = 0; /** * mode is I2C_MODE_UNINIT, I2C_MODE_MASTER, I2C_MODE_MULTIMASTER or * I2C_MODE_SLAVE. Parameter add is the address in slave mode, it is - * composed from: + * composed from: * b7 : true if the uC can be addressed with GENCALL * b0-6: slave address */ -void +void i2c_init(i2c_mode_t mode, uint8_t add) { uint8_t flags; IRQ_LOCK(flags); - + if (mode == I2C_MODE_UNINIT) { /* disable all */ TWCR = 0; @@ -110,20 +110,20 @@ i2c_init(i2c_mode_t mode, uint8_t add) } TWBR = I2C_BITRATE; - + /* prescaler */ if (I2C_PRESCALER & 1) sbi(TWSR, TWPS0); if (I2C_PRESCALER & 2) sbi(TWSR, TWPS1); - + /* change for TWAR format */ TWAR = add << 1 ; - + /* general call */ if (add & 0x80) sbi(TWAR, TWGCE); - + /* init vars */ g_mode = mode; g_status = I2C_STATUS_READY; @@ -138,18 +138,18 @@ i2c_init(i2c_mode_t mode, uint8_t add) } -/** +/** * Register a function that is called when a buffer is received. The * user application is always notified when data frame is received. * Arguments of the callback are: - * - (recv_buf, n>0) if transmission succedded. The first parameter + * - (recv_buf, n>0) if transmission succedded. The first parameter * contains the address of the reception buffer and * the second contains the number of received bytes. * - (NULL, err<0) if the transmission failed (slave not answering - * or arbiteration lost). The first parameter is + * or arbiteration lost). The first parameter is * NULL and the second contains the error code. */ -void +void i2c_register_recv_event(void (*event)(uint8_t *, int8_t)) { uint8_t flags; @@ -158,14 +158,14 @@ i2c_register_recv_event(void (*event)(uint8_t *, int8_t)) IRQ_UNLOCK(flags); } -/** +/** * Register a function that is called when a byte is received. * Arguments of the callback are: (hwstatus, numbyte, byte). The user * app can modify the g_recv_size value, which is the number of bytes * to be received in the frame: this can be done by calling * i2c_set_recv_size(). */ -void +void i2c_register_recv_byte_event(void (*event)(uint8_t, uint8_t, uint8_t)) { uint8_t flags; @@ -183,7 +183,7 @@ i2c_register_recv_byte_event(void (*event)(uint8_t, uint8_t, uint8_t)) * - Else, the number of transmitted bytes is given, including the * one that was not acked. */ -void +void i2c_register_send_event(void (*event)(int8_t)) { uint8_t flags; @@ -195,30 +195,30 @@ i2c_register_send_event(void (*event)(int8_t)) /** * Send a buffer. Return 0 if xmit starts correctly. * On error, return < 0. - * - If mode is slave, dest_add should be I2C_ADD_MASTER, and transmission - * starts when the master transmits a clk. - * - If mode is master and if dest_add != I2C_ADD_MASTER, it will transmit - * a START condition if bus is available (the uc will act as a + * - If mode is slave, dest_add should be I2C_ADD_MASTER, and transmission + * starts when the master transmits a clk. + * - If mode is master and if dest_add != I2C_ADD_MASTER, it will transmit + * a START condition if bus is available (the uc will act as a * master) * - If mode is master and if dest_add == I2C_ADD_MASTER, the uC will * act as a slave, and data will be sent when the uC will be * addressed. * The transmission will be processed with these params until a - * i2c_flush() is called. - * The 'ctrl' parameter is composed by the flags I2C_CTRL_SYNC and + * i2c_flush() is called. + * The 'ctrl' parameter is composed by the flags I2C_CTRL_SYNC and * I2C_CTRL_DONT_RELEASE_BUS */ -int8_t -i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl) +int8_t +i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl) { uint8_t flags; - + IRQ_LOCK(flags); if (g_mode == I2C_MODE_UNINIT) { IRQ_UNLOCK(flags); return -ENXIO; } - + if (g_status & (I2C_STATUS_MASTER_XMIT | I2C_STATUS_MASTER_RECV | I2C_STATUS_SLAVE_XMIT_WAIT | @@ -288,7 +288,7 @@ i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl) return 0; return g_sync_res; } - + return -ESUCCESS; } @@ -298,8 +298,8 @@ i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl) * the same parameters as the last call. It safe to call it from the * send_event, but else the send buffer may have been overwritten. */ -int8_t -i2c_resend(void) +int8_t +i2c_resend(void) { return i2c_send(g_dest, g_send_buf, g_send_size, g_ctrl); } @@ -307,8 +307,8 @@ i2c_resend(void) /** * same but for recv */ -int8_t -i2c_rerecv(void) +int8_t +i2c_rerecv(void) { return i2c_recv(g_dest, g_recv_size, g_ctrl); } @@ -334,11 +334,11 @@ i2c_reset(void) TWCR = 0; g_status = I2C_STATUS_READY; #ifdef CONFIG_MODULE_I2C_MASTER - if (g_mode == I2C_MODE_MASTER) + if (g_mode == I2C_MODE_MASTER) TWCR = (1< g_recv_nbytes) + if (size > g_recv_nbytes) size = g_recv_nbytes; memcpy(buf, g_recv_buf, size); @@ -549,10 +549,10 @@ SIGNAL(SIG_2WIRE_SERIAL) switch(hard_status) { #ifdef CONFIG_MODULE_I2C_MASTER - case TW_START: + case TW_START: case TW_REP_START: /* a start has been transmitted, transmit SLA+W which is : - * b7-1: slave address + * b7-1: slave address * b0 : 0 (write operation) or 1 (read) */ if (g_status & I2C_STATUS_MASTER_RECV) { TWDR = (g_dest << 1) | (0x01); @@ -590,12 +590,12 @@ SIGNAL(SIG_2WIRE_SERIAL) break; case TW_MT_DATA_NACK: - /* we transmitted data but slave sent us a NACK. + /* we transmitted data but slave sent us a NACK. * Notify the number of bytes sent, including the one * that were not acked, and send a stop condition */ g_status |= (I2C_STATUS_OP_FINISHED | I2C_STATUS_NEED_XMIT_EVT); break; - + /* MASTER RECEIVER */ @@ -606,7 +606,7 @@ SIGNAL(SIG_2WIRE_SERIAL) if (g_recv_size > 1) command |= (1< 1) { command |= (1< #include "main.h" +#include "robotsim.h" void pwm_set_and_save(void *pwm, int32_t val) { @@ -62,19 +63,27 @@ void pwm_set_and_save(void *pwm, int32_t val) mainboard.pwm_l = val; else if (pwm == RIGHT_PWM) mainboard.pwm_r = val; +#ifdef HOST_VERSION + robotsim_pwm(pwm, val); +#else pwm_ng_set(pwm, val); +#endif } void support_balls_deploy(void) { +#ifndef HOST_VERSION pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510); pwm_ng_set(SUPPORT_BALLS_L_SERVO, 205); +#endif } void support_balls_pack(void) { +#ifndef HOST_VERSION pwm_ng_set(SUPPORT_BALLS_R_SERVO, 250); pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480); +#endif } void actuator_init(void) diff --git a/projects/microb2010/mainboard/ax12_user.c b/projects/microb2010/mainboard/ax12_user.c index de5bb88..6bb8129 100644 --- a/projects/microb2010/mainboard/ax12_user.c +++ b/projects/microb2010/mainboard/ax12_user.c @@ -86,6 +86,8 @@ * DDR to manage the port directions. */ +#ifndef HOST_VERSION + static volatile uint8_t ax12_state = AX12_STATE_READ; extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ static volatile uint8_t ax12_nsent = 0; @@ -163,14 +165,18 @@ static void ax12_switch_uart(uint8_t state) IRQ_UNLOCK(flags); } } - +#endif void ax12_user_init(void) { +#ifdef HOST_VERSION + return; +#else /* AX12 */ AX12_init(&gen.ax12); AX12_set_hardware_send(&gen.ax12, ax12_send_char); AX12_set_hardware_recv(&gen.ax12, ax12_recv_char); AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart); uart_register_tx_event(UART_AX12_NUM, ax12_send_callback); +#endif } diff --git a/projects/microb2010/mainboard/cmdline.c b/projects/microb2010/mainboard/cmdline.c index eeeede6..3ae4d30 100644 --- a/projects/microb2010/mainboard/cmdline.c +++ b/projects/microb2010/mainboard/cmdline.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -26,6 +26,8 @@ #include #include +#include + #include #include #include @@ -53,13 +55,13 @@ /******** See in commands.c for the list of commands. */ extern parse_pgm_ctx_t main_ctx[]; -static void write_char(char c) +static void write_char(char c) { uart_send(CMDLINE_UART, c); } -static void -valid_buffer(const char *buf, uint8_t size) +static void +valid_buffer(const char *buf, uint8_t size) { int8_t ret; @@ -76,7 +78,7 @@ valid_buffer(const char *buf, uint8_t size) printf_P(PSTR("Bad arguments\r\n")); } -static int8_t +static int8_t complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, int16_t *state) { @@ -88,34 +90,40 @@ complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, void emergency(char c) { static uint8_t i = 0; - + /* interrupt traj here */ if (c == '\003') interrupt_traj(); - + if ((i == 0 && c == 'p') || (i == 1 && c == 'o') || - (i == 2 && c == 'p')) + (i == 2 && c == 'p')) i++; else if ( !(i == 1 && c == 'p') ) i = 0; - if (i == 3) + if (i == 3) { +#ifdef HOST_VERSION + hostsim_exit(); +#endif reset(); + } } /* log function, add a command to configure * it dynamically */ -void mylog(struct error * e, ...) +void mylog(struct error * e, ...) { va_list ap; +#ifndef HOST_VERSION u16 stream_flags = stdout->flags; +#endif uint8_t i; time_h tv; if (e->severity > ERROR_SEVERITY_ERROR) { if (gen.log_level < e->severity) return; - + for (i=0; ierr_num) break; @@ -125,17 +133,19 @@ void mylog(struct error * e, ...) va_start(ap, e); tv = time_get_time(); - printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL)); - + printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL)); + printf_P(PSTR("(%d,%d,%d) "), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); - + vfprintf_P(stdout, e->text, ap); printf_P(PSTR("\r\n")); va_end(ap); +#ifndef HOST_VERSION stdout->flags = stream_flags; +#endif } int cmdline_interact(void) @@ -143,14 +153,14 @@ int cmdline_interact(void) const char *history, *buffer; int8_t ret, same = 0; int16_t c; - + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); - snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > "); + snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > "); rdline_newline(&gen.rdl, gen.prompt); while (1) { c = uart_recv_nowait(CMDLINE_UART); - if (c == -1) + if (c == -1) continue; ret = rdline_char_in(&gen.rdl, c); if (ret != 2 && ret != 0) { diff --git a/projects/microb2010/mainboard/cmdline.h b/projects/microb2010/mainboard/cmdline.h index f80b9b3..6b5c824 100644 --- a/projects/microb2010/mainboard/cmdline.h +++ b/projects/microb2010/mainboard/cmdline.h @@ -20,7 +20,11 @@ * */ +#ifdef HOST_VERSION +#define CMDLINE_UART 0 +#else #define CMDLINE_UART 1 +#endif /* uart rx callback for reset() */ void emergency(char c); diff --git a/projects/microb2010/mainboard/commands_ax12.c b/projects/microb2010/mainboard/commands_ax12.c index 75616f7..050b406 100644 --- a/projects/microb2010/mainboard/commands_ax12.c +++ b/projects/microb2010/mainboard/commands_ax12.c @@ -182,6 +182,9 @@ struct cmd_baudrate_result { /* function called when cmd_baudrate is parsed successfully */ static void cmd_baudrate_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_baudrate_result *res = parsed_result; struct uart_config c; @@ -190,6 +193,7 @@ static void cmd_baudrate_parsed(void * parsed_result, void * data) c.baudrate = res->arg1; uart_setconf(1, &c); printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); +#endif } prog_char str_baudrate_arg0[] = "baudrate"; diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index e6850f3..272f1e9 100644 --- a/projects/microb2010/mainboard/commands_cs.c +++ b/projects/microb2010/mainboard/commands_cs.c @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -193,7 +194,7 @@ static void cmd_speed_parsed(void *parsed_result, void *show) if (!show) ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ - printf_P(PSTR("%s %lu\r\n"), + printf_P(PSTR("%s %"PRIu32"\r\n"), res->cs.csname, ext.r_b.var_pos); #else @@ -380,7 +381,7 @@ static void cmd_maximum_parsed(void *parsed_result, void *show) if (!show) pid_set_maximums(&csb->pid, res->in, res->i, res->out); - printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), + printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"), res->cs.csname, pid_get_max_in(&csb->pid), pid_get_max_I(&csb->pid), @@ -461,7 +462,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); } - printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"), res->cs.csname, csb->qr.var_2nd_ord_pos, csb->qr.var_2nd_ord_neg, @@ -623,7 +624,7 @@ static void cmd_blocking_i_parsed(void *parsed_result, void *show) bd_set_current_thresholds(&csb->bd, res->k1, res->k2, res->i, res->cpt); - printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), + printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), res->cs.cmdname, res->cs.csname, csb->bd.k1, diff --git a/projects/microb2010/mainboard/commands_gen.c b/projects/microb2010/mainboard/commands_gen.c index fb9024d..bec2ae4 100644 --- a/projects/microb2010/mainboard/commands_gen.c +++ b/projects/microb2010/mainboard/commands_gen.c @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -70,6 +71,9 @@ struct cmd_reset_result { /* function called when cmd_reset is parsed successfully */ static void cmd_reset_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + hostsim_exit(); +#endif reset(); } @@ -98,7 +102,11 @@ struct cmd_bootloader_result { /* function called when cmd_bootloader is parsed successfully */ static void cmd_bootloader_parsed(void *parsed_result, void *data) { +#ifndef HOST_VERSION bootloader(); +#else + printf("not implemented\n"); +#endif } prog_char str_bootloader_arg0[] = "bootloader"; @@ -127,6 +135,9 @@ struct cmd_encoders_result { /* function called when cmd_encoders is parsed successfully */ static void cmd_encoders_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_encoders_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("reset"))) { @@ -146,6 +157,7 @@ static void cmd_encoders_parsed(void *parsed_result, void *data) encoders_spi_get_value((void *)3)); wait_ms(100); } +#endif } prog_char str_encoders_arg0[] = "encoders"; @@ -211,6 +223,9 @@ struct cmd_pwm_result { /* function called when cmd_pwm is parsed successfully */ static void cmd_pwm_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else void * pwm_ptr = NULL; struct cmd_pwm_result * res = parsed_result; @@ -236,6 +251,7 @@ static void cmd_pwm_parsed(void * parsed_result, void * data) pwm_ng_set(pwm_ptr, res->arg2); printf_P(PSTR("done\r\n")); +#endif } prog_char str_pwm_arg0[] = "pwm"; @@ -269,6 +285,9 @@ struct cmd_adc_result { /* function called when cmd_adc is parsed successfully */ static void cmd_adc_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_adc_result *res = parsed_result; uint8_t i, loop = 0; @@ -283,6 +302,7 @@ static void cmd_adc_parsed(void *parsed_result, void *data) printf_P(PSTR("\r\n")); wait_ms(100); } while (loop && !cmdline_keypressed()); +#endif } prog_char str_adc_arg0[] = "adc"; @@ -315,6 +335,9 @@ struct cmd_sensor_result { /* function called when cmd_sensor is parsed successfully */ static void cmd_sensor_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_sensor_result *res = parsed_result; uint8_t i, loop = 0; @@ -329,6 +352,7 @@ static void cmd_sensor_parsed(void *parsed_result, void *data) printf_P(PSTR("\r\n")); wait_ms(100); } while (loop && !cmdline_keypressed()); +#endif } prog_char str_sensor_arg0[] = "sensor"; @@ -426,12 +450,16 @@ static void cmd_log_do_show(void) for (i=0; i #include +#include #include #include #include @@ -56,6 +57,7 @@ #include "../common/eeprom_mapping.h" #include "main.h" +#include "robotsim.h" #include "sensor.h" #include "cmdline.h" #include "strat.h" @@ -127,8 +129,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) { if (!strcmp_P(res->arg1, PSTR("cs"))) { +#ifdef HOST_VERSION + robotsim_pwm(LEFT_PWM, 0); + robotsim_pwm(RIGHT_PWM, 0); +#else pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); +#endif } mainboard.flags &= (~bit); } @@ -168,6 +175,9 @@ struct cmd_spi_test_result { /* function called when cmd_spi_test is parsed successfully */ static void cmd_spi_test_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else uint16_t i = 0, ret = 0, ret2 = 0; if (mainboard.flags & DO_ENCODERS) { @@ -187,6 +197,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) i++; } while(!cmdline_keypressed()); +#endif } prog_char str_spi_test_arg0[] = "spi_test"; @@ -278,6 +289,9 @@ struct cmd_start_result { /* function called when cmd_start is parsed successfully */ static void cmd_start_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_start_result *res = parsed_result; uint8_t old_level = gen.log_level; @@ -306,6 +320,7 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; +#endif } prog_char str_start_arg0[] = "start"; @@ -340,8 +355,8 @@ struct cmd_interact_result { static void print_cs(void) { - printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld " - "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), + printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -385,8 +400,8 @@ static void print_sensors(void) static void print_pid(void) { - printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | " - "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | " + "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), @@ -542,6 +557,9 @@ struct cmd_color_result { /* function called when cmd_color is parsed successfully */ static void cmd_color_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; if (!strcmp_P(res->color, PSTR("yellow"))) { mainboard.our_color = I2C_COLOR_YELLOW; @@ -554,6 +572,7 @@ static void cmd_color_parsed(void *parsed_result, void *data) i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } printf_P(PSTR("Done\r\n")); +#endif } prog_char str_color_arg0[] = "color"; @@ -588,15 +607,15 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "), + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_feedback(&mainboard.angle.cs), cs_get_out(&mainboard.angle.cs)); - printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "), + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.distance.cs), cs_get_filtered_feedback(&mainboard.distance.cs), cs_get_out(&mainboard.distance.cs)); - printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l, + printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l, mainboard.pwm_r); wait_ms(100); } while(!cmdline_keypressed()); @@ -630,8 +649,12 @@ struct cmd_i2cdebug_result { /* function called when cmd_i2cdebug is parsed successfully */ static void cmd_i2cdebug_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else i2c_debug(); i2c_protocol_debug(); +#endif } prog_char str_i2cdebug_arg0[] = "i2cdebug"; @@ -660,11 +683,15 @@ struct cmd_cobboard_show_result { /* function called when cmd_cobboard_show is parsed successfully */ static void cmd_cobboard_show_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else printf_P(PSTR("mode = %x\r\n"), cobboard.mode); printf_P(PSTR("status = %x\r\n"), cobboard.status); printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count); printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed); printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed); +#endif } prog_char str_cobboard_show_arg0[] = "cobboard"; @@ -696,12 +723,16 @@ struct cmd_cobboard_setmode1_result { /* function called when cmd_cobboard_setmode1 is parsed successfully */ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) i2c_cobboard_mode_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_mode_eject(); +#endif } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -734,6 +765,9 @@ struct cmd_cobboard_setmode2_result { /* function called when cmd_cobboard_setmode2 is parsed successfully */ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -748,6 +782,7 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) i2c_cobboard_mode_harvest(side); else if (!strcmp_P(res->arg1, PSTR("pack"))) i2c_cobboard_mode_pack(side); +#endif } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; @@ -783,9 +818,13 @@ struct cmd_cobboard_setmode3_result { /* function called when cmd_cobboard_setmode3 is parsed successfully */ static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_cobboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); +#endif } prog_char str_cobboard_setmode3_arg0[] = "cobboard"; @@ -819,9 +858,13 @@ struct cmd_ballboard_show_result { /* function called when cmd_ballboard_show is parsed successfully */ static void cmd_ballboard_show_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else printf_P(PSTR("mode = %x\r\n"), ballboard.mode); printf_P(PSTR("status = %x\r\n"), ballboard.status); printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count); +#endif } prog_char str_ballboard_show_arg0[] = "ballboard"; @@ -853,6 +896,9 @@ struct cmd_ballboard_setmode1_result { /* function called when cmd_ballboard_setmode1 is parsed successfully */ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_ballboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) @@ -865,6 +911,7 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* other commands */ +#endif } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; @@ -897,6 +944,9 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_ballboard_setmode2_result *res = parsed_result; uint8_t mode = I2C_BALLBOARD_MODE_INIT; @@ -913,6 +963,7 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } i2c_ballboard_set_mode(mode); +#endif } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; @@ -948,9 +999,13 @@ struct cmd_ballboard_setmode3_result { /* function called when cmd_ballboard_setmode3 is parsed successfully */ static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_ballboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); +#endif } prog_char str_ballboard_setmode3_arg0[] = "ballboard"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 55bda53..81e2e04 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -144,7 +145,7 @@ static void cmd_circle_coef_parsed(void *parsed_result, void *data) trajectory_set_circle_coef(&mainboard.traj, res->circle_coef); } - printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef); + printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef); } prog_char str_circle_coef_arg0[] = "circle_coef"; @@ -264,6 +265,9 @@ struct cmd_rs_gains_result { /* function called when cmd_rs_gains is parsed successfully */ static void cmd_rs_gains_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_rs_gains_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("set"))) { @@ -274,6 +278,7 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data) } printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"), mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain); +#endif } prog_char str_rs_gains_arg0[] = "rs_gains"; @@ -395,14 +400,13 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) struct cmd_pt_list_result * res = parsed_result; uint8_t i, why=0; - if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { - printf_P(PSTR("not implemented\r\n")); - return; - } - if (!strcmp_P(res->arg1, PSTR("append"))) { res->arg2 = pt_list_len; } + if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + printf_P(PSTR("removed\r\n")); + return; + } if (!strcmp_P(res->arg1, PSTR("insert")) || !strcmp_P(res->arg1, PSTR("append"))) { @@ -442,12 +446,17 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("List empty\r\n")); return; } + restart: for (i=0 ; iarg1, PSTR("start"))) { trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); why = wait_traj_end(0xFF); /* all */ } + else if (!strcmp_P(res->arg1, PSTR("loop_start"))) { + 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) { @@ -460,7 +469,13 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) #endif if (why & (~(END_TRAJ | END_NEAR))) trajectory_stop(&mainboard.traj); + if (why & END_INTR) + break; } + if (why & END_INTR) + return; + if (!strcmp_P(res->arg1, PSTR("loop_start"))) + goto restart; } prog_char str_pt_list_arg0[] = "pt_list"; @@ -524,7 +539,7 @@ parse_pgm_inst_t cmd_pt_list_del = { }; /* show */ -prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start"; parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); prog_char help_pt_list_show[] = "Show, start or reset pt_list"; @@ -745,14 +760,18 @@ static void cmd_position_parsed(void * parsed_result, void * data) } else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) { mainboard.our_color = I2C_COLOR_BLUE; +#ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); +#endif auto_position(); } else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { mainboard.our_color = I2C_COLOR_YELLOW; +#ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); +#endif auto_position(); } diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 39e39ef..81e5245 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -51,9 +52,11 @@ #include #include "main.h" +#include "robotsim.h" #include "strat.h" #include "actuator.h" +#ifndef HOST_VERSION int32_t encoders_left_cobroller_speed(void *number) { static volatile int32_t roller_pos; @@ -73,21 +76,23 @@ int32_t encoders_right_cobroller_speed(void *number) roller_pos = tmp; return speed; } +#endif /* called every 5 ms */ -static void do_cs(void *dummy) +static void do_cs(void *dummy) { static uint16_t cpt = 0; static int32_t old_a = 0, old_d = 0; +#ifdef HOST_VERSION + robotsim_update(); +#else /* read encoders */ if (mainboard.flags & DO_ENCODERS) { encoders_spi_manage(NULL); } +#endif - /* XXX there is an issue which is probably related to avr-libc - * 1.6.2 (debian): this code using fixed_point lib does not - * work with it */ /* robot system, conversion to angle,distance */ if (mainboard.flags & DO_RS) { int16_t a,d; @@ -107,10 +112,12 @@ static void do_cs(void *dummy) cs_manage(&mainboard.angle.cs); if (mainboard.distance.on) cs_manage(&mainboard.distance.cs); +#ifndef HOST_VERSION if (mainboard.left_cobroller.on) cs_manage(&mainboard.left_cobroller.cs); if (mainboard.right_cobroller.on) cs_manage(&mainboard.right_cobroller.cs); +#endif } if ((cpt & 1) && (mainboard.flags & DO_POS)) { /* about 1.5ms (worst case without centrifugal force @@ -120,9 +127,12 @@ static void do_cs(void *dummy) if (mainboard.flags & DO_BD) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); +#ifndef HOST_VERSION bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs); bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs); +#endif } +#ifndef HOST_VERSION if (mainboard.flags & DO_TIMER) { uint8_t second; /* the robot should stop correctly in the strat, but @@ -136,18 +146,24 @@ static void do_cs(void *dummy) while(1); } } +#endif /* brakes */ if (mainboard.flags & DO_POWER) BRAKE_OFF(); else BRAKE_ON(); cpt++; + +#ifdef HOST_VERSION + if ((cpt & 7) == 0) + robotsim_dump(); +#endif } void dump_cs_debug(const char *name, struct cs *cs) { - DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld", + DEBUG(E_USER_CS, "%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" " + "in=% .5"PRIi32" out=% .5"PRIi32"", name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -155,8 +171,8 @@ void dump_cs_debug(const char *name, struct cs *cs) void dump_cs(const char *name, struct cs *cs) { - printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), + printf_P(PSTR("%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" " + "in=% .5"PRIi32" out=% .5"PRIi32"\r\n"), name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -164,7 +180,7 @@ void dump_cs(const char *name, struct cs *cs) void dump_pid(const char *name, struct pid_filter *pid) { - printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + printf_P(PSTR("%s P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), name, pid_get_value_in(pid) * pid_get_gain_P(pid), pid_get_value_I(pid) * pid_get_gain_I(pid), @@ -179,10 +195,17 @@ void microb_cs_init(void) rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM); /* increase gain to decrease dist, increase left and it will turn more left */ - rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, +#ifdef HOST_VERSION + rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get, + LEFT_ENCODER, IMP_COEF); + rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get, + RIGHT_ENCODER, IMP_COEF); +#else + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, LEFT_ENCODER, IMP_COEF * -1.00); - rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, RIGHT_ENCODER, IMP_COEF * 1.00); +#endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -254,6 +277,7 @@ void microb_cs_init(void) bd_set_speed_threshold(&mainboard.distance.bd, 60); bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); +#ifndef HOST_VERSION /* ---- CS left_cobroller */ /* PID */ pid_init(&mainboard.left_cobroller.pid); @@ -293,6 +317,7 @@ void microb_cs_init(void) 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); +#endif /* !HOST_VERSION */ /* set them on !! */ mainboard.angle.on = 0; diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py new file mode 100644 index 0000000..239b2d6 --- /dev/null +++ b/projects/microb2010/mainboard/display.py @@ -0,0 +1,282 @@ +import math, sys, time, os, random +from visual import * + +AREA_X = 3000. +AREA_Y = 2100. + +area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ] +areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area) +area_box = box(size=areasize, color=(0.0, 1.0, 0.0)) + +scene.autoscale=0 + +# all positions of robot every 5ms +save_pos = [] + +robot = box(pos = (0, 0, 150), + size = (250,320,350), + color = (1, 0, 0) ) + +last_pos = robot.pos.x, robot.pos.y, robot.pos.z +hcenter_line = curve() +hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)] +vcenter_line = curve() +vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)] + +def square(sz): + sq = curve() + sq.pos = [(-sz, -sz, 0.3), + (-sz, sz, 0.3), + (sz, sz, 0.3), + (sz, -sz, 0.3), + (-sz, -sz, 0.3),] + return sq + +sq1 = square(250) +sq2 = square(500) + +robot_trail = curve() +robot_trail_list = [] +max_trail = 500 + +area_objects = [] + +OFFSET_CORN_X=150 +OFFSET_CORN_Y=222 +STEP_CORN_X=225 +STEP_CORN_Y=250 + +WAYPOINTS_NBX = 13 +WAYPOINTS_NBY = 8 + +TYPE_WAYPOINT=0 +TYPE_DANGEROUS=1 +TYPE_WHITE_CORN=2 +TYPE_BLACK_CORN=3 +TYPE_OBSTACLE=4 +TYPE_NEIGH=5 + +col = [TYPE_WAYPOINT] * WAYPOINTS_NBY +waypoints = [col[:] for i in range(WAYPOINTS_NBX)] +corn_table = [TYPE_WHITE_CORN]*18 + +corn_side_confs = [ + [ 1, 4 ], + [ 0, 4 ], + [ 2, 4 ], + [ 2, 3 ], + [ 0, 3 ], + [ 1, 3 ], + [ 1, 6 ], + [ 0, 6 ], + [ 2, 6 ], + ] +corn_center_confs = [ + [ 5, 8 ], + [ 7, 8 ], + [ 5, 9 ], + [ 7, 8 ], + ] + +def pt2corn(i,j): + if i == 0 and j == 2: return 0 + if i == 0 and j == 4: return 1 + if i == 0 and j == 6: return 2 + if i == 2 and j == 3: return 3 + if i == 2 and j == 5: return 4 + if i == 2 and j == 7: return 5 + if i == 4 and j == 4: return 6 + if i == 4 and j == 6: return 7 + if i == 6 and j == 5: return 8 + if i == 6 and j == 7: return 9 + if i == 8 and j == 4: return 10 + if i == 8 and j == 6: return 11 + if i == 10 and j == 3: return 12 + if i == 10 and j == 5: return 13 + if i == 10 and j == 7: return 14 + if i == 12 and j == 2: return 15 + if i == 12 and j == 4: return 16 + if i == 12 and j == 6: return 17 + return -1 + +def corn_get_sym(i): + sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2] + return sym[i] + +def init_corn_table(conf_side, conf_center): + global corn_table, corn_side_confs, corn_center_confs + print "confs = %d, %d"%(conf_side, conf_center) + for i in range(18): + if i in corn_side_confs[conf_side]: + corn_table[i] = TYPE_BLACK_CORN + continue + if corn_get_sym(i) in corn_side_confs[conf_side]: + corn_table[i] = TYPE_BLACK_CORN + continue + if i in corn_center_confs[conf_center]: + corn_table[i] = TYPE_BLACK_CORN + continue + if corn_get_sym(i) in corn_center_confs[conf_center]: + corn_table[i] = TYPE_BLACK_CORN + continue + corn_table[i] = TYPE_WHITE_CORN + +def init_waypoints(): + global waypoints, corn_table + + for i in range(WAYPOINTS_NBX): + for j in range(WAYPOINTS_NBY): + # corn + c = pt2corn(i, j) + if c >= 0: + waypoints[i][j] = corn_table[c] + continue + # too close of border + if (i & 1) == 1 and j == WAYPOINTS_NBY -1: + waypoints[i][j] = TYPE_OBSTACLE + continue + # hill + if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2: + waypoints[i][j] = TYPE_OBSTACLE + continue + # dangerous points + if i == 0 or i == WAYPOINTS_NBX-1: + waypoints[i][j] = TYPE_DANGEROUS + continue + if (i&1) == 0 and j == WAYPOINTS_NBY-1: + waypoints[i][j] = TYPE_DANGEROUS + continue + + waypoints[i][j] = TYPE_WAYPOINT + + print i, waypoints[i] + return waypoints + + +def toggle_obj_disp(): + global area_objects + + if area_objects == []: + i = 0 + j = 0 + x = OFFSET_CORN_X + while x < 3000: + if (i & 1) == 0: + y = OFFSET_CORN_Y + else: + y = OFFSET_CORN_Y + STEP_CORN_Y/2 + j = 0 + while y < 2100: + print x,y + if waypoints[i][j] == TYPE_WHITE_CORN: + c = cylinder(axis=(0,0,1), length=150, + radius=25, color=(0.8,0.8,0.8), + pos=(x-AREA_X/2,y-AREA_Y/2,75)) + area_objects.append(c) + elif waypoints[i][j] == TYPE_BLACK_CORN: + c = cylinder(axis=(0,0,1), length=150, + radius=25, color=(0.2,0.2,0.2), + pos=(x-AREA_X/2,y-AREA_Y/2,75)) + area_objects.append(c) + j += 1 + y += STEP_CORN_Y + i += 1 + x += STEP_CORN_X + else: + for o in area_objects: + if o.visible: + o.visible = 0 + else: + o.visible = 1 + + +def set_robot(x, y, a): + global robot, last_pos, robot_trail, robot_trail_list + global save_pos + + robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150) + robot.axis = (math.cos(a*math.pi/180), + math.sin(a*math.pi/180), + 0) + robot.size = (250, 320, 350) + + # save position + save_pos.append((robot.pos.x, robot.pos, a)) + + pos = robot.pos.x, robot.pos.y, 0.3 + if pos != last_pos: + robot_trail_list.append(pos) + last_pos = pos + robot_trail_l = len(robot_trail_list) + if robot_trail_l > max_trail: + robot_trail_list = robot_trail_list[robot_trail_l - max_trail:] + robot_trail.pos = robot_trail_list + +def graph(): + pass + +def save(): + f = open("/tmp/robot_save", "w") + for p in save_pos: + f.write("%f %f %f\n"%(p[0], p[1], p[2])) + f.close() + +def silent_mkfifo(f): + try: + os.mkfifo(f) + except: + pass + +init_corn_table(random.randint(0,8), random.randint(0,3)) +waypoints = init_waypoints() +toggle_obj_disp() + +while True: + silent_mkfifo("/tmp/.robot_sim2dis") + silent_mkfifo("/tmp/.robot_dis2sim") + while True: + fr = open("/tmp/.robot_sim2dis", "r") + fw = open("/tmp/.robot_dis2sim", "w", 0) + while True: + l = fr.readline() + try: + x,y,a = map(lambda x:int(x), l[:-1].split(" ")) + set_robot(x,y,a) + except ValueError: + pass + + if scene.kb.keys == 0: + continue + + k = scene.kb.getkey() + x,y,z = scene.center + if k == "left": + scene.center = x-10,y,z + elif k == "right": + scene.center = x+10,y,z + elif k == "up": + scene.center = x,y+10,z + elif k == "down": + scene.center = x,y-10,z + elif k == "l": + fw.write("l") + elif k == "r": + fw.write("r") + elif k == "c": + robot_trail_list = [] + elif k == "x": + save_pos = [] + elif k == "g": + graph() + elif k == "s": + save() + elif k == "h": + toggle_obj_disp() + + # EOF + if l == "": + break + + fr.close() + fw.close() + diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 403decd..4e5c174 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -1,6 +1,6 @@ /* * 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 @@ -70,16 +70,19 @@ static volatile uint16_t i2c_errors = 0; static volatile uint8_t running_op = OP_READY; #define I2C_MAX_LOG 3 +#ifndef HOST_VERSION static uint8_t error_log = 0; +static int8_t i2c_req_cobboard_status(void); +static int8_t i2c_req_ballboard_status(void); + +#endif + /* used for commands */ uint8_t command_buf[I2C_SEND_BUFFER_SIZE]; volatile int8_t command_dest=-1; volatile uint8_t command_size=0; -static int8_t i2c_req_cobboard_status(void); -static int8_t i2c_req_ballboard_status(void); - #define I2C_ERROR(args...) do { \ if (error_log < I2C_MAX_LOG) { \ ERROR(E_USER_I2C_PROTO, args); \ @@ -99,6 +102,9 @@ void i2c_protocol_init(void) void i2c_protocol_debug(void) { +#ifdef HOST_VERSION + return; +#else printf_P(PSTR("I2C protocol debug infos:\r\n")); printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state); printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors); @@ -106,8 +112,10 @@ void i2c_protocol_debug(void) printf_P(PSTR(" command_size=%d\r\n"), command_size); printf_P(PSTR(" command_dest=%d\r\n"), command_dest); printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status()); +#endif } +#ifndef HOST_VERSION static void i2cproto_next_state(uint8_t inc) { i2c_state += inc; @@ -131,11 +139,11 @@ void i2c_poll_slaves(void *dummy) uint8_t flags; int8_t err; static uint8_t a = 0; - + a++; if (a & 0x4) LED2_TOGGLE(); - + /* already running */ IRQ_LOCK(flags); if (running_op != OP_READY) { @@ -168,8 +176,8 @@ void i2c_poll_slaves(void *dummy) #define I2C_ANS_COBBOARD 1 case I2C_ANS_COBBOARD: - if ((err = i2c_recv(I2C_COBBOARD_ADDR, - sizeof(struct i2c_ans_cobboard_status), + if ((err = i2c_recv(I2C_COBBOARD_ADDR, + sizeof(struct i2c_ans_cobboard_status), I2C_CTRL_GENERIC))) goto error; break; @@ -183,8 +191,8 @@ void i2c_poll_slaves(void *dummy) #define I2C_ANS_BALLBOARD 3 case I2C_ANS_BALLBOARD: - if ((err = i2c_recv(I2C_BALLBOARD_ADDR, - sizeof(struct i2c_ans_ballboard_status), + if ((err = i2c_recv(I2C_BALLBOARD_ADDR, + sizeof(struct i2c_ans_ballboard_status), I2C_CTRL_GENERIC))) goto error; break; @@ -203,7 +211,7 @@ void i2c_poll_slaves(void *dummy) IRQ_UNLOCK(flags); i2c_errors++; if (i2c_errors > I2C_MAX_ERRORS) { - I2C_ERROR("I2C send is_cmd=%d proto_state=%d " + I2C_ERROR("I2C send is_cmd=%d proto_state=%d " "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status()); i2c_reset(); i2c_errors = 0; @@ -229,7 +237,7 @@ void i2c_sendevent(int8_t size) i2c_reset(); i2c_errors = 0; } - + if (running_op == OP_POLL) { /* skip associated answer */ i2cproto_next_state(2); @@ -246,7 +254,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* recv is only trigged after a poll */ running_op = OP_READY; - + if (size < 0) { goto error; } @@ -254,9 +262,9 @@ void i2c_recvevent(uint8_t * buf, int8_t size) switch (buf[0]) { case I2C_ANS_COBBOARD_STATUS: { - struct i2c_ans_cobboard_status * ans = + struct i2c_ans_cobboard_status * ans = (struct i2c_ans_cobboard_status *)buf; - + if (size != sizeof (*ans)) goto error; @@ -272,7 +280,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) } case I2C_ANS_BALLBOARD_STATUS: { - struct i2c_ans_ballboard_status * ans = + struct i2c_ans_ballboard_status * ans = (struct i2c_ans_ballboard_status *)buf; if (size != sizeof (*ans)) @@ -290,7 +298,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) return; error: i2c_errors++; - NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d", + NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d", i2c_state, running_op); if (i2c_errors > I2C_MAX_ERRORS) { I2C_ERROR("I2C error, slave not ready"); @@ -302,6 +310,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) { } +#endif /* !HOST_VERSION */ /* ******** ******** ******** ******** */ /* commands */ @@ -309,8 +318,11 @@ void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) static int8_t -i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) +i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) { +#ifdef HOST_VERSION + return 0; +#else uint8_t flags; microseconds us = time_get_us2(); @@ -329,8 +341,10 @@ i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) * interrupt context, but it's forbidden */ I2C_ERROR("I2C command send failed"); return -EBUSY; +#endif } +#ifndef HOST_VERSION static int8_t i2c_req_cobboard_status(void) { struct i2c_req_cobboard_status buf; @@ -346,11 +360,12 @@ static int8_t i2c_req_cobboard_status(void) static int8_t i2c_req_ballboard_status(void) { struct i2c_req_ballboard_status buf; - + buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS; return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); } +#endif /* !HOST_VERSION */ int8_t i2c_set_color(uint8_t addr, uint8_t color) { diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index aa76651..4061d19 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -59,12 +59,14 @@ #include "../common/i2c_commands.h" #include "main.h" +#include "robotsim.h" #include "ax12_user.h" #include "strat.h" #include "cmdline.h" #include "sensor.h" #include "actuator.h" #include "cs.h" +#include "strat_base.h" #include "i2c_protocol.h" /* 0 means "programmed" @@ -84,6 +86,7 @@ struct mainboard mainboard; struct cobboard cobboard; struct ballboard ballboard; +#ifndef HOST_VERSION /***********************/ void bootloader(void) @@ -143,11 +146,11 @@ static void main_timer_interrupt(void) if ((cpt & 0x3) == 0) scheduler_interrupt(); } +#endif int main(void) { - uint16_t seconds; - +#ifndef HOST_VERSION /* brake */ BRAKE_DDR(); BRAKE_OFF(); @@ -163,6 +166,7 @@ int main(void) LED2_OFF(); LED3_OFF(); LED4_OFF(); +#endif memset(&gen, 0, sizeof(gen)); memset(&mainboard, 0, sizeof(mainboard)); @@ -171,17 +175,15 @@ int main(void) /* UART */ uart_init(); + uart_register_rx_event(CMDLINE_UART, emergency); +#ifndef HOST_VERSION #if CMDLINE_UART == 3 fdevopen(uart3_dev_send, uart3_dev_recv); - uart_register_rx_event(3, emergency); #elif CMDLINE_UART == 1 fdevopen(uart1_dev_send, uart1_dev_recv); - uart_register_rx_event(1, emergency); -#else -# error not supported #endif - eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_MAINBOARD) { @@ -189,6 +191,7 @@ int main(void) printf_P(PSTR("Bad eeprom value\r\n")); while(1); } +#endif /* ! HOST_VERSION */ /* LOGS */ error_register_emerg(mylog); @@ -197,6 +200,7 @@ int main(void) error_register_notice(mylog); error_register_debug(mylog); +#ifndef HOST_VERSION /* SPI + ENCODERS */ encoders_spi_init(); /* this will also init spi hardware */ @@ -240,16 +244,25 @@ int main(void) PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); support_balls_deploy(); /* init pwm for servos */ +#endif /* !HOST_VERSION */ /* SCHEDULER */ scheduler_init(); +#ifdef HOST_VERSION + hostsim_init(); + robotsim_init(); +#endif +#ifndef HOST_VERSION scheduler_add_periodical_event_priority(do_led_blink, NULL, 100000L / SCHEDULER_UNIT, LED_PRIO); +#endif /* !HOST_VERSION */ + /* all cs management */ microb_cs_init(); +#ifndef HOST_VERSION /* sensors, will also init hardware adc */ sensor_init(); @@ -259,6 +272,7 @@ int main(void) /* start i2c slave polling */ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); +#endif /* !HOST_VERSION */ /* strat */ gen.logs[0] = E_USER_STRAT; @@ -270,17 +284,29 @@ int main(void) 25000L / SCHEDULER_UNIT, STRAT_PRIO); +#ifndef HOST_VERSION /* eeprom time monitor */ scheduler_add_periodical_event_priority(do_time_monitor, NULL, 1000000L / SCHEDULER_UNIT, EEPROM_TIME_PRIO); +#endif /* !HOST_VERSION */ sei(); printf_P(PSTR("\r\n")); printf_P(PSTR("Respect et robustesse.\r\n")); - seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); - printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +#ifndef HOST_VERSION + { + uint16_t seconds; + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); + } +#endif + +#ifdef HOST_VERSION + strat_reset_pos(1000, 1000, -90); +#endif + cmdline_interact(); return 0; diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index c248644..53cfe69 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -26,6 +26,29 @@ port |= _BV(bit); \ } while(0) +#ifdef HOST_VERSION +#define LED1_ON() +#define LED1_OFF() +#define LED1_TOGGLE() + +#define LED2_ON() +#define LED2_OFF() +#define LED2_TOGGLE() + +#define LED3_ON() +#define LED3_OFF() +#define LED3_TOGGLE() + +#define LED4_ON() +#define LED4_OFF() +#define LED4_TOGGLE() + +#define BRAKE_DDR() +#define BRAKE_ON() +#define BRAKE_OFF() + +#else + #define LED1_ON() sbi(PORTJ, 2) #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) @@ -45,6 +68,7 @@ #define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0) #define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) #define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) +#endif /* only 90 seconds, don't forget it :) */ #define MATCH_TIME 89 diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c new file mode 100644 index 0000000..15b1075 --- /dev/null +++ b/projects/microb2010/mainboard/robotsim.c @@ -0,0 +1,186 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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.9.4.5 2007-06-01 09:37:22 zer0 Exp $ + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "strat.h" +#include "strat_utils.h" +#include "main.h" + +static int32_t l_pwm, r_pwm; +static int32_t l_enc, r_enc; + +static int fdr, fdw; + +/* */ +#define FILTER 97 +#define FILTER2 (100-FILTER) +#define SHIFT 4 + +void robotsim_dump(void) +{ + char buf[BUFSIZ]; + int len; + + len = snprintf(buf, sizeof(buf), "%d %d %d\n", + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); +} + +/* must be called periodically */ +void robotsim_update(void) +{ + static int32_t l_pwm_shift[SHIFT]; + static int32_t r_pwm_shift[SHIFT]; + static int32_t l_speed, r_speed; + static unsigned i = 0; + static unsigned cpt = 0; + int32_t local_l_pwm, local_r_pwm; + double x, y, a, a2, d; + char cmd = 0; + + /* corners of the robot */ + double xfl, yfl; /* front left */ + double xrl, yrl; /* rear left */ + double xrr, yrr; /* rear right */ + double xfr, yfr; /* front right */ + + /* time shift the command */ + l_pwm_shift[i] = l_pwm; + r_pwm_shift[i] = r_pwm; + i ++; + i %= SHIFT; + local_l_pwm = l_pwm_shift[i]; + local_r_pwm = r_pwm_shift[i]; + + /* read command */ + if (((cpt ++) & 0x7) == 0) { + if (read(fdr, &cmd, 1) != 1) + cmd = 0; + } + + x = position_get_x_double(&mainboard.pos); + y = position_get_y_double(&mainboard.pos); + a = position_get_a_rad_double(&mainboard.pos); + + l_speed = ((l_speed * FILTER) / 100) + + ((local_l_pwm * 1000 * FILTER2)/1000); + r_speed = ((r_speed * FILTER) / 100) + + ((local_r_pwm * 1000 * FILTER2)/1000); + + /* basic collision detection */ + a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + + xfl = x + cos(a+a2) * d; + yfl = y + sin(a+a2) * d; + if (!is_in_area(xfl, yfl, 0) && l_speed > 0) + l_speed = 0; + + xrl = x + cos(a+M_PI-a2) * d; + yrl = y + sin(a+M_PI-a2) * d; + if (!is_in_area(xrl, yrl, 0) && l_speed < 0) + l_speed = 0; + + xrr = x + cos(a+M_PI+a2) * d; + yrr = y + sin(a+M_PI+a2) * d; + if (!is_in_area(xrr, yrr, 0) && r_speed < 0) + r_speed = 0; + + xfr = x + cos(a-a2) * d; + yfr = y + sin(a-a2) * d; + if (!is_in_area(xfr, yfr, 0) && r_speed > 0) + r_speed = 0; + + /* perturbation */ + if (cmd == 'l') + l_enc += 5000; /* push 1 cm */ + if (cmd == 'r') + r_enc += 5000; /* push 1 cm */ + + /* XXX should lock */ + l_enc += (l_speed / 1000); + r_enc += (r_speed / 1000); +} + +void robotsim_pwm(void *arg, int32_t val) +{ + // printf("%p, %d\n", arg, val); + if (arg == LEFT_PWM) + l_pwm = val; + else if (arg == RIGHT_PWM) + r_pwm = val; +} + +int32_t robotsim_encoder_get(void *arg) +{ + if (arg == LEFT_ENCODER) + return l_enc; + else if (arg == RIGHT_ENCODER) + return r_enc; + return 0; +} + +int robotsim_init(void) +{ + mkfifo("/tmp/.robot_sim2dis", 0600); + mkfifo("/tmp/.robot_dis2sim", 0600); + fdw = open("/tmp/.robot_sim2dis", O_WRONLY, 0); + if (fdw < 0) + return -1; + fdr = open("/tmp/.robot_dis2sim", O_RDONLY | O_NONBLOCK, 0); + if (fdr < 0) { + close(fdw); + return -1; + } + return 0; +} diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h new file mode 100644 index 0000000..8777de8 --- /dev/null +++ b/projects/microb2010/mainboard/robotsim.h @@ -0,0 +1,26 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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.9.4.5 2007-06-01 09:37:22 zer0 Exp $ + * + */ + +void robotsim_update(void); +void robotsim_pwm(void *arg, int32_t val); +int32_t robotsim_encoder_get(void *arg); +int robotsim_init(void); +void robotsim_dump(void); diff --git a/projects/microb2010/mainboard/scheduler_config.h b/projects/microb2010/mainboard/scheduler_config.h index daf5853..944cc80 100755 --- a/projects/microb2010/mainboard/scheduler_config.h +++ b/projects/microb2010/mainboard/scheduler_config.h @@ -27,9 +27,13 @@ /** maximum number of allocated events */ #define SCHEDULER_NB_MAX_EVENT 10 - +#ifdef HOST_VERSION +#define SCHEDULER_UNIT_FLOAT 1000.0 +#define SCHEDULER_UNIT 1000UL +#else #define SCHEDULER_UNIT_FLOAT 512.0 #define SCHEDULER_UNIT 512L +#endif /** number of allowed imbricated scheduler interrupts. The maximum * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index 6ecff35..a6f85de 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -48,6 +48,8 @@ #include "main.h" #include "sensor.h" +#ifndef HOST_VERSION + /************ ADC */ struct adc_infos { @@ -82,7 +84,7 @@ int16_t rii_strong(struct adc_infos *adc, int16_t val) #define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) /* define which ADC to poll, see in sensor.h */ -static struct adc_infos adc_infos[ADC_MAX] = { +static struct adc_infos adc_infos[ADC_MAX] = { [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium }, [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium }, [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium }, @@ -100,7 +102,7 @@ static struct adc_infos adc_infos[ADC_MAX] = { static void adc_event(int16_t result); /* called every 10 ms, see init below */ -static void do_adc(void *dummy) +static void do_adc(void *dummy) { /* launch first conversion */ adc_launch(adc_infos[0].config); @@ -123,9 +125,13 @@ static void adc_event(int16_t result) else adc_launch(adc_infos[i].config); } +#endif /* !HOST_VERSION */ int16_t sensor_get_adc(uint8_t i) { +#ifdef HOST_VERSION + return 0; +#else int16_t tmp; uint8_t flags; @@ -133,11 +139,12 @@ int16_t sensor_get_adc(uint8_t i) tmp = adc_infos[i].value; IRQ_UNLOCK(flags); return tmp; +#endif } /************ boolean sensors */ - +#ifndef HOST_VERSION struct sensor_filter { uint8_t filter; uint8_t prev; @@ -168,11 +175,12 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */ [S_RESERVED8] = { 1, 0, 0, 1, 0, 0 }, /* 15 */ }; +#endif /* !HOST_VERSION */ /* value of filtered sensors */ static uint16_t sensor_filtered = 0; -/* sensor mapping : +/* sensor mapping : * 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13) * 4-5: PORTL 0->1 (cap5 -> cap6) * 6-7: PORTE 3->4 (cap7 -> cap8) @@ -191,10 +199,15 @@ uint16_t sensor_get_all(void) uint8_t sensor_get(uint8_t i) { +#ifdef HOST_VERSION + return 0; +#else uint16_t tmp = sensor_get_all(); return (tmp & _BV(i)); +#endif } +#ifndef HOST_VERSION /* get the physical value of pins */ static uint16_t sensor_read(void) { @@ -227,7 +240,7 @@ static void do_boolean_sensors(void *dummy) if (sensor_filter[i].cpt <= sensor_filter[i].thres_off) sensor_filter[i].prev = 0; } - + if (sensor_filter[i].prev) { tmp |= (1UL << i); } @@ -236,11 +249,12 @@ static void do_boolean_sensors(void *dummy) sensor_filtered = tmp; IRQ_UNLOCK(flags); } +#endif /* !HOST_VERSION */ /* virtual obstacle */ #define DISABLE_CPT_MAX 200 -static uint8_t disable = 0; /* used to disable obstacle detection +static uint8_t disable = 0; /* used to disable obstacle detection * during some time */ /* called every 10 ms */ @@ -273,6 +287,7 @@ uint8_t sensor_obstacle_is_disabled(void) /************ global sensor init */ +#ifndef HOST_VERSION /* called every 10 ms, see init below */ static void do_sensors(void *dummy) { @@ -280,14 +295,19 @@ static void do_sensors(void *dummy) do_boolean_sensors(NULL); sensor_obstacle_update(); } +#endif void sensor_init(void) { +#ifdef HOST_VERSION + return; +#else adc_init(); adc_register_event(adc_event); /* CS EVENT */ - scheduler_add_periodical_event_priority(do_sensors, NULL, - 10000L / SCHEDULER_UNIT, + scheduler_add_periodical_event_priority(do_sensors, NULL, + 10000L / SCHEDULER_UNIT, ADC_PRIO); +#endif } -- 2.20.1 From 48ea32b5f2e7e3eba76955e458066eac050ce965 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 16:31:13 +0200 Subject: [PATCH 04/16] merge hostsim in main --- include/aversive/errno.h | 5 +++-- projects/microb2010/tests/hostsim/main.h | 8 ++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/aversive/errno.h b/include/aversive/errno.h index 766c48c..54acf84 100644 --- a/include/aversive/errno.h +++ b/include/aversive/errno.h @@ -27,13 +27,14 @@ #ifndef _AVERSIVE_ERRNO_H_ #define _AVERSIVE_ERRNO_H_ +/** No error */ +#define ESUCCESS 0 + #ifndef HOST_VERSION /* from avr-libc, does not define a lots of errors */ #include -/** No error */ -#define ESUCCESS 0 /** Operation not permitted */ #define EPERM 1 /** No such file or directory */ diff --git a/projects/microb2010/tests/hostsim/main.h b/projects/microb2010/tests/hostsim/main.h index 73e3da7..b4fae54 100644 --- a/projects/microb2010/tests/hostsim/main.h +++ b/projects/microb2010/tests/hostsim/main.h @@ -32,11 +32,11 @@ #define LED2_ON() #define LED2_OFF() -#define LED2_TOGGLE() +#define LED2_TOGGLE() -#define LED3_ON() -#define LED3_OFF() -#define LED3_TOGGLE() +#define LED3_ON() +#define LED3_OFF() +#define LED3_TOGGLE() #define LED4_ON() #define LED4_OFF() -- 2.20.1 From b14123cac428083a50e2d0871514018810a779e5 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 18:23:01 +0200 Subject: [PATCH 05/16] trajectories on hostsim --- .../devices/robot/robot_system/robot_system.c | 84 +++++++++---------- .../trajectory_manager/trajectory_manager.h | 9 ++ .../trajectory_manager_core.c | 8 +- projects/microb2010/mainboard/Makefile | 4 +- .../microb2010/mainboard/commands_mainboard.c | 77 ++++++++++++++++- projects/microb2010/mainboard/display.py | 34 +++++++- 6 files changed, 165 insertions(+), 51 deletions(-) diff --git a/modules/devices/robot/robot_system/robot_system.c b/modules/devices/robot/robot_system/robot_system.c index 5e91561..b5b3d32 100755 --- a/modules/devices/robot/robot_system/robot_system.c +++ b/modules/devices/robot/robot_system/robot_system.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -40,7 +40,7 @@ #include "robot_system.h" -/** Call a pwm() pointer : +/** Call a pwm() pointer : * - lock the interrupts * - read the pointer to the pwm function * - unlock the interrupts @@ -62,7 +62,7 @@ safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value) } } -/** Call a encoder() pointer : +/** Call a encoder() pointer : * - lock the interrupts * - read the pointer to the encoder function * - unlock the interrupts @@ -134,7 +134,7 @@ void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_ #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT /** define left motor encoder function and param */ -void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), +void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), void *left_mot_encoder_param, double gain) { uint8_t flags; @@ -147,7 +147,7 @@ void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encode } /** define right motor encoder function and param */ -void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), +void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), void *right_mot_encoder_param, double gain) { uint8_t flags; @@ -161,7 +161,7 @@ void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_enco #endif /** define left external encoder function and param */ -void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), +void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), void *left_ext_encoder_param, double gain) { uint8_t flags; @@ -178,7 +178,7 @@ void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encode } /** define right external encoder function and param */ -void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), +void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), void *right_ext_encoder_param, double gain) { uint8_t flags; @@ -196,9 +196,9 @@ void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_enco /**** Virtual encoders and PWM */ -/** +/** * set the real pwms according to the specified angle (it also - * depends on the last distance command sent) + * depends on the last distance command sent) */ void rs_set_angle(void * data, int32_t angle) { @@ -214,14 +214,14 @@ void rs_set_angle(void * data, int32_t angle) p.angle = angle; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** +/** * set the real pwms according to the specified distance (it also - * depends on the last angle command sent) + * depends on the last angle command sent) */ void rs_set_distance(void * data, int32_t distance) { @@ -237,37 +237,37 @@ void rs_set_distance(void * data, int32_t distance) p.distance = distance; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** - * get the virtual angle according to real encoders value. +/** + * get the virtual angle according to real encoders value. */ int32_t rs_get_angle(void * data) { struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->virtual_encoders.angle ; + angle = rs->virtual_encoders.angle ; IRQ_UNLOCK(flags); return angle; } -/** - * get the virtual distance according to real encoders value. +/** + * get the virtual distance according to real encoders value. */ int32_t rs_get_distance(void * data) { struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->virtual_encoders.distance ; + distance = rs->virtual_encoders.distance ; IRQ_UNLOCK(flags); return distance; } @@ -277,9 +277,9 @@ int32_t rs_get_ext_angle(void * data) struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->pext_prev.angle ; + angle = rs->pext_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -289,9 +289,9 @@ int32_t rs_get_ext_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pext_prev.distance ; + distance = rs->pext_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -301,10 +301,10 @@ int32_t rs_get_mot_angle(void * data) { struct robot_system * rs = data; int32_t angle; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - angle = rs->pmot_prev.angle ; + angle = rs->pmot_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -314,9 +314,9 @@ int32_t rs_get_mot_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pmot_prev.distance ; + distance = rs->pmot_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -327,9 +327,9 @@ int32_t rs_get_ext_left(void * data) struct robot_system * rs = data; int32_t left; uint8_t flags; - + IRQ_LOCK(flags); - left = rs->wext_prev.left ; + left = rs->wext_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -339,9 +339,9 @@ int32_t rs_get_ext_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wext_prev.right ; + right = rs->wext_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -351,10 +351,10 @@ int32_t rs_get_mot_left(void * data) { struct robot_system * rs = data; int32_t left; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - left = rs->wmot_prev.left ; + left = rs->wmot_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -364,9 +364,9 @@ int32_t rs_get_mot_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wmot_prev.right ; + right = rs->wmot_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -375,13 +375,13 @@ int32_t rs_get_mot_right(void * data) void rs_set_flags(struct robot_system * rs, uint8_t flags) { uint8_t i_flags; - + IRQ_LOCK(i_flags); rs->flags = flags; IRQ_UNLOCK(i_flags); } -/** +/** * Read the encoders, and update internal virtual counters. Call this * function is needed before reading the virtual encoders.The program * will decide if it the external encoders or the motor encoders are @@ -398,7 +398,7 @@ void rs_update(void * data) #endif int32_t delta_angle, delta_distance; uint8_t flags; - + /* read encoders */ wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param); wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param); @@ -407,7 +407,7 @@ void rs_update(void * data) wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param); wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param); #endif - + /* apply gains to each wheel */ if (! (rs->flags & RS_IGNORE_EXT_GAIN )) { #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 43730ea..6e7e82e 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -197,6 +197,15 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r void trajectory_circle_rel(struct trajectory *traj, double x, double y, double radius_mm, double rel_a_deg, uint8_t flags); +/* + * Compute the fastest distance and angle speeds matching the radius + * from current traj_speed + */ +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a); + /* do a line */ void trajectory_line_abs(struct trajectory *traj, double x1, double y1, double x2, double y2, double advance); diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 33c2a30..0177419 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -479,10 +479,10 @@ void trajectory_manager_xy_event(struct trajectory *traj) * Compute the fastest distance and angle speeds matching the radius * from current traj_speed */ -/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a) +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a) { /* speed_d = coef * speed_a */ double coef; diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 7eff3cb..141943f 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,14 +7,14 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c -ifneq ($(HOST),avr) +ifeq ($(H),1) SRC += robotsim.c endif ASRC = CFLAGS += -Wall -Werror -ifeq ($(HOST),avr) +ifneq ($(H),1) LDFLAGS = -T ../common/avr6.x endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 9f39f86..ced8e58 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1074,16 +1074,92 @@ parse_pgm_inst_t cmd_servo_balls = { struct cmd_test_result { fixed_string_t arg0; int32_t radius; + int32_t dist; }; +/* function called when cmd_test is parsed successfully */ +static void line2line(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist) +{ + uint8_t err; + int32_t dist_imp_target; + double speed_d, speed_a; + double distance, angle; + double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); + double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); + + printf("%s()\n", __FUNCTION__); + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = line2_angle - line1_angle; + distance = angle * radius; + if (distance < 0) + distance = -distance; + dist_imp_target = rs_get_distance(&mainboard.rs) + + distance * mainboard.pos.phys.distance_imp_per_mm; + angle = DEG(angle); + distance += 100; /* take some margin to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance, angle); + + err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target, + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + /* 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; +#ifdef HOST_VERSION + strat_reset_pos(400, 400, 90); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + line2line(375, 347, 375, 1847, + 375, 1847, 1050, 1472, + 100, 200); + line2line(825, 1596, 1050, 1472, + 1050, 1472, 1500, 1722, + 180, 120); + line2line(1050, 1472, 1500, 1722, + 1500, 1722, 2175, 1347, + 180, 120); + line2line(1500, 1722, 2175, 1347, + 2175, 1347, 2175, 847, + 150, 120); + line2line(2175, 1347, 2175, 847, + 2175, 847, 2400, 722, + 150, 120); + line2line(2175, 847, 2400, 722, + 2400, 722, 2625, 847, + 150, 100); + line2line(2400, 722, 2625, 847, + 2625, 847, 2625, 1847, + 150, 100); + line2line(2625, 847, 2625, 1847, + 2625, 1847, 375, 597, + 100, 200); } prog_char str_test_arg0[] = "test"; parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); +parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32); prog_char help_test[] = "Test function"; parse_pgm_inst_t cmd_test = { @@ -1092,7 +1168,6 @@ parse_pgm_inst_t cmd_test = { .help_str = help_test, .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_test_arg0, - (prog_void *)&cmd_test_arg1, NULL, }, }; diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 239b2d6..3ca1a47 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -15,7 +15,7 @@ save_pos = [] robot = box(pos = (0, 0, 150), size = (250,320,350), - color = (1, 0, 0) ) + color = (0.3, 0.3, 0.3) ) last_pos = robot.pos.x, robot.pos.y, robot.pos.z hcenter_line = curve() @@ -23,6 +23,18 @@ hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)] vcenter_line = curve() vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)] +yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ] +yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea) +yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0)) + +bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ] +blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea) +bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0)) + +greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ] +greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea) +greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3)) + def square(sz): sq = curve() sq.pos = [(-sz, -sz, 0.3), @@ -54,7 +66,8 @@ TYPE_DANGEROUS=1 TYPE_WHITE_CORN=2 TYPE_BLACK_CORN=3 TYPE_OBSTACLE=4 -TYPE_NEIGH=5 +TYPE_BALL=5 +TYPE_NEIGH=6 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY waypoints = [col[:] for i in range(WAYPOINTS_NBX)] @@ -131,6 +144,15 @@ def init_waypoints(): if c >= 0: waypoints[i][j] = corn_table[c] continue + + # balls + if (i & 1) == 0 and j > 3: + waypoints[i][j] = TYPE_BALL + continue + if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2: + waypoints[i][j] = TYPE_BALL + continue + # too close of border if (i & 1) == 1 and j == WAYPOINTS_NBY -1: waypoints[i][j] = TYPE_OBSTACLE @@ -178,6 +200,14 @@ def toggle_obj_disp(): radius=25, color=(0.2,0.2,0.2), pos=(x-AREA_X/2,y-AREA_Y/2,75)) area_objects.append(c) + elif waypoints[i][j] == TYPE_BALL: + c = sphere(radius=50, color=(1., 0.,0.), + pos=(x-AREA_X/2,y-AREA_Y/2,50)) + area_objects.append(c) + else: + c = sphere(radius=5, color=(0., 0.,1.), + pos=(x-AREA_X/2,y-AREA_Y/2,5)) + area_objects.append(c) j += 1 y += STEP_CORN_Y i += 1 -- 2.20.1 From a33ceba76b3770d48c68a321b5d259893ddc613c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 10 Apr 2010 22:28:23 +0200 Subject: [PATCH 06/16] quadramp_indent --- .../filters/quadramp/quadramp.c | 31 +++++++++--------- .../filters/quadramp/quadramp.h | 32 +++++++++---------- 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c index 847d01a..b7f18a4 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.c +++ b/modules/devices/control_system/filters/quadramp/quadramp.c @@ -1,7 +1,6 @@ - -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * 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 @@ -45,8 +44,8 @@ void quadramp_reset(struct quadramp_filter * q) q->previous_in = 0; } -void quadramp_set_2nd_order_vars(struct quadramp_filter * q, - uint32_t var_2nd_ord_pos, +void quadramp_set_2nd_order_vars(struct quadramp_filter * q, + uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg) { uint8_t flags; @@ -56,8 +55,8 @@ void quadramp_set_2nd_order_vars(struct quadramp_filter * q, IRQ_UNLOCK(flags); } -void quadramp_set_1st_order_vars(struct quadramp_filter * q, - uint32_t var_1st_ord_pos, +void quadramp_set_1st_order_vars(struct quadramp_filter * q, + uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg) { uint8_t flags; @@ -76,10 +75,10 @@ uint8_t quadramp_is_finished(struct quadramp_filter *q) /** * Process the ramp - * + * * \param data should be a (struct quadramp_filter *) pointer * \param in is the input of the filter - * + * */ int32_t quadramp_do_filter(void * data, int32_t in) { @@ -93,13 +92,13 @@ int32_t quadramp_do_filter(void * data, int32_t in) int32_t previous_var, previous_out ; if ( q->var_1st_ord_pos ) - var_1st_ord_pos = q->var_1st_ord_pos ; + var_1st_ord_pos = q->var_1st_ord_pos ; if ( q->var_1st_ord_neg ) var_1st_ord_neg = -q->var_1st_ord_neg ; if ( q->var_2nd_ord_pos ) - var_2nd_ord_pos = q->var_2nd_ord_pos ; + var_2nd_ord_pos = q->var_2nd_ord_pos ; if ( q->var_2nd_ord_neg ) var_2nd_ord_neg = -q->var_2nd_ord_neg ; @@ -122,16 +121,16 @@ int32_t quadramp_do_filter(void * data, int32_t in) else if (d < 0 && var_2nd_ord_pos) { int32_t ramp_neg; - + /* var_2nd_ord_pos > 0 */ /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */ ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2; - + /* ramp_neg < 0 */ if(ramp_neg > var_1st_ord_neg) var_1st_ord_neg = ramp_neg ; } - + /* try to set the speed : can we reach the speed with our acceleration ? */ /* si on va moins vite que la Vmax */ if ( previous_var < var_1st_ord_pos ) { @@ -142,14 +141,14 @@ int32_t quadramp_do_filter(void * data, int32_t in) var_1st_ord_pos = previous_var + var_2nd_ord_pos ; } /* si on va plus vite que Vmax */ - else if ( previous_var > var_1st_ord_pos ) { + else if ( previous_var > var_1st_ord_pos ) { /* deceleration would be to high, we increase the speed */ /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax, * on sature Vmax a Vcourante + deceleration */ if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) ) var_1st_ord_pos = previous_var + var_2nd_ord_neg; } - + /* same for the neg */ /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */ if ( previous_var > var_1st_ord_neg ) { diff --git a/modules/devices/control_system/filters/quadramp/quadramp.h b/modules/devices/control_system/filters/quadramp/quadramp.h index 69b95b3..5e8100c 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.h +++ b/modules/devices/control_system/filters/quadramp/quadramp.h @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * 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 @@ -26,27 +26,27 @@ struct quadramp_filter { - uint32_t var_2nd_ord_pos; - uint32_t var_2nd_ord_neg; - uint32_t var_1st_ord_pos; - uint32_t var_1st_ord_neg; + uint32_t var_2nd_ord_pos; + uint32_t var_2nd_ord_neg; + uint32_t var_1st_ord_pos; + uint32_t var_1st_ord_neg; - int32_t previous_var; - int32_t previous_out; - int32_t previous_in; + int32_t previous_var; + int32_t previous_out; + int32_t previous_in; }; /** Initialization of the filter */ void quadramp_init(struct quadramp_filter *q); -void quadramp_reset(struct quadramp_filter * q); +void quadramp_reset(struct quadramp_filter *q); -void quadramp_set_2nd_order_vars(struct quadramp_filter *q, - uint32_t var_2nd_ord_pos, +void quadramp_set_2nd_order_vars(struct quadramp_filter *q, + uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg); -void quadramp_set_1st_order_vars(struct quadramp_filter *q, - uint32_t var_1st_ord_pos, +void quadramp_set_1st_order_vars(struct quadramp_filter *q, + uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg); /** @@ -57,10 +57,10 @@ uint8_t quadramp_is_finished(struct quadramp_filter *q); /** * Process the ramp - * + * * \param data should be a (struct quadramp_filter *) pointer * \param in is the input of the filter */ -int32_t quadramp_do_filter(void * data, int32_t in); +int32_t quadramp_do_filter(void *data, int32_t in); #endif -- 2.20.1 From fa8546ea39c7442ad3bf5a822a72a2b50a41045d Mon Sep 17 00:00:00 2001 From: zer0 Date: Sun, 11 Apr 2010 17:19:40 +0200 Subject: [PATCH 07/16] work on trajectory, update cobboard and ballboard too --- modules/base/math/geometry/lines.c | 31 +- modules/base/math/geometry/lines.h | 3 + modules/comm/uart/uart_host.c | 5 +- .../filters/quadramp/quadramp.c | 98 ++--- .../filters/quadramp/quadramp.h | 18 +- .../trajectory_manager/trajectory_manager.c | 15 +- .../trajectory_manager/trajectory_manager.h | 30 +- .../trajectory_manager_core.c | 259 ++++++++++- .../trajectory_manager_utils.c | 103 ++++- .../trajectory_manager_utils.h | 22 +- projects/microb2010/ballboard/commands_cs.c | 34 +- projects/microb2010/ballboard/cs.c | 2 +- projects/microb2010/ballboard/main.c | 5 +- projects/microb2010/cobboard/actuator.c | 38 +- projects/microb2010/cobboard/actuator.h | 8 +- .../microb2010/cobboard/commands_cobboard.c | 53 ++- projects/microb2010/cobboard/commands_cs.c | 18 +- projects/microb2010/cobboard/main.c | 12 +- projects/microb2010/cobboard/sensor.c | 6 +- projects/microb2010/cobboard/sensor.h | 10 +- projects/microb2010/cobboard/shovel.c | 24 + projects/microb2010/cobboard/shovel.h | 5 +- projects/microb2010/cobboard/spickle.c | 65 ++- projects/microb2010/cobboard/spickle.h | 8 +- projects/microb2010/cobboard/state.c | 114 +++-- projects/microb2010/mainboard/commands.c | 2 + projects/microb2010/mainboard/commands_cs.c | 144 +++--- .../microb2010/mainboard/commands_mainboard.c | 409 ++++++++++++++++-- projects/microb2010/mainboard/commands_traj.c | 32 +- projects/microb2010/mainboard/cs.c | 31 +- projects/microb2010/mainboard/display.py | 19 +- projects/microb2010/mainboard/main.c | 2 +- projects/microb2010/mainboard/main.h | 18 +- projects/microb2010/mainboard/robotsim.c | 8 +- projects/microb2010/mainboard/strat.h | 16 +- projects/microb2010/mainboard/strat_avoid.c | 55 +++ projects/microb2010/mainboard/strat_base.c | 1 + projects/microb2010/mainboard/strat_utils.c | 1 + projects/microb2010/mainboard/strat_utils.h | 4 - projects/microb2010/tests/oa/graph.py | 7 + 40 files changed, 1297 insertions(+), 438 deletions(-) create mode 100644 projects/microb2010/mainboard/strat_avoid.c diff --git a/modules/base/math/geometry/lines.c b/modules/base/math/geometry/lines.c index 09ad1a1..935d012 100755 --- a/modules/base/math/geometry/lines.c +++ b/modules/base/math/geometry/lines.c @@ -46,24 +46,24 @@ * p argument is the crossing point coordinates (dummy for 0 or 2 * result) */ -uint8_t +uint8_t intersect_line(const line_t *l1, const line_t *l2, point_t *p) -{ +{ double tmp1, tmp2; - debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n", + debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n", l1->a, l1->b, l1->c, l2->a, l2->b, l2->c); /* if dummy lines */ if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0)) return 0; - + if (l1->a == 0) { if (l2->a == 0) { if (l1->b*l2->c == l2->b*l1->c) return 2; return 0; } - + /* by + c = 0 * a'x + b'y + c' = 0 */ /* @@ -74,7 +74,7 @@ intersect_line(const line_t *l1, const line_t *l2, point_t *p) p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b); return 1; } - + if (l1->b == 0) { if (l2->b == 0) { if (l1->a*l2->c == l2->a*l1->c) @@ -118,12 +118,12 @@ void pts2line(const point_t *p1, const point_t *p2, line_t *l) p1y = p1->y; p2x = p2->x; p2y = p2->y; - + l->a = -(p2y - p1y); l->b = (p2x - p1x); l->c = -(l->a * p1x + l->b * p1y); - + debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n", __FUNCTION__, l->a, l->b, l->c); } @@ -145,16 +145,16 @@ void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out) /* return values: * 0 dont cross - * 1 cross + * 1 cross * 2 cross on point * 3 parallel and one point in * * p argument is the crossing point coordinates (dummy for 0 1 or 3 * result) */ -uint8_t -intersect_segment(const point_t *s1, const point_t *s2, - const point_t *t1, const point_t *t2, +uint8_t +intersect_segment(const point_t *s1, const point_t *s2, + const point_t *t1, const point_t *t2, point_t *p) { line_t l1, l2; @@ -210,7 +210,7 @@ intersect_segment(const point_t *s1, const point_t *s2, *p = *s2; return 2; } - + debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y); /* Consider as parallel if intersection is too far */ @@ -242,3 +242,8 @@ intersect_segment(const point_t *s1, const point_t *s2, return 1; } + +void line_translate(line_t *l, vect_t *v) +{ + l->c -= (l->a * v->x + l->b * v->y); +} diff --git a/modules/base/math/geometry/lines.h b/modules/base/math/geometry/lines.h index 6763b91..62094f4 100755 --- a/modules/base/math/geometry/lines.h +++ b/modules/base/math/geometry/lines.h @@ -52,4 +52,7 @@ intersect_segment(const point_t *s1, const point_t *s2, const point_t *t1, const point_t *t2, point_t *p); +/* translate the line */ +void +line_translate(line_t *l, vect_t *v); #endif /* _LINES_H_ */ diff --git a/modules/comm/uart/uart_host.c b/modules/comm/uart/uart_host.c index bd6aeed..04231c2 100644 --- a/modules/comm/uart/uart_host.c +++ b/modules/comm/uart/uart_host.c @@ -24,6 +24,8 @@ #include #include +#include + /* this file os a stub for host */ void uart_init(void) @@ -61,12 +63,13 @@ void uart_getconf(uint8_t num, struct uart_config *u) int uart_recv(uint8_t num) { + fcntl(0, F_SETFL, 0); return getchar(); } -/* XXX should not wait */ int uart_recv_nowait(uint8_t num) { + fcntl(0, F_SETFL, O_NONBLOCK); return getchar(); } diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c index b7f18a4..793df38 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.c +++ b/modules/devices/control_system/filters/quadramp/quadramp.c @@ -26,9 +26,7 @@ #include #include -#define NEXT(n, i) (((n) + (i)/(n)) >> 1) - -void quadramp_init(struct quadramp_filter * q) +void quadramp_init(struct quadramp_filter *q) { uint8_t flags; IRQ_LOCK(flags); @@ -37,16 +35,16 @@ void quadramp_init(struct quadramp_filter * q) } -void quadramp_reset(struct quadramp_filter * q) +void quadramp_reset(struct quadramp_filter *q) { q->previous_var = 0; q->previous_out = 0; q->previous_in = 0; } -void quadramp_set_2nd_order_vars(struct quadramp_filter * q, - uint32_t var_2nd_ord_pos, - uint32_t var_2nd_ord_neg) +void quadramp_set_2nd_order_vars(struct quadramp_filter *q, + double var_2nd_ord_pos, + double var_2nd_ord_neg) { uint8_t flags; IRQ_LOCK(flags); @@ -55,9 +53,9 @@ void quadramp_set_2nd_order_vars(struct quadramp_filter * q, IRQ_UNLOCK(flags); } -void quadramp_set_1st_order_vars(struct quadramp_filter * q, - uint32_t var_1st_ord_pos, - uint32_t var_1st_ord_neg) +void quadramp_set_1st_order_vars(struct quadramp_filter *q, + double var_1st_ord_pos, + double var_1st_ord_neg) { uint8_t flags; IRQ_LOCK(flags); @@ -83,104 +81,100 @@ uint8_t quadramp_is_finished(struct quadramp_filter *q) int32_t quadramp_do_filter(void * data, int32_t in) { struct quadramp_filter * q = data; - int32_t d ; + int32_t d; /* remaining distance */ int32_t pos_target; - int32_t var_1st_ord_pos = 0; - int32_t var_1st_ord_neg = 0; - int32_t var_2nd_ord_pos = 0; - int32_t var_2nd_ord_neg = 0; - int32_t previous_var, previous_out ; - - if ( q->var_1st_ord_pos ) - var_1st_ord_pos = q->var_1st_ord_pos ; - - if ( q->var_1st_ord_neg ) - var_1st_ord_neg = -q->var_1st_ord_neg ; - - if ( q->var_2nd_ord_pos ) - var_2nd_ord_pos = q->var_2nd_ord_pos ; - - if ( q->var_2nd_ord_neg ) - var_2nd_ord_neg = -q->var_2nd_ord_neg ; + double var_1st_ord_pos = q->var_1st_ord_pos; + double var_1st_ord_neg = -q->var_1st_ord_neg; + double var_2nd_ord_pos = q->var_2nd_ord_pos; + double var_2nd_ord_neg = -q->var_2nd_ord_neg; + double previous_var, d_float; + int32_t previous_out; previous_var = q->previous_var; previous_out = q->previous_out; d = in - previous_out ; + d_float = d; /* Deceleration ramp */ - if ( d > 0 && var_2nd_ord_neg) { - int32_t ramp_pos; + if (d > 0 && var_2nd_ord_neg) { + double ramp_pos; /* var_2nd_ord_neg < 0 */ - /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */ - ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2; + ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 - + 2*d_float*var_2nd_ord_neg) + + var_2nd_ord_neg/2; - if(ramp_pos < var_1st_ord_pos) - var_1st_ord_pos = ramp_pos ; + if (ramp_pos < var_1st_ord_pos) + var_1st_ord_pos = ramp_pos; } else if (d < 0 && var_2nd_ord_pos) { - int32_t ramp_neg; + double ramp_neg; /* var_2nd_ord_pos > 0 */ - /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */ - ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2; + ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - + 2*d_float*var_2nd_ord_pos ) - + var_2nd_ord_pos/2; /* ramp_neg < 0 */ - if(ramp_neg > var_1st_ord_neg) + if (ramp_neg > var_1st_ord_neg) var_1st_ord_neg = ramp_neg ; } /* try to set the speed : can we reach the speed with our acceleration ? */ /* si on va moins vite que la Vmax */ - if ( previous_var < var_1st_ord_pos ) { + if (previous_var < var_1st_ord_pos) { /* acceleration would be to high, we reduce the speed */ /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax, * on sature Vmax a Vcourante + acceleration */ - if (var_2nd_ord_pos && ( var_1st_ord_pos - previous_var > var_2nd_ord_pos) ) - var_1st_ord_pos = previous_var + var_2nd_ord_pos ; + if (var_2nd_ord_pos && + (var_1st_ord_pos - previous_var) > var_2nd_ord_pos) + var_1st_ord_pos = previous_var + var_2nd_ord_pos; } /* si on va plus vite que Vmax */ - else if ( previous_var > var_1st_ord_pos ) { + else if (previous_var > var_1st_ord_pos) { /* deceleration would be to high, we increase the speed */ /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) ) + if (var_2nd_ord_neg && + (var_1st_ord_pos - previous_var) < var_2nd_ord_neg) var_1st_ord_pos = previous_var + var_2nd_ord_neg; } /* same for the neg */ /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */ - if ( previous_var > var_1st_ord_neg ) { + if (previous_var > var_1st_ord_neg) { /* acceleration would be to high, we reduce the speed */ /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) ) - var_1st_ord_neg = previous_var + var_2nd_ord_neg ; + if (var_2nd_ord_neg && + (var_1st_ord_neg - previous_var) < var_2nd_ord_neg) + var_1st_ord_neg = previous_var + var_2nd_ord_neg; } /* si on va moins vite que Vmin (mais vitesse absolue superieure) */ - else if ( previous_var < var_1st_ord_neg ) { + else if (previous_var < var_1st_ord_neg) { /* deceleration would be to high, we increase the speed */ /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) ) + if (var_2nd_ord_pos && + (var_1st_ord_neg - previous_var) > var_2nd_ord_pos) var_1st_ord_neg = previous_var + var_2nd_ord_pos; } /* * Position consign : can we reach the position with our speed ? */ - if ( /* var_1st_ord_pos && */d > var_1st_ord_pos ) { + if (d_float > var_1st_ord_pos) { pos_target = previous_out + var_1st_ord_pos ; previous_var = var_1st_ord_pos ; } - else if ( /* var_1st_ord_neg && */d < var_1st_ord_neg ) { + else if (d_float < var_1st_ord_neg) { pos_target = previous_out + var_1st_ord_neg ; previous_var = var_1st_ord_neg ; } else { pos_target = previous_out + d ; - previous_var = d ; + previous_var = d_float ; } // update previous_out and previous_var @@ -188,5 +182,5 @@ int32_t quadramp_do_filter(void * data, int32_t in) q->previous_out = pos_target; q->previous_in = in; - return pos_target ; + return pos_target; } diff --git a/modules/devices/control_system/filters/quadramp/quadramp.h b/modules/devices/control_system/filters/quadramp/quadramp.h index 5e8100c..7073df0 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.h +++ b/modules/devices/control_system/filters/quadramp/quadramp.h @@ -26,12 +26,12 @@ struct quadramp_filter { - uint32_t var_2nd_ord_pos; - uint32_t var_2nd_ord_neg; - uint32_t var_1st_ord_pos; - uint32_t var_1st_ord_neg; + double var_2nd_ord_pos; + double var_2nd_ord_neg; + double var_1st_ord_pos; + double var_1st_ord_neg; - int32_t previous_var; + double previous_var; int32_t previous_out; int32_t previous_in; }; @@ -42,12 +42,12 @@ void quadramp_init(struct quadramp_filter *q); void quadramp_reset(struct quadramp_filter *q); void quadramp_set_2nd_order_vars(struct quadramp_filter *q, - uint32_t var_2nd_ord_pos, - uint32_t var_2nd_ord_neg); + double var_2nd_ord_pos, + double var_2nd_ord_neg); void quadramp_set_1st_order_vars(struct quadramp_filter *q, - uint32_t var_1st_ord_pos, - uint32_t var_1st_ord_neg); + double var_1st_ord_pos, + double var_1st_ord_neg); /** * Return 1 when (filter_input == filter_output && 1st_ord variation diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.c b/modules/devices/robot/trajectory_manager/trajectory_manager.c index 773388e..7311532 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.c @@ -41,12 +41,13 @@ /************ INIT FUNCS */ /** structure initialization */ -void trajectory_init(struct trajectory *traj) +void trajectory_init(struct trajectory *traj, double cs_hz) { uint8_t flags; IRQ_LOCK(flags); memset(traj, 0, sizeof(struct trajectory)); + traj->cs_hz = cs_hz; traj->state = READY; traj->scheduler_task = -1; IRQ_UNLOCK(flags); @@ -77,7 +78,7 @@ void trajectory_set_robot_params(struct trajectory *traj, } /** set speed consign */ -void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed) +void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed) { uint8_t flags; IRQ_LOCK(flags); @@ -86,6 +87,16 @@ void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_sp IRQ_UNLOCK(flags); } +/** set acc consign */ +void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc) +{ + uint8_t flags; + IRQ_LOCK(flags); + traj->d_acc = d_acc; + traj->a_acc = a_acc; + IRQ_UNLOCK(flags); +} + /** set windows for trajectory */ void trajectory_set_windows(struct trajectory *traj, double d_win, double a_win_deg, double a_start_deg) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 6e7e82e..594b7b0 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -52,6 +52,10 @@ enum trajectory_state { /* line */ RUNNING_LINE, + + /* clitoid */ + RUNNING_CLITOID_LINE, + RUNNING_CLITOID_CURVE, }; struct circle_target { @@ -64,10 +68,18 @@ struct circle_target { uint8_t flags; /**< flags for this trajectory */ }; +/* for line and clitoid */ struct line_target { line_t line; double angle; double advance; + + /* only for clitoid */ + point_t turn_pt; + double Aa; + double Va; + double alpha; + double R; }; struct trajectory { @@ -86,19 +98,24 @@ struct trajectory { * when a_target < a_start */ double circle_coef;/**<< corrective circle coef */ - uint16_t d_speed; /**<< distance speed consign */ - uint16_t a_speed; /**<< angle speed consign */ + double d_speed; /**<< distance speed consign */ + double a_speed; /**<< angle speed consign */ + + double d_acc; /**<< distance acceleration consign */ + double a_acc; /**<< angle acceleration consign */ struct robot_position *position; /**<< associated robot_position */ struct robot_system *robot; /**<< associated robot_system */ struct cs *csm_angle; /**<< associated control system (angle) */ struct cs *csm_distance; /**<< associated control system (distance) */ + double cs_hz; + int8_t scheduler_task; /**<< id of current task (-1 if no running task) */ }; /** structure initialization */ -void trajectory_init(struct trajectory *traj); +void trajectory_init(struct trajectory *traj, double cs_hz); /** structure initialization */ void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, @@ -110,7 +127,10 @@ void trajectory_set_robot_params(struct trajectory *traj, struct robot_position *pos) ; /** set speed consign */ -void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed); +void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed); + +/** set speed consign */ +void trajectory_set_speed(struct trajectory *traj, double d_acc, double a_acc); /** * set windows for trajectory. @@ -133,6 +153,8 @@ void trajectory_set_circle_coef(struct trajectory *traj, double coef); * position consign (after quadramp filter), for angle and * distance. */ uint8_t trajectory_finished(struct trajectory *traj); +uint8_t trajectory_angle_finished(struct trajectory *traj); +uint8_t trajectory_distance_finished(struct trajectory *traj); /** return true if traj is nearly finished depending on specified * parameters */ diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 0177419..5246285 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -55,6 +55,7 @@ static uint8_t evt_debug_cpt = 0; } \ } while (0) +static void start_clitoid(struct trajectory *traj); /** * update angle and/or distance @@ -294,13 +295,28 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r /************ FUNCS FOR GETTING TRAJ STATE */ +uint8_t trajectory_angle_finished(struct trajectory *traj) +{ + return cs_get_consign(traj->csm_angle) == + cs_get_filtered_consign(traj->csm_angle); +} + +uint8_t trajectory_distance_finished(struct trajectory *traj) +{ + if (traj->state == RUNNING_CLITOID_CURVE) + return 1; + + return cs_get_consign(traj->csm_distance) == + cs_get_filtered_consign(traj->csm_distance) ; +} + /** return true if the position consign is equal to the filtered * position consign (after quadramp filter), for angle and * distance. */ uint8_t trajectory_finished(struct trajectory *traj) { - return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) && - cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ; + return trajectory_angle_finished(traj) && + trajectory_distance_finished(traj); } /** return true if traj is nearly finished */ @@ -642,6 +658,19 @@ static void trajectory_manager_line_event(struct trajectory *traj) cs_set_consign(traj->csm_angle, a_consign); cs_set_consign(traj->csm_distance, d_consign); + + /* we reached dest, start clitoid */ + if (traj->state == RUNNING_CLITOID_LINE && + xy_norm(proj.x, + proj.y, + traj->target.line.turn_pt.x, + traj->target.line.turn_pt.y) < + xy_norm(proj.x + cos(traj->target.line.angle), + proj.y + sin(traj->target.line.angle), + traj->target.line.turn_pt.x, + traj->target.line.turn_pt.y)) { + start_clitoid(traj); + } } @@ -668,6 +697,7 @@ void trajectory_manager_event(void * param) break; case RUNNING_LINE: + case RUNNING_CLITOID_LINE: trajectory_manager_line_event(traj); break; @@ -762,15 +792,13 @@ void trajectory_circle_abs_dist_deg(struct trajectory *traj, /*********** *LINE */ /* Follow a line */ -void trajectory_line_abs(struct trajectory *traj, - double x1, double y1, - double x2, double y2, - double advance) +static void __trajectory_line_abs(struct trajectory *traj, + double x1, double y1, + double x2, double y2, + double advance) { point_t p1, p2; - delete_event(traj); - /* find the line EQ */ p1.x = x1; p1.y = y1; @@ -788,8 +816,223 @@ void trajectory_line_abs(struct trajectory *traj, traj->target.line.line.c, traj->target.line.angle); +} + +/* Follow a line */ +void trajectory_line_abs(struct trajectory *traj, + double x1, double y1, + double x2, double y2, + double advance) +{ + delete_event(traj); + __trajectory_line_abs(traj, x1, y1, x2, y2, advance); traj->state = RUNNING_LINE; trajectory_manager_event(traj); schedule_event(traj); } +/*** CLOTHOID */ + +/** + * process clitoid parameters + * + * - alpha: total angle + * - beta: circular part of angle (lower than alpha) + * - R: the radius of the circle (must be != 0) + * - Vd: linear speed to use (in imp per cs period) + * - Amax: maximum angular acceleration + * - d_inter: distance in mm until the intersection of the + * 2 lines + * + * return 0 on success: in this case these parameters are filled: + * - Aa_out: the angular acceleration to configure in quadramp + * - Va_out: the angular speed to configure in quadramp + * - remain_d_mm_out: remaining distance before start to turn + */ +static uint8_t calc_clitoid(struct trajectory *traj, + double x, double y, double a_rad, + double alpha_deg, double beta_deg, double R_mm, + double Vd, double Amax, double d_inter_mm, + double *Aa_out, double *Va_out, double *remain_d_mm_out) +{ + double Vd_mm_s; + double Va, Va_rd_s; + double t, d_mm, alpha_rad, beta_rad; + double remain_d_mm; + double Aa, Aa_rd_s2; + line_t line1, line2; + point_t robot, intersect, pt2, center, proj; + vect_t v; + + /* param check */ + if (fabs(alpha_deg) <= fabs(beta_deg)) { + DEBUG(E_TRAJECTORY, "alpha is smaller than beta"); + return -1; + } + + /* get angular speed Va */ + Vd_mm_s = speed_imp2mm(traj, Vd); + DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s); + Va_rd_s = Vd_mm_s / R_mm; + Va = speed_rd2imp(traj, Va_rd_s); + DEBUG(E_TRAJECTORY, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va); + + /* process 't', the time in seconds that we will take to do + * the first clothoid */ + alpha_rad = RAD(alpha_deg); + beta_rad = RAD(beta_deg); + t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); + DEBUG(E_TRAJECTORY, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", + R_mm, alpha_rad, beta_rad, t); + + /* process the angular acceleration */ + Aa_rd_s2 = Va_rd_s / t; + Aa = acc_rd2imp(traj, Aa_rd_s2); + DEBUG(E_TRAJECTORY, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa); + + /* exit if the robot cannot physically do it */ + if (Aa > Amax) { + DEBUG(E_TRAJECTORY, "greater than max acceleration"); + return -1; + } + + /* the robot position */ +/* x = position_get_x_double(&mainboard.pos); */ +/* y = position_get_y_double(&mainboard.pos); */ +/* a_rad = position_get_a_rad_double(&mainboard.pos); */ + + /* define line1 and line2 */ + robot.x = x; + robot.y = y; + intersect.x = x + cos(a_rad) * d_inter_mm; + intersect.y = y + sin(a_rad) * d_inter_mm; + pts2line(&robot, &intersect, &line1); + pt2.x = intersect.x + cos(a_rad + alpha_rad); + pt2.y = intersect.y + sin(a_rad + alpha_rad); + pts2line(&intersect, &pt2, &line2); + DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)", + intersect.x, intersect.y); + + /* the center of the circle is at (d_mm, d_mm) when we have to + * start the clothoid */ + d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * + sqrt(M_PI) / 2.; + DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm); + + /* translate line1 */ + v.x = intersect.x - robot.x; + v.y = intersect.y - robot.y; + if (a_rad > 0) + vect_rot_trigo(&v); + else + vect_rot_retro(&v); + vect_resize(&v, d_mm); + line_translate(&line1, &v); + + /* translate line2 */ + v.x = intersect.x - pt2.x; + v.y = intersect.y - pt2.y; + if (a_rad > 0) + vect_rot_trigo(&v); + else + vect_rot_retro(&v); + vect_resize(&v, d_mm); + line_translate(&line2, &v); + + /* find the center of the circle, at the intersection of the + * new translated lines */ + if (intersect_line(&line1, &line2, ¢er) != 1) { + DEBUG(E_TRAJECTORY, "cannot find circle center"); + return -1; + } + DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y); + + /* project center of circle on line1 */ + proj_pt_line(¢er, &line1, &proj); + DEBUG(E_TRAJECTORY, "proj=(%2.2f,%2.2f)", proj.x, proj.y); + + /* process remaining distance before start turning */ + remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); + DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm); + if (remain_d_mm < 0) { + DEBUG(E_TRAJECTORY, "too late, cannot turn"); + return -1; + } + + /* return result */ + *Aa_out = Aa; + *Va_out = Va; + *remain_d_mm_out = remain_d_mm; + return 0; +} + +static void start_clitoid(struct trajectory *traj) +{ + double Aa = traj->target.line.Aa; + double Va = traj->target.line.Va; + double a_rad = traj->target.line.alpha; + double R_mm = traj->target.line.R; + double d; + + delete_event(traj); + traj->state = RUNNING_CLITOID_CURVE; + set_quadramp_acc(traj, Aa, Aa); + set_quadramp_speed(traj, Va, Va); + d = R_mm * a_rad; + d *= 2.; /* margin to avoid deceleration */ + trajectory_d_a_rel(traj, d, DEG(a_rad)); +} + + +/** + * do a superb curve joining line1 to line2 which is composed of: + * - a clothoid starting from line1 + * - a circle + * - another clothoid up to line2 + * this curve is called a clitoid (hehe) + * + * the function assumes that the initial linear speed is Vd and + * angular speed is 0. + * + * - x,y,a: starting position + * - advance: parameter for line following + * - alpha: total angle + * - beta: circular part of angle (lower than alpha) + * - R: the radius of the circle (must be != 0) + * - Vd: linear speed to use (in imp per cs period) + * - Amax: maximum angular acceleration + * - d_inter: distance in mm until the intersection of the + * 2 lines + * + * return 0 if trajectory can be loaded, then it is processed in + * background. + */ +int8_t trajectory_clitoid(struct trajectory *traj, + double x, double y, double a, double advance, + double alpha_deg, double beta_deg, double R_mm, + double Vd, double d_inter_mm) +{ + double remain = 0, Aa = 0, Va = 0; + double turnx, turny; + + if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm, + Vd, traj->a_acc, d_inter_mm, + &Aa, &Va, &remain) < 0) + return -1; + + delete_event(traj); + turnx = x + cos(a) * remain; + turny = y + sin(a) * remain; + traj->target.line.Aa = Aa; + traj->target.line.Va = Va; + traj->target.line.alpha = RAD(alpha_deg); + traj->target.line.R = R_mm; + traj->target.line.turn_pt.x = turnx; + traj->target.line.turn_pt.y = turny; + __trajectory_line_abs(traj, x, y, turnx, turny, + advance); + traj->state = RUNNING_CLITOID_LINE; + trajectory_manager_event(traj); + schedule_event(traj); + return 0; +} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c index 0cf01ac..c48b592 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c @@ -41,7 +41,7 @@ #include "trajectory_manager_core.h" /** set speed consign in quadramp filter */ -void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed) +void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed) { struct quadramp_filter * q_d, * q_a; q_d = traj->csm_distance->consign_filter_params; @@ -51,7 +51,7 @@ void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_spee } /** get angle speed consign in quadramp filter */ -uint32_t get_quadramp_angle_speed(struct trajectory *traj) +double get_quadramp_angle_speed(struct trajectory *traj) { struct quadramp_filter *q_a; q_a = traj->csm_angle->consign_filter_params; @@ -59,17 +59,28 @@ uint32_t get_quadramp_angle_speed(struct trajectory *traj) } /** get distance speed consign in quadramp filter */ -uint32_t get_quadramp_distance_speed(struct trajectory *traj) +double get_quadramp_distance_speed(struct trajectory *traj) { struct quadramp_filter *q_d; q_d = traj->csm_distance->consign_filter_params; return q_d->var_1st_ord_pos; } +/** set speed consign in quadramp filter */ +void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc) +{ + struct quadramp_filter * q_d, * q_a; + q_d = traj->csm_distance->consign_filter_params; + q_a = traj->csm_angle->consign_filter_params; + quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc)); + quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc)); +} + /** remove event if any */ void delete_event(struct trajectory *traj) { set_quadramp_speed(traj, traj->d_speed, traj->a_speed); + set_quadramp_acc(traj, traj->d_acc, traj->a_acc); if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Delete event"); scheduler_del_event(traj->scheduler_task); @@ -143,3 +154,89 @@ uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad) a *= 2.; return ABS(a) < a_win_rad; } + +/* distance unit conversions */ + +double pos_mm2imp(struct trajectory *traj, double pos) +{ + return pos * traj->position->phys.distance_imp_per_mm; +} + +double pos_imp2mm(struct trajectory *traj, double pos) +{ + return pos / traj->position->phys.distance_imp_per_mm; +} + +double speed_mm2imp(struct trajectory *traj, double speed) +{ + return speed * + traj->position->phys.distance_imp_per_mm / + traj->cs_hz; +} + +double speed_imp2mm(struct trajectory *traj, double speed) +{ + return speed * traj->cs_hz / + traj->position->phys.distance_imp_per_mm; +} + +double acc_mm2imp(struct trajectory *traj, double acc) +{ + return acc * traj->position->phys.distance_imp_per_mm / + (traj->cs_hz * traj->cs_hz); +} + +double acc_imp2mm(struct trajectory *traj, double acc) +{ + return acc * traj->cs_hz * traj->cs_hz / + traj->position->phys.distance_imp_per_mm; +} + +/* angle unit conversions */ + +double pos_rd2imp(struct trajectory *traj, double pos) +{ + return pos * + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / 2.); +} + +double pos_imp2rd(struct trajectory *traj, double pos) +{ + return pos / + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / 2.); +} + +double speed_rd2imp(struct trajectory *traj, double speed) +{ + return speed * + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / + (2. * traj->cs_hz)); +} + +double speed_imp2rd(struct trajectory *traj, double speed) +{ + return speed / + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / + (2. * traj->cs_hz)); +} + +double acc_rd2imp(struct trajectory *traj, double acc) +{ + return acc * + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / + (2. * traj->cs_hz * traj->cs_hz)); +} + +double acc_imp2rd(struct trajectory *traj, double acc) +{ + return acc / + (traj->position->phys.distance_imp_per_mm * + traj->position->phys.track_mm / + (2. * traj->cs_hz * traj->cs_hz)); +} + diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h index 9ceaf48..f90475e 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h @@ -30,13 +30,16 @@ #define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT) /** set speed consign in quadramp filter */ -void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed); +void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed); + +/** set acc consign in quadramp filter */ +void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc); /** get angle speed consign in quadramp filter */ -uint32_t get_quadramp_angle_speed(struct trajectory *traj); +double get_quadramp_angle_speed(struct trajectory *traj); /** get distance speed consign in quadramp filter */ -uint32_t get_quadramp_distance_speed(struct trajectory *traj); +double get_quadramp_distance_speed(struct trajectory *traj); /** remove event if any */ void delete_event(struct trajectory *traj); @@ -60,3 +63,16 @@ uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win); * traj->target.pol.angle is set (i.e. an angle command, not an xy * command) */ uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad); + +double pos_mm2imp(struct trajectory *traj, double pos); +double pos_imp2mm(struct trajectory *traj, double pos); +double speed_mm2imp(struct trajectory *traj, double speed); +double speed_imp2mm(struct trajectory *traj, double speed); +double acc_mm2imp(struct trajectory *traj, double acc); +double acc_imp2mm(struct trajectory *traj, double acc); +double pos_rd2imp(struct trajectory *traj, double pos); +double pos_imp2rd(struct trajectory *traj, double pos); +double speed_rd2imp(struct trajectory *traj, double speed); +double speed_imp2rd(struct trajectory *traj, double speed); +double acc_rd2imp(struct trajectory *traj, double acc); +double acc_imp2rd(struct trajectory *traj, double acc); diff --git a/projects/microb2010/ballboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c index 098ffb3..c1dca26 100644 --- a/projects/microb2010/ballboard/commands_cs.c +++ b/projects/microb2010/ballboard/commands_cs.c @@ -428,17 +428,17 @@ parse_pgm_inst_t cmd_maximum_show = { /* this structure is filled when cmd_quadramp is parsed successfully */ struct cmd_quadramp_result { struct cmd_cs_result cs; - uint32_t ap; - uint32_t an; - uint32_t sp; - uint32_t sn; + double ap; + double an; + double sp; + double sn; }; /* function called when cmd_quadramp is parsed successfully */ static void cmd_quadramp_parsed(void *parsed_result, void *show) { struct cmd_quadramp_result * res = parsed_result; - + struct cs_block *csb; csb = cs_from_name(res->cs.csname); @@ -452,7 +452,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); } - printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"), res->cs.csname, csb->qr.var_2nd_ord_pos, csb->qr.var_2nd_ord_neg, @@ -462,10 +462,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) prog_char str_quadramp_arg0[] = "quadramp"; parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); -parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); -parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); -parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); -parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT); prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; parse_pgm_inst_t cmd_quadramp = { @@ -473,13 +473,13 @@ parse_pgm_inst_t cmd_quadramp = { .data = NULL, /* 2nd arg of func */ .help_str = help_quadramp, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_quadramp_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_quadramp_ap, - (prog_void *)&cmd_quadramp_an, - (prog_void *)&cmd_quadramp_sp, - (prog_void *)&cmd_quadramp_sn, - + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + NULL, }, }; diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index a82f3f4..bab700d 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -108,7 +108,7 @@ void microb_cs_init(void) /* PID */ pid_init(&ballboard.roller.pid); pid_set_gains(&ballboard.roller.pid, 80, 80, 250); - pid_set_maximums(&ballboard.roller.pid, 0, 10000, 2000); + pid_set_maximums(&ballboard.roller.pid, 0, 10000, 4095); pid_set_out_shift(&ballboard.roller.pid, 6); pid_set_derivate_filter(&ballboard.roller.pid, 6); diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index 9633368..082b658 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -163,7 +163,7 @@ int main(void) # error not supported #endif - eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_BALLBOARD) { @@ -197,8 +197,7 @@ int main(void) PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); - PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | - PWM_NG_MODE_SIGN_INVERTED, + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED, &PORTD, 5); diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 79f91cd..6811854 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -49,13 +49,14 @@ //#define COBROLLER_SPEED 400 #define SERVO_DOOR_OPEN 260 -#define SERVO_DOOR_CLOSED 490 +#define SERVO_DOOR_CLOSED 510 +#define SERVO_DOOR_BLOCK 510 -#define SERVO_CARRY_L_OPEN 280 -#define SERVO_CARRY_L_CLOSED 510 +#define SERVO_CARRY_L_OPEN 295 +#define SERVO_CARRY_L_CLOSED 400 // 510 -#define SERVO_CARRY_R_OPEN 470 -#define SERVO_CARRY_R_CLOSED 250 +#define SERVO_CARRY_R_OPEN 455 +#define SERVO_CARRY_R_CLOSED 350 // 250 void actuator_init(void); @@ -81,24 +82,25 @@ void servo_door_close(void) pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_CLOSED); } -void left_cobroller_on(void) -{ - cobboard.left_cobroller_speed = COBROLLER_SPEED; -} - -void right_cobroller_on(void) +void servo_door_block(void) { - cobboard.right_cobroller_speed = COBROLLER_SPEED; + pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_BLOCK); } -void left_cobroller_off(void) +void cobroller_on(uint8_t side) { - cobboard.left_cobroller_speed = 0; + if (side == I2C_LEFT_SIDE) + cobboard.left_cobroller_speed = COBROLLER_SPEED; + else + cobboard.right_cobroller_speed = -COBROLLER_SPEED; } -void right_cobroller_off(void) +void cobroller_off(uint8_t side) { - cobboard.right_cobroller_speed = 0; + if (side == I2C_LEFT_SIDE) + cobboard.left_cobroller_speed = 0; + else + cobboard.right_cobroller_speed = 0; } void actuator_init(void) diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index 90ff487..5b59a73 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -26,13 +26,13 @@ void actuator_init(void); void servo_carry_open(void); void servo_carry_close(void); + void servo_door_open(void); void servo_door_close(void); +void servo_door_block(void); -void left_cobroller_on(void); -void right_cobroller_on(void); -void left_cobroller_off(void); -void right_cobroller_off(void); +void cobroller_on(uint8_t side); +void cobroller_off(uint8_t side); #endif diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 888d15a..4ea3b80 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -402,11 +402,13 @@ static void cmd_servo_door_parsed(void *parsed_result, servo_door_open(); else if (!strcmp_P(res->arg1, PSTR("closed"))) servo_door_close(); + else if (!strcmp_P(res->arg1, PSTR("block"))) + servo_door_close(); } prog_char str_servo_door_arg0[] = "door"; parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0); -prog_char str_servo_door_arg1[] = "open#closed"; +prog_char str_servo_door_arg1[] = "open#closed#block"; parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1); prog_char help_servo_door[] = "Servo door function"; @@ -436,19 +438,17 @@ static void cmd_cobroller_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_cobroller_result *res = parsed_result; + uint8_t side; - if (!strcmp_P(res->arg1, PSTR("left"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - left_cobroller_on(); - else if (!strcmp_P(res->arg2, PSTR("off"))) - left_cobroller_off(); - } - else if (!strcmp_P(res->arg1, PSTR("right"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - right_cobroller_on(); - else if (!strcmp_P(res->arg2, PSTR("off"))) - right_cobroller_off(); - } + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + if (!strcmp_P(res->arg2, PSTR("on"))) + cobroller_on(side); + else if (!strcmp_P(res->arg2, PSTR("off"))) + cobroller_off(side); } prog_char str_cobroller_arg0[] = "cobroller"; @@ -575,6 +575,9 @@ static void cmd_spickle_parsed(void * parsed_result, else if (!strcmp_P(res->arg2, PSTR("pack"))) { spickle_pack(side); } + else if (!strcmp_P(res->arg2, PSTR("mid"))) { + spickle_mid(side); + } printf_P(PSTR("done\r\n")); } @@ -584,7 +587,7 @@ parse_pgm_token_string_t cmd_spickle_arg0 = prog_char str_spickle_arg1[] = "left#right"; parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1); -prog_char str_spickle_arg2[] = "deploy#pack"; +prog_char str_spickle_arg2[] = "deploy#pack#mid"; parse_pgm_token_string_t cmd_spickle_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2); @@ -611,6 +614,7 @@ struct cmd_spickle_params_result { fixed_string_t arg2; int32_t arg3; int32_t arg4; + int32_t arg5; }; /* function called when cmd_spickle_params is parsed successfully */ @@ -631,9 +635,7 @@ static void cmd_spickle_params_parsed(void *parsed_result, side = I2C_RIGHT_SIDE; if (!strcmp_P(res->arg2, PSTR("pos"))) - spickle_set_pos(side, res->arg3, res->arg4); - else if (!strcmp_P(res->arg2, PSTR("delay"))) - spickle_set_delay(side, res->arg3, res->arg4); + spickle_set_pos(side, res->arg3, res->arg4, res->arg5); } prog_char str_spickle_params_arg0[] = "spickle_params"; @@ -642,25 +644,28 @@ parse_pgm_token_string_t cmd_spickle_params_arg0 = prog_char str_spickle_params_arg1[] = "left#right"; parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1); -prog_char str_spickle_params_arg2[] = "pos#delay"; +prog_char str_spickle_params_arg2[] = "pos"; parse_pgm_token_string_t cmd_spickle_params_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2); parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32); parse_pgm_token_num_t cmd_spickle_params_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32); +parse_pgm_token_num_t cmd_spickle_params_arg5 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg5, INT32); -prog_char help_spickle_params[] = "Set spickle pos values"; +prog_char help_spickle_params[] = "Set spickle pos values: left|right pos INTPACK INTMID INTDEPL"; 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, - (prog_void *)&cmd_spickle_params_arg4, + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1, + (prog_void *)&cmd_spickle_params_arg2, + (prog_void *)&cmd_spickle_params_arg3, + (prog_void *)&cmd_spickle_params_arg4, + (prog_void *)&cmd_spickle_params_arg5, NULL, }, }; diff --git a/projects/microb2010/cobboard/commands_cs.c b/projects/microb2010/cobboard/commands_cs.c index 27be783..dfb7785 100644 --- a/projects/microb2010/cobboard/commands_cs.c +++ b/projects/microb2010/cobboard/commands_cs.c @@ -428,10 +428,10 @@ parse_pgm_inst_t cmd_maximum_show = { /* this structure is filled when cmd_quadramp is parsed successfully */ struct cmd_quadramp_result { struct cmd_cs_result cs; - uint32_t ap; - uint32_t an; - uint32_t sp; - uint32_t sn; + double ap; + double an; + double sp; + double sn; }; /* function called when cmd_quadramp is parsed successfully */ @@ -452,7 +452,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); } - printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"), res->cs.csname, csb->qr.var_2nd_ord_pos, csb->qr.var_2nd_ord_neg, @@ -462,10 +462,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) prog_char str_quadramp_arg0[] = "quadramp"; parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); -parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); -parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); -parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); -parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT); prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; parse_pgm_inst_t cmd_quadramp = { diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 62103c8..24f14bc 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -183,8 +183,6 @@ int main(void) error_register_notice(mylog); error_register_debug(mylog); - wait_ms(3000); - /* SPI + ENCODERS */ encoders_spi_init(); /* this will also init spi hardware */ @@ -205,8 +203,8 @@ int main(void) 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.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | + PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, &PORTD, 6); PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED | @@ -248,13 +246,15 @@ int main(void) sei(); - /* actuators */ - actuator_init(); + printf_P(PSTR("cobboard start\r\n")); /* spickle, shovel */ spickle_init(); shovel_init(); + /* actuators */ + actuator_init(); + state_init(); printf_P(PSTR("\r\n")); diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index b820d98..5bbf43f 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -142,14 +142,14 @@ struct sensor_filter { * CAP 1,5,6,7,8 */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { - [S_CAP1] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ - [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ + [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */ + [S_CAP2] = { 10, 0, 3, 7, 0, 0 }, /* 1 */ [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ [S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ - [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ + [S_RCOB] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 4e634fc..0ba6b0c 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -28,14 +28,14 @@ #define ADC_MAX 4 /* synchronize with sensor.c */ -#define S_CAP1 0 -#define S_COB_INSIDE_L 1 +#define S_COB_INSIDE_L 0 +#define S_CAP2 1 #define S_COB_INSIDE_R 2 #define S_CAP4 3 #define S_LCOB 4 #define S_LEFT 5 #define S_RIGHT 6 -#define S_COL_RIGHT 7 +#define S_RCOB 7 #define S_RESERVED1 8 #define S_RESERVED2 9 #define S_RESERVED3 10 diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 39ce2bc..6d71e58 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -40,14 +40,38 @@ #include #include "main.h" +#include "shovel.h" /* init spickle position at beginning */ static void shovel_autopos(void) { + printf_P(PSTR("shovel autopos...")); pwm_ng_set(SHOVEL_PWM, -500); wait_ms(1000); pwm_ng_set(LEFT_SPICKLE_PWM, 0); encoders_spi_set_value(SHOVEL_ENCODER, 0); + printf_P(PSTR("ok\r\n")); +} + +static uint8_t shovel_is_at_pos(int32_t pos) +{ + int32_t diff; + diff = pos - encoders_spi_get_value(SHOVEL_ENCODER); + if (diff < 0) + diff = -diff; + if (diff < 500) + return 1; + return 0; +} + +uint8_t shovel_is_up(void) +{ + return shovel_is_at_pos(SHOVEL_UP); +} + +uint8_t shovel_is_down(void) +{ + return shovel_is_at_pos(SHOVEL_DOWN); } void shovel_init(void) diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index 7ba8445..241b065 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -23,7 +23,7 @@ #define _SHOVEL_H_ #define SHOVEL_DOWN 100 -#define SHOVEL_MID 6000 +#define SHOVEL_MID 4900 #define SHOVEL_UP 10000 void shovel_init(void); @@ -43,4 +43,7 @@ static inline void shovel_up(void) cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); } +uint8_t shovel_is_up(void); +uint8_t shovel_is_down(void); + #endif /* _SHOVEL_H_ */ diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 62732c2..e1d9a7b 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -54,9 +54,8 @@ struct spickle_params { struct cs_block * const csb[2]; /* params */ - int16_t delay_deployed[2]; - int16_t delay_packed[2]; int32_t pos_deployed[2]; + int32_t pos_mid[2]; int32_t pos_packed[2]; }; @@ -67,17 +66,13 @@ static struct spickle_params spickle = { &cobboard.left_spickle, &cobboard.right_spickle, }, - .delay_deployed = { - 500, /* left */ - 500, /* right */ - }, - .delay_packed = { - 500, /* left */ - 500, /* right */ - }, .pos_deployed = { - 35000, /* left */ - 35000, /* right */ + 40000, /* left */ + -40000, /* right */ + }, + .pos_mid = { + 20000, /* left */ + -20000, /* right */ }, .pos_packed = { 0, /* left */ @@ -88,13 +83,15 @@ static struct spickle_params spickle = { /* init spickle position at beginning */ static void spickle_autopos(void) { + printf_P(PSTR("spickle autopos...")); pwm_ng_set(LEFT_SPICKLE_PWM, -500); - //pwm_ng_set(RIGHT_SPICKLE_PWM, -500); + pwm_ng_set(RIGHT_SPICKLE_PWM, 500); wait_ms(1000); pwm_ng_set(LEFT_SPICKLE_PWM, 0); pwm_ng_set(RIGHT_SPICKLE_PWM, 0); encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0); encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0); + printf_P(PSTR("ok\r\n")); } /* Set CS command for spickle. Called by CS manager. */ @@ -140,33 +137,26 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2) spickle.k2 = k2; } -void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed) + +void spickle_set_pos(uint8_t side, int32_t pos_packed, + int32_t pos_mid, int32_t pos_deployed) { spickle.pos_deployed[side] = pos_deployed; + spickle.pos_mid[side] = pos_mid; spickle.pos_packed[side] = pos_packed; } -void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed) -{ - spickle.delay_deployed[side] = delay_deployed; - spickle.delay_packed[side] = delay_packed; -} - void spickle_dump_params(void) { printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2); printf_P(PSTR("left pos %ld %ld\r\n"), - spickle.pos_deployed[I2C_LEFT_SIDE], - spickle.pos_packed[I2C_LEFT_SIDE]); - printf_P(PSTR("left delay %ld %ld\r\n"), - spickle.delay_deployed[I2C_LEFT_SIDE], - spickle.delay_packed[I2C_LEFT_SIDE]); + spickle.pos_packed[I2C_LEFT_SIDE], + spickle.pos_mid[I2C_LEFT_SIDE], + spickle.pos_deployed[I2C_LEFT_SIDE]); printf_P(PSTR("right pos %ld %ld\r\n"), - spickle.pos_deployed[I2C_RIGHT_SIDE], - spickle.pos_packed[I2C_RIGHT_SIDE]); - printf_P(PSTR("right delay %ld %ld\r\n"), - spickle.delay_deployed[I2C_RIGHT_SIDE], - spickle.delay_packed[I2C_RIGHT_SIDE]); + spickle.pos_packed[I2C_RIGHT_SIDE], + spickle.pos_mid[I2C_RIGHT_SIDE], + spickle.pos_deployed[I2C_RIGHT_SIDE]); } void spickle_deploy(uint8_t side) @@ -174,24 +164,19 @@ void spickle_deploy(uint8_t side) cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); } -void spickle_pack(uint8_t side) +void spickle_mid(uint8_t side) { - cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]); } -uint16_t spickle_get_deploy_delay(uint8_t side) -{ - return spickle.delay_deployed[side]; -} - -uint16_t spickle_get_pack_delay(uint8_t side) +void spickle_pack(uint8_t side) { - return spickle.delay_packed[side]; + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]); } void spickle_init(void) { spickle_autopos(); cobboard.left_spickle.on = 1; - //cobboard.right_spickle.on = 1; + cobboard.right_spickle.on = 1; } diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h index d36f80f..81c74d8 100644 --- a/projects/microb2010/cobboard/spickle.h +++ b/projects/microb2010/cobboard/spickle.h @@ -24,17 +24,15 @@ void spickle_set(void *dummy, int32_t cmd); void spickle_set_coefs(uint32_t k1, uint32_t k2); -void spickle_set_pos(uint8_t side, uint32_t pos_deploy, uint32_t pos_pack); -void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed); +void spickle_set_pos(uint8_t side, int32_t pos_pack, + int32_t pos_mid, int32_t pos_deployed); void spickle_dump_params(void); void spickle_deploy(uint8_t side); +void spickle_mid(uint8_t side); void spickle_pack(uint8_t side); -uint16_t spickle_get_deploy_delay(uint8_t side); -uint16_t spickle_get_pack_delay(uint8_t side); - void spickle_init(void); #endif diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 3308405..9d082ba 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -98,20 +98,17 @@ static uint8_t state_cob_present(uint8_t side) if (side == I2C_LEFT_SIDE) return sensor_get(S_LCOB); else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return sensor_get(S_RCOB); } /* return the detected color of the cob (only valid if present) */ static uint8_t state_cob_color(uint8_t side) { + /* XXX no color sensor for now */ if (side == I2C_LEFT_SIDE) return I2C_COB_WHITE; else - /* XXX */ - // return sensor_get(S_LCOB); - return 0; + return I2C_COB_WHITE; } /* return true if the cob is correctly inside */ @@ -121,6 +118,13 @@ static uint8_t state_cob_inside(void) sensor_get(S_COB_INSIDE_R); } +/* return true if there is no cob correctly inside */ +static uint8_t state_no_cob_inside(void) +{ + return !sensor_get(S_COB_INSIDE_L) && + !sensor_get(S_COB_INSIDE_R); +} + /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { @@ -158,8 +162,6 @@ uint8_t state_get_mode(void) /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { - uint16_t delay; - /* if there is no cob, return */ if (state_cob_present(side)) return; @@ -168,40 +170,67 @@ static void state_do_harvest(uint8_t side) if (state_cob_color(side) == I2C_COB_BLACK) return; + STMCH_DEBUG("start"); + /* eat the cob */ spickle_pack(side); - /* xxx */ + time_wait_ms(250); - left_cobroller_on(); - delay = spickle_get_pack_delay(side); + cobroller_on(side); - WAIT_COND_OR_TIMEOUT(state_cob_inside(), delay); + if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { + if (state_no_cob_inside()) { + printf_P(PSTR("NO COB\r\n")); + return; + } + printf_P(PSTR("BAD COB - press a key\r\n")); + while(!cmdline_keypressed()); + } + + /* cob is inside, switch off roller */ + cobroller_off(side); + cob_count ++; + + /* last cob, nothing to do */ + if (cob_count == 5) + return; /* redeploy the spickle */ spickle_deploy(side); state_debug_wait_key_pressed(); - /* let the cob go */ + /* let the loaded cobs go */ + servo_door_block(); servo_carry_open(); - wait_ms(300); /* XXX */ + time_wait_ms(100); state_debug_wait_key_pressed(); - cob_count ++; - /* store it */ shovel_up(); - wait_ms(200); + + if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + BRAKE_ON(); + printf_P(PSTR("BLOCKED\r\n")); + while(!cmdline_keypressed()); + } + state_debug_wait_key_pressed(); /* close the carry servos */ servo_carry_close(); - wait_ms(300); /* XXX */ + servo_door_close(); + time_wait_ms(200); state_debug_wait_key_pressed(); shovel_down(); - left_cobroller_off(); - state_debug_wait_key_pressed(); - time_wait_ms(500); + + if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { + BRAKE_ON(); + printf_P(PSTR("BLOCKED\r\n")); + while(!cmdline_keypressed()); + } + + STMCH_DEBUG("end"); } /* eject cobs */ @@ -228,30 +257,35 @@ void state_machine(void) state_init(); } + cobroller_off(I2C_LEFT_SIDE); + cobroller_off(I2C_RIGHT_SIDE); + /* pack/deply spickles, enable/disable roller */ - if (L_DEPLOY(state_mode)) { + if (cob_count >= 5) + spickle_pack(I2C_LEFT_SIDE); + else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode)) + spickle_mid(I2C_LEFT_SIDE); + else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) spickle_deploy(I2C_LEFT_SIDE); - //left_cobroller_on(); - left_cobroller_off(); - } - else { + else spickle_pack(I2C_LEFT_SIDE); - left_cobroller_off(); - } - if (R_DEPLOY(state_mode)) { + + if (cob_count >= 5) + spickle_pack(I2C_RIGHT_SIDE); + else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode)) + spickle_mid(I2C_RIGHT_SIDE); + else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) spickle_deploy(I2C_RIGHT_SIDE); - right_cobroller_on(); - } - else { + else spickle_pack(I2C_RIGHT_SIDE); - right_cobroller_off(); - } /* harvest */ - if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) - state_do_harvest(I2C_LEFT_SIDE); - if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) - state_do_harvest(I2C_RIGHT_SIDE); + if (cob_count < 5) { + if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) + state_do_harvest(I2C_LEFT_SIDE); + if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) + state_do_harvest(I2C_RIGHT_SIDE); + } /* eject */ if (EJECT(state_mode)) { diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index b1f09d0..cdbeb6e 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -76,6 +76,7 @@ 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_servo_balls; +extern parse_pgm_inst_t cmd_clitoid; extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -159,6 +160,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_cobboard_setmode2, (parse_pgm_inst_t *)&cmd_cobboard_setmode3, (parse_pgm_inst_t *)&cmd_servo_balls, + (parse_pgm_inst_t *)&cmd_clitoid, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index 272f1e9..c749c61 100644 --- a/projects/microb2010/mainboard/commands_cs.c +++ b/projects/microb2010/mainboard/commands_cs.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -89,7 +89,7 @@ struct cs_block *cs_from_name(const char *name) } return NULL; } - + /**********************************************************/ /* Gains for control system */ @@ -113,7 +113,7 @@ static void cmd_gain_parsed(void * parsed_result, void *show) return; } - if (!show) + if (!show) pid_set_gains(&csb->pid, res->p, res->i, res->d); printf_P(PSTR("%s %s %d %d %d\r\n"), @@ -136,11 +136,11 @@ parse_pgm_inst_t cmd_gain = { .data = NULL, /* 2nd arg of func */ .help_str = help_gain, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_gain_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_gain_p, - (prog_void *)&cmd_gain_i, - (prog_void *)&cmd_gain_d, + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, NULL, }, }; @@ -161,8 +161,8 @@ parse_pgm_inst_t cmd_gain_show = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_gain_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_gain_arg0, - (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, (prog_void *)&cmd_gain_show_arg, NULL, }, @@ -181,7 +181,7 @@ struct cmd_speed_result { static void cmd_speed_parsed(void *parsed_result, void *show) { struct cmd_speed_result * res = parsed_result; - + struct cs_block *csb; csb = cs_from_name(res->cs.csname); @@ -191,10 +191,10 @@ static void cmd_speed_parsed(void *parsed_result, void *show) } #if notyet - if (!show) + if (!show) ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ - printf_P(PSTR("%s %"PRIu32"\r\n"), + printf_P(PSTR("%s %"PRIu32"\r\n"), res->cs.csname, ext.r_b.var_pos); #else @@ -212,9 +212,9 @@ parse_pgm_inst_t cmd_speed = { .data = NULL, /* 2nd arg of func */ .help_str = help_speed, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_speed_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_speed_s, + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, NULL, }, }; @@ -234,8 +234,8 @@ parse_pgm_inst_t cmd_speed_show = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_speed_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_speed_arg0, - (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, (prog_void *)&cmd_speed_show_arg, NULL, }, @@ -262,10 +262,10 @@ static void cmd_derivate_filter_parsed(void *parsed_result, void *show) return; } - if (!show) + if (!show) pid_set_derivate_filter(&csb->pid, res->size); - printf_P(PSTR("%s %s %u\r\n"), + printf_P(PSTR("%s %s %u\r\n"), res->cs.cmdname, res->cs.csname, pid_get_derivate_filter(&csb->pid)); @@ -281,9 +281,9 @@ parse_pgm_inst_t cmd_derivate_filter = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_derivate_filter, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_derivate_filter_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_derivate_filter_size, + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, NULL, }, }; @@ -304,8 +304,8 @@ parse_pgm_inst_t cmd_derivate_filter_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_derivate_filter_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_derivate_filter_arg0, - (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, (prog_void *)&cmd_derivate_filter_show_arg, NULL, }, @@ -346,9 +346,9 @@ parse_pgm_inst_t cmd_consign = { .data = NULL, /* 2nd arg of func */ .help_str = help_consign, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_consign_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_consign_p, + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, NULL, }, }; @@ -369,7 +369,7 @@ struct cmd_maximum_result { static void cmd_maximum_parsed(void *parsed_result, void *show) { struct cmd_maximum_result * res = parsed_result; - + struct cs_block *csb; csb = cs_from_name(res->cs.csname); @@ -381,7 +381,7 @@ static void cmd_maximum_parsed(void *parsed_result, void *show) if (!show) pid_set_maximums(&csb->pid, res->in, res->i, res->out); - printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"), + printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"), res->cs.csname, pid_get_max_in(&csb->pid), pid_get_max_I(&csb->pid), @@ -400,11 +400,11 @@ parse_pgm_inst_t cmd_maximum = { .data = NULL, /* 2nd arg of func */ .help_str = help_maximum, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_maximum_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_maximum_in, - (prog_void *)&cmd_maximum_i, - (prog_void *)&cmd_maximum_out, + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, NULL, }, }; @@ -425,8 +425,8 @@ parse_pgm_inst_t cmd_maximum_show = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_maximum_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_maximum_arg0, - (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, (prog_void *)&cmd_maximum_show_arg, NULL, }, @@ -438,17 +438,17 @@ parse_pgm_inst_t cmd_maximum_show = { /* this structure is filled when cmd_quadramp is parsed successfully */ struct cmd_quadramp_result { struct cmd_cs_result cs; - uint32_t ap; - uint32_t an; - uint32_t sp; - uint32_t sn; + double ap; + double an; + double sp; + double sn; }; /* function called when cmd_quadramp is parsed successfully */ static void cmd_quadramp_parsed(void *parsed_result, void *show) { struct cmd_quadramp_result * res = parsed_result; - + struct cs_block *csb; csb = cs_from_name(res->cs.csname); @@ -462,7 +462,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); } - printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"), + printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"), res->cs.csname, csb->qr.var_2nd_ord_pos, csb->qr.var_2nd_ord_neg, @@ -472,10 +472,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) prog_char str_quadramp_arg0[] = "quadramp"; parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); -parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); -parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); -parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); -parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT); prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; parse_pgm_inst_t cmd_quadramp = { @@ -483,13 +483,13 @@ parse_pgm_inst_t cmd_quadramp = { .data = NULL, /* 2nd arg of func */ .help_str = help_quadramp, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_quadramp_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_quadramp_ap, - (prog_void *)&cmd_quadramp_an, - (prog_void *)&cmd_quadramp_sp, - (prog_void *)&cmd_quadramp_sn, - + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + NULL, }, }; @@ -510,9 +510,9 @@ parse_pgm_inst_t cmd_quadramp_show = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_quadramp_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_quadramp_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_quadramp_show_arg, + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, NULL, }, }; @@ -535,7 +535,7 @@ static void cmd_cs_status_parsed(void *parsed_result, void *data) struct cs_block *csb; uint8_t loop = 0; uint8_t print_pid = 0, print_cs = 0; - + csb = cs_from_name(res->cs.csname); if (csb == NULL) { printf_P(PSTR("null csb\r\n")); @@ -587,9 +587,9 @@ parse_pgm_inst_t cmd_cs_status = { .data = NULL, /* 2nd arg of func */ .help_str = help_cs_status, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cs_status_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_cs_status_arg, + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, NULL, }, }; @@ -611,7 +611,7 @@ struct cmd_blocking_i_result { static void cmd_blocking_i_parsed(void *parsed_result, void *show) { struct cmd_blocking_i_result * res = parsed_result; - + struct cs_block *csb; csb = cs_from_name(res->cs.csname); @@ -624,7 +624,7 @@ static void cmd_blocking_i_parsed(void *parsed_result, void *show) bd_set_current_thresholds(&csb->bd, res->k1, res->k2, res->i, res->cpt); - printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), + printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), res->cs.cmdname, res->cs.csname, csb->bd.k1, @@ -646,11 +646,11 @@ parse_pgm_inst_t cmd_blocking_i = { .data = NULL, /* 2nd arg of func */ .help_str = help_blocking_i, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_blocking_i_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_blocking_i_k1, - (prog_void *)&cmd_blocking_i_k2, - (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, (prog_void *)&cmd_blocking_i_cpt, NULL, }, @@ -672,9 +672,9 @@ parse_pgm_inst_t cmd_blocking_i_show = { .data = (void *)1, /* 2nd arg of func */ .help_str = help_blocking_i_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_blocking_i_arg0, - (prog_void *)&cmd_csb_name_tok, - (prog_void *)&cmd_blocking_i_show_arg, + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, NULL, }, }; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index ced8e58..fe4fabb 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -40,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -1068,16 +1070,207 @@ parse_pgm_inst_t cmd_servo_balls = { }; /**********************************************************/ -/* Test */ +/* Clitoid */ -/* this structure is filled when cmd_test is parsed successfully */ -struct cmd_test_result { +/* this structure is filled when cmd_clitoid is parsed successfully */ +struct cmd_clitoid_result { fixed_string_t arg0; - int32_t radius; - int32_t dist; + float alpha_deg; + float beta_deg; + float R_mm; + float Vd; + float Amax; + float d_inter_mm; }; +/** + * do a superb curve joining line1 to line2 which is composed of: + * - a clothoid starting from line1 + * - a circle + * - another clothoid up to line2 + * + * the function assumes that the initial linear speed is Vd and + * angular speed is 0. + * + * - alpha: total angle + * - beta: circular part of angle (lower than alpha) + * - R: the radius of the circle (must be != 0) + * - Vd: linear speed to use (in imp per cs period) + * - Amax: maximum angular acceleration + * - d_inter: distance in mm until the intersection of the + * 2 lines + * + * return 0 on success: in this case these parameters are filled: + * - Aa_out: the angular acceleration to configure in quadramp + * - remain_d_mm_out: remaining distance before start to turn + */ +uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, + double Vd, double Amax, double d_inter_mm) +{ + double Vd_mm_s; + double Va, Va_rd_s; + double t, d_mm, alpha_rad, beta_rad; + double remain_d_mm; + double Aa, Aa_rd_s2; + line_t line1, line2; + double x, y, a_rad; + point_t robot, intersect, pt2, center, proj; + vect_t v; + + /* param check */ + if (fabs(alpha_deg) <= fabs(beta_deg)) { + DEBUG(E_USER_STRAT, "alpha is smaller than beta"); + return END_ERROR; + } + + /* get angular speed Va */ + Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM); + DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s); + Va_rd_s = Vd_mm_s / R_mm; + Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ)); + DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va); + + /* process 't', the time in seconds that we will take to do + * the first clothoid */ + alpha_rad = RAD(alpha_deg); + beta_rad = RAD(beta_deg); + t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); + DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", + R_mm, alpha_rad, beta_rad, t); + + /* process the angular acceleration */ + Aa_rd_s2 = Va_rd_s / t; + Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM / + (2 * CS_HZ * CS_HZ)); + DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa); + + /* exit if the robot cannot physically do it */ + if (Aa > Amax) { + DEBUG(E_USER_STRAT, "greater than max acceleration"); + return END_ERROR; + } + + /* the robot position */ + x = position_get_x_double(&mainboard.pos); + y = position_get_y_double(&mainboard.pos); + a_rad = position_get_a_rad_double(&mainboard.pos); + + /* define line1 and line2 */ + robot.x = x; + robot.y = y; + intersect.x = x + cos(a_rad) * d_inter_mm; + intersect.y = y + sin(a_rad) * d_inter_mm; + pts2line(&robot, &intersect, &line1); + pt2.x = intersect.x + cos(a_rad + alpha_rad); + pt2.y = intersect.y + sin(a_rad + alpha_rad); + pts2line(&intersect, &pt2, &line2); + DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)", + intersect.x, intersect.y); + + /* the center of the circle is at (d_mm, d_mm) when we have to + * start the clothoid */ + d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * + sqrt(M_PI) / 2.; + DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm); + + /* translate line1 */ + v.x = intersect.x - robot.x; + v.y = intersect.y - robot.y; + if (a_rad > 0) + vect_rot_trigo(&v); + else + vect_rot_retro(&v); + vect_resize(&v, d_mm); + line_translate(&line1, &v); + + /* translate line2 */ + v.x = intersect.x - pt2.x; + v.y = intersect.y - pt2.y; + if (a_rad > 0) + vect_rot_trigo(&v); + else + vect_rot_retro(&v); + vect_resize(&v, d_mm); + line_translate(&line2, &v); + + /* find the center of the circle, at the intersection of the + * new translated lines */ + if (intersect_line(&line1, &line2, ¢er) != 1) { + DEBUG(E_USER_STRAT, "cannot find circle center"); + return END_ERROR; + } + DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y); + + /* project center of circle on line1 */ + proj_pt_line(¢er, &line1, &proj); + DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y); + + /* process remaining distance before start turning */ + remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); + DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm); + if (remain_d_mm < 0) { + DEBUG(E_USER_STRAT, "too late, cannot turn"); + return END_ERROR; + } + + return END_TRAJ; +} + /* function called when cmd_test is parsed successfully */ +static void cmd_clitoid_parsed(void *parsed_result, void *data) +{ + struct cmd_clitoid_result *res = parsed_result; + clitoid(res->alpha_deg, res->beta_deg, res->R_mm, + res->Vd, res->Amax, res->d_inter_mm); +} + +prog_char str_clitoid_arg0[] = "clitoid"; +parse_pgm_token_string_t cmd_clitoid_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result, + arg0, str_clitoid_arg0); +parse_pgm_token_num_t cmd_clitoid_alpha_deg = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + alpha_deg, FLOAT); +parse_pgm_token_num_t cmd_clitoid_beta_deg = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + beta_deg, FLOAT); +parse_pgm_token_num_t cmd_clitoid_R_mm = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + R_mm, FLOAT); +parse_pgm_token_num_t cmd_clitoid_Vd = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + Vd, FLOAT); +parse_pgm_token_num_t cmd_clitoid_Amax = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + Amax, FLOAT); +parse_pgm_token_num_t cmd_clitoid_d_inter_mm = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + d_inter_mm, FLOAT); + +prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)"; +parse_pgm_inst_t cmd_clitoid = { + .f = cmd_clitoid_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_clitoid, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_clitoid_arg0, + (prog_void *)&cmd_clitoid_alpha_deg, + (prog_void *)&cmd_clitoid_beta_deg, + (prog_void *)&cmd_clitoid_R_mm, + (prog_void *)&cmd_clitoid_Vd, + (prog_void *)&cmd_clitoid_Amax, + (prog_void *)&cmd_clitoid_d_inter_mm, + NULL, + }, +}; + +////////////////////// + +// 500 -- 5 +// 400 -- 3 +#define TEST_SPEED 400 +#define TEST_ACC 3 + static void line2line(double line1x1, double line1y1, double line1x2, double line1y2, double line2x1, double line2y1, @@ -1085,14 +1278,16 @@ static void line2line(double line1x1, double line1y1, double radius, double dist) { uint8_t err; - int32_t dist_imp_target; double speed_d, speed_a; double distance, angle; double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); - printf("%s()\n", __FUNCTION__); - strat_set_speed(500, 500); + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + circle_get_da_speed_from_radius(&mainboard.traj, radius, &speed_d, &speed_a); trajectory_line_abs(&mainboard.traj, @@ -1106,21 +1301,100 @@ static void line2line(double line1x1, double line1y1, distance = angle * radius; if (distance < 0) distance = -distance; - dist_imp_target = rs_get_distance(&mainboard.rs) + - distance * mainboard.pos.phys.distance_imp_per_mm; + angle = simple_modulo_2pi(angle); + angle = DEG(angle); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), + distance, angle); + + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("line\r\n")); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + +static void halfturn(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist, double dir) +{ + uint8_t err; + double speed_d, speed_a; + double distance, angle; + + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = dir * M_PI/2.; + distance = angle * radius; + if (distance < 0) + distance = -distance; + angle = simple_modulo_2pi(angle); angle = DEG(angle); - distance += 100; /* take some margin to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance, angle); - err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target, + /* take some margin on dist to avoid deceleration */ + DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", + distance, angle); + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + DEBUG(E_USER_STRAT, "miniline"); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < + dist, TRAJ_FLAGS_NO_NEAR); + DEBUG(E_USER_STRAT, "circle2"); + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_NO_NEAR); strat_set_speed(500, 500); + DEBUG(E_USER_STRAT, "line"); trajectory_line_abs(&mainboard.traj, line2x1, line2y1, line2x2, line2y2, 150.); } +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; + int32_t radius; + int32_t dist; +}; + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { @@ -1130,30 +1404,91 @@ static void cmd_test_parsed(void *parsed_result, void *data) mainboard.angle.on = 1; mainboard.distance.on = 1; #endif - line2line(375, 347, 375, 1847, - 375, 1847, 1050, 1472, - 100, 200); - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - 180, 120); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - 180, 120); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - 150, 120); - line2line(2175, 1347, 2175, 847, - 2175, 847, 2400, 722, - 150, 120); - line2line(2175, 847, 2400, 722, - 2400, 722, 2625, 847, - 150, 100); - line2line(2400, 722, 2625, 847, - 2625, 847, 2625, 1847, - 150, 100); - line2line(2625, 847, 2625, 1847, - 2625, 1847, 375, 597, - 100, 200); + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + while (!cmdline_keypressed()) { + /****** PASS1 */ + +#define DIST_HARD_TURN 260 +#define RADIUS_HARD_TURN 100 +#define DIST_EASY_TURN 190 +#define RADIUS_EASY_TURN 190 +#define DIST_HALF_TURN 225 +#define RADIUS_HALF_TURN 130 + + /* hard turn */ + line2line(375, 597, 375, 1847, + 375, 1847, 1050, 1472, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy left and easy right !*/ + line2line(825, 1596, 1050, 1472, + 1050, 1472, 1500, 1722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1050, 1472, 1500, 1722, + 1500, 1722, 2175, 1347, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 1722, 2175, 1347, + 2175, 1347, 2175, 847, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* half turns */ + halfturn(2175, 1347, 2175, 722, + 2625, 722, 2625, 1597, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + halfturn(2625, 847, 2625, 1722, + 2175, 1722, 2175, 1097, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + + /* easy turns */ + line2line(2175, 1597, 2175, 1097, + 2175, 1097, 1500, 722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(2175, 1097, 1500, 722, + 1500, 722, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 722, 1050, 972, + 1050, 972, 375, 597, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1097, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /****** PASS2 */ + + /* easy turn */ + line2line(375, 597, 375, 1097, + 375, 1097, 1050, 1472, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1097, 1050, 1472, + 1050, 1472, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 1472, 375, 1847, + 375, 1847, 375, 1347, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy turn */ + line2line(375, 1847, 375, 1347, + 375, 1347, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1347, 1050, 972, + 1050, 972, 375, 597, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + } + trajectory_hardstop(&mainboard.traj); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 81e2e04..7bf6057 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -701,43 +702,46 @@ static void auto_position(void) strat_get_speed(&old_spdd, &old_spda); strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); - trajectory_d_rel(&mainboard.traj, -300); + trajectory_d_rel(&mainboard.traj, 300); err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; wait_ms(100); - strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + strat_reset_pos(ROBOT_WIDTH/2, + COLOR_Y(ROBOT_HALF_LENGTH_FRONT), + COLOR_A(-90)); - trajectory_d_rel(&mainboard.traj, 120); + trajectory_d_rel(&mainboard.traj, -180); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - trajectory_a_rel(&mainboard.traj, COLOR_A(90)); + trajectory_a_rel(&mainboard.traj, COLOR_A(-90)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - trajectory_d_rel(&mainboard.traj, -300); + trajectory_d_rel(&mainboard.traj, 300); err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; wait_ms(100); - strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2), - COLOR_A(90)); + strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, + DO_NOT_SET_POS, + 180); - trajectory_d_rel(&mainboard.traj, 120); + trajectory_d_rel(&mainboard.traj, -170); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; wait_ms(100); - - trajectory_a_rel(&mainboard.traj, COLOR_A(-40)); + + trajectory_a_rel(&mainboard.traj, COLOR_A(-110)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; wait_ms(100); - + strat_set_speed(old_spdd, old_spda); return; @@ -750,7 +754,7 @@ intr: static void cmd_position_parsed(void * parsed_result, void * data) { struct cmd_position_result * res = parsed_result; - + /* display raw position values */ if (!strcmp_P(res->arg1, PSTR("reset"))) { position_set(&mainboard.pos, 0, 0, 0); @@ -766,7 +770,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) #endif auto_position(); } - else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) { mainboard.our_color = I2C_COLOR_YELLOW; #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); @@ -784,7 +788,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) prog_char str_position_arg0[] = "position"; parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); -prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red"; +prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow"; parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); prog_char help_position[] = "Show/reset (x,y,a) position"; diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 81e5245..905d533 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -56,6 +56,8 @@ #include "strat.h" #include "actuator.h" +void dump_cs(const char *name, struct cs *cs); + #ifndef HOST_VERSION int32_t encoders_left_cobroller_speed(void *number) { @@ -155,8 +157,10 @@ static void do_cs(void *dummy) cpt++; #ifdef HOST_VERSION - if ((cpt & 7) == 0) + if ((cpt & 7) == 0) { + // dump_cs("dist", &mainboard.distance.cs); robotsim_dump(); + } #endif } @@ -197,14 +201,14 @@ void microb_cs_init(void) /* increase gain to decrease dist, increase left and it will turn more left */ #ifdef HOST_VERSION rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get, - LEFT_ENCODER, IMP_COEF); + LEFT_ENCODER, IMP_COEF * 1.); rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get, - RIGHT_ENCODER, IMP_COEF); + RIGHT_ENCODER, IMP_COEF * 1.); #else rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, - LEFT_ENCODER, IMP_COEF * -1.00); + LEFT_ENCODER, IMP_COEF * -1.036); rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * 1.00); + RIGHT_ENCODER, IMP_COEF * 1.037); #endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -217,11 +221,12 @@ void microb_cs_init(void) position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ - trajectory_init(&mainboard.traj); + trajectory_init(&mainboard.traj, CS_HZ); trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs, &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */ + trajectory_set_speed(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); @@ -235,8 +240,8 @@ void microb_cs_init(void) /* QUADRAMP */ quadramp_init(&mainboard.angle.qr); - quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ + quadramp_set_1st_order_vars(&mainboard.angle.qr, 500, 500); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.angle.qr, 5, 5); /* set accel */ /* CS */ cs_init(&mainboard.angle.cs); @@ -254,15 +259,15 @@ void microb_cs_init(void) /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); - pid_set_gains(&mainboard.distance.pid, 500, 100, 7000); + pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); /* QUADRAMP */ quadramp_init(&mainboard.distance.qr); - quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ + quadramp_set_1st_order_vars(&mainboard.distance.qr, 500, 500); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.distance.qr, 5., 5.); /* set accel */ /* CS */ cs_init(&mainboard.distance.cs); @@ -310,7 +315,7 @@ void microb_cs_init(void) cs_init(&mainboard.right_cobroller.cs); 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_process_out(&mainboard.right_cobroller.cs, encoders_right_cobroller_speed, RIGHT_COBROLLER_ENCODER); cs_set_consign(&mainboard.right_cobroller.cs, 0); /* Blocking detection */ @@ -323,7 +328,7 @@ void microb_cs_init(void) mainboard.angle.on = 0; mainboard.distance.on = 0; mainboard.left_cobroller.on = 1; - mainboard.right_cobroller.on = 0; + mainboard.right_cobroller.on = 1; scheduler_add_periodical_event_priority(do_cs, NULL, diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 3ca1a47..ad8cff2 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -4,6 +4,9 @@ from visual import * AREA_X = 3000. AREA_Y = 2100. +ROBOT_HEIGHT=5 # 350 +CORN_HEIGHT=5 # 150 + area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ] areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area) area_box = box(size=areasize, color=(0.0, 1.0, 0.0)) @@ -13,8 +16,8 @@ scene.autoscale=0 # all positions of robot every 5ms save_pos = [] -robot = box(pos = (0, 0, 150), - size = (250,320,350), +robot = box(pos = (0, 0, ROBOT_HEIGHT/2), + size = (250,320,ROBOT_HEIGHT), color = (0.3, 0.3, 0.3) ) last_pos = robot.pos.x, robot.pos.y, robot.pos.z @@ -191,14 +194,14 @@ def toggle_obj_disp(): while y < 2100: print x,y if waypoints[i][j] == TYPE_WHITE_CORN: - c = cylinder(axis=(0,0,1), length=150, + c = cylinder(axis=(0,0,1), length=CORN_HEIGHT, radius=25, color=(0.8,0.8,0.8), - pos=(x-AREA_X/2,y-AREA_Y/2,75)) + pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2)) area_objects.append(c) elif waypoints[i][j] == TYPE_BLACK_CORN: - c = cylinder(axis=(0,0,1), length=150, + c = cylinder(axis=(0,0,1), length=CORN_HEIGHT, radius=25, color=(0.2,0.2,0.2), - pos=(x-AREA_X/2,y-AREA_Y/2,75)) + pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2)) area_objects.append(c) elif waypoints[i][j] == TYPE_BALL: c = sphere(radius=50, color=(1., 0.,0.), @@ -224,11 +227,11 @@ def set_robot(x, y, a): global robot, last_pos, robot_trail, robot_trail_list global save_pos - robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150) + robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2) robot.axis = (math.cos(a*math.pi/180), math.sin(a*math.pi/180), 0) - robot.size = (250, 320, 350) + robot.size = (250, 320, ROBOT_HEIGHT) # save position save_pos.append((robot.pos.x, robot.pos, a)) diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 4061d19..88d4f97 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -304,7 +304,7 @@ int main(void) #endif #ifdef HOST_VERSION - strat_reset_pos(1000, 1000, -90); + strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90)); #endif cmdline_interact(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 53cfe69..48655eb 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -74,18 +74,19 @@ #define MATCH_TIME 89 /* decrease track to decrease angle */ -#define EXT_TRACK_MM 302.0188 +#define EXT_TRACK_MM 304.61875 #define VIRTUAL_TRACK_MM EXT_TRACK_MM -#define ROBOT_LENGTH 320 +#define ROBOT_HALF_LENGTH_FRONT 130 +#define ROBOT_HALF_LENGTH_REAR 120 #define ROBOT_WIDTH 320 -/* it is a 2048 imps -> 8192 because we see 1/4 period - * and diameter: 55mm -> perimeter 173mm - * 8192/173 -> 473 */ +/* it is a 1024 imps -> 4096 because we see 1/4 period + * and diameter: 55mm -> perimeter 134mm + * dist_imp_mm = 4096/134 x 10 -> 304 */ /* increase it to go further */ -#define IMP_ENCODERS 2048 -#define WHEEL_DIAMETER_MM 55.0 +#define IMP_ENCODERS 1024 +#define WHEEL_DIAMETER_MM 42.9 #define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI) #define IMP_COEF 10. #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF) @@ -118,7 +119,8 @@ #define I2C_POLL_PRIO 20 #define EEPROM_TIME_PRIO 10 -#define CS_PERIOD 5000L +#define CS_PERIOD 5000L /* in microsecond */ +#define CS_HZ (1000000. / CS_PERIOD) #define NB_LOGS 4 diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 15b1075..d0aa3ae 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -118,8 +118,8 @@ void robotsim_update(void) ((local_r_pwm * 1000 * FILTER2)/1000); /* basic collision detection */ - a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2); - d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2); + a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); + d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR); xfl = x + cos(a+a2) * d; yfl = y + sin(a+a2) * d; @@ -156,9 +156,9 @@ void robotsim_pwm(void *arg, int32_t val) { // printf("%p, %d\n", arg, val); if (arg == LEFT_PWM) - l_pwm = val; + l_pwm = (val / 1.55); else if (arg == RIGHT_PWM) - r_pwm = val; + r_pwm = (val / 1.55); } int32_t robotsim_encoder_get(void *arg) diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index e92ff4f..53d9192 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -69,13 +69,17 @@ #define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR) #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) +/* default acc */ +#define ACC_DIST 5. +#define ACC_ANGLE 5. + /* default speeds */ -#define SPEED_DIST_FAST 2500 -#define SPEED_ANGLE_FAST 2000 -#define SPEED_DIST_SLOW 1000 -#define SPEED_ANGLE_SLOW 1000 -#define SPEED_DIST_VERY_SLOW 400 -#define SPEED_ANGLE_VERY_SLOW 400 +#define SPEED_DIST_FAST 2500. +#define SPEED_ANGLE_FAST 2000. +#define SPEED_DIST_SLOW 1000. +#define SPEED_ANGLE_SLOW 1000. +#define SPEED_DIST_VERY_SLOW 400. +#define SPEED_ANGLE_VERY_SLOW 400. /* strat infos structures */ diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c new file mode 100644 index 0000000..60dead3 --- /dev/null +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -0,0 +1,55 @@ +/* + * 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_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" + +#include "main.h" + diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 40c6f86..2c52778 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 06df8c7..708f08a 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index 85997c2..7502a43 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -20,10 +20,6 @@ */ -#define DEG(x) (((double)(x)) * (180.0 / M_PI)) -#define RAD(x) (((double)(x)) * (M_PI / 180.0)) -#define M_2PI (2*M_PI) - struct xy_point { int16_t x; int16_t y; diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py index b7e5853..49d3649 100644 --- a/projects/microb2010/tests/oa/graph.py +++ b/projects/microb2010/tests/oa/graph.py @@ -197,6 +197,11 @@ def build_area(ax): x,y = build_path([(600,1972), (600+bcoef, 1972-acoef)]) ax.plot(x, y, 'r-') + p = PatchCollection([Circle((2400,972), 225)], + cmap=matplotlib.cm.jet, + alpha=0.5, facecolor=(1.,0.,0.)) + ax.add_collection(p) + # limit #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)]) #ax.plot(x, y, 'g--') @@ -275,6 +280,8 @@ def graph(filename): ax.grid(color = (0.3, 0.3, 0.3)) ax.set_xlim(-100, 3100) ax.set_ylim(-100, 2200) + #ax.set_xlim(0, 825) + #ax.set_ylim(1472, 1972) #ax.set_title('spline paths') #plt.show() fig.savefig(filename) -- 2.20.1 From 6914527de2ecfef9d790740c71739e7418246b96 Mon Sep 17 00:00:00 2001 From: zer0 Date: Tue, 13 Apr 2010 20:16:32 +0200 Subject: [PATCH 08/16] save --- .../blocking_detection_manager.c | 69 +-- .../trajectory_manager/trajectory_manager.h | 2 +- .../trajectory_manager_core.c | 96 ++-- .../trajectory_manager_core.h | 30 ++ projects/microb2010/ballboard/actuator.c | 6 +- projects/microb2010/ballboard/cmdline.h | 2 +- .../microb2010/ballboard/commands_ballboard.c | 58 +-- projects/microb2010/ballboard/commands_gen.c | 10 +- projects/microb2010/ballboard/cs.c | 37 +- projects/microb2010/ballboard/main.c | 29 +- projects/microb2010/ballboard/main.h | 5 +- projects/microb2010/ballboard/state.c | 2 + projects/microb2010/cobboard/actuator.c | 8 + projects/microb2010/cobboard/actuator.h | 1 + .../microb2010/cobboard/commands_cobboard.c | 100 ++-- projects/microb2010/cobboard/cs.c | 18 +- projects/microb2010/cobboard/i2c_protocol.c | 2 +- projects/microb2010/cobboard/main.c | 26 +- projects/microb2010/cobboard/main.h | 1 + projects/microb2010/cobboard/state.c | 112 +++-- projects/microb2010/cobboard/state.h | 1 + projects/microb2010/common/i2c_commands.h | 9 +- projects/microb2010/mainboard/commands.c | 12 + .../microb2010/mainboard/commands_mainboard.c | 381 ++++----------- projects/microb2010/mainboard/commands_traj.c | 441 ++++++++++++++---- projects/microb2010/mainboard/cs.c | 11 +- projects/microb2010/mainboard/display.py | 90 +++- projects/microb2010/mainboard/i2c_protocol.c | 7 +- projects/microb2010/mainboard/main.c | 28 +- projects/microb2010/mainboard/main.h | 1 + projects/microb2010/mainboard/robotsim.c | 78 +++- projects/microb2010/mainboard/robotsim.h | 5 +- projects/microb2010/mainboard/strat.c | 26 +- projects/microb2010/mainboard/strat.h | 15 +- 34 files changed, 1064 insertions(+), 655 deletions(-) diff --git a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c index cdb2b58..8b5f547 100644 --- a/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c +++ b/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2007) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -38,8 +38,8 @@ void bd_init(struct blocking_detection * bd) } /* thresholds */ -void bd_set_current_thresholds(struct blocking_detection * bd, - int32_t k1, int32_t k2, +void bd_set_current_thresholds(struct blocking_detection * bd, + int32_t k1, int32_t k2, uint32_t i_thres, uint16_t cpt_thres) { uint8_t flags; @@ -53,7 +53,7 @@ void bd_set_current_thresholds(struct blocking_detection * bd, } /* speed threshold */ -void bd_set_speed_threshold(struct blocking_detection * bd, +void bd_set_speed_threshold(struct blocking_detection * bd, uint16_t speed) { uint8_t flags; @@ -71,45 +71,48 @@ void bd_reset(struct blocking_detection * bd) IRQ_UNLOCK(flags); } - - /** function to be called periodically */ -void bd_manage_from_speed_cmd(struct blocking_detection * bd, - int32_t speed, int32_t cmd) +void bd_manage_from_speed_cmd(struct blocking_detection * bd, + int32_t speed, int32_t cmd) { - int32_t i=0; + int32_t i = 0; + + /* disabled */ + if (bd->cpt_thres == 0) + return; - /* if current-based blocking_detection enabled */ - if ( bd->cpt_thres ) { - i = bd->k1 * cmd - bd->k2 * speed; - if ((uint32_t)ABS(i) > bd->i_thres && - (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) { - if (bd->cpt == bd->cpt_thres - 1) - WARNING(E_BLOCKING_DETECTION_MANAGER, - "BLOCKING cmd=%ld, speed=%ld i=%ld", - cmd, speed, i); - if (bd->cpt < bd->cpt_thres) - bd->cpt++; - } - else { - bd->cpt=0; - } + i = bd->k1 * cmd - bd->k2 * speed; + + /* if i is above threshold, speed is below threshold, and cmd + * has the same sign than i */ + if ((uint32_t)ABS(i) > bd->i_thres && + (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) && + (i * cmd > 0)) { + if (bd->cpt == bd->cpt_thres - 1) + WARNING(E_BLOCKING_DETECTION_MANAGER, + "BLOCKING cmd=%ld, speed=%ld i=%ld", + cmd, speed, i); + if (bd->cpt < bd->cpt_thres) + bd->cpt++; + } + else { + bd->cpt=0; + } #if BD_DEBUG - if (bd->debug_cpt++ == BD_DEBUG) { - DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld", - cmd, speed, i); - bd->debug_cpt = 0; - } + if (bd->debug_cpt++ == BD_DEBUG) { + DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld", + cmd, speed, i); + bd->debug_cpt = 0; } #endif } /** function to be called periodically */ -void bd_manage_from_pos_cmd(struct blocking_detection * bd, - int32_t pos, int32_t cmd) +void bd_manage_from_pos_cmd(struct blocking_detection * bd, + int32_t pos, int32_t cmd) { int32_t speed = (pos - bd->prev_pos); - bd_manage_from_speed_cmd(bd, speed, cmd); + bd_manage_from_speed_cmd(bd, speed, cmd); bd->prev_pos = pos; } diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 594b7b0..3e22502 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -130,7 +130,7 @@ void trajectory_set_robot_params(struct trajectory *traj, void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed); /** set speed consign */ -void trajectory_set_speed(struct trajectory *traj, double d_acc, double a_acc); +void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc); /** * set windows for trajectory. diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 5246285..5197300 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -643,13 +643,11 @@ static void trajectory_manager_line_event(struct trajectory *traj) } /* position consign is infinite */ - d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign = pos_mm2imp(traj, v2pol_target.r); d_consign += rs_get_distance(traj->robot); - /* angle consign */ - a_consign = (int32_t)(v2pol_target.theta * - (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2.2); + /* angle consign (1.1 to avoid oscillations) */ + a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1; a_consign += rs_get_angle(traj->robot); EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " @@ -849,7 +847,7 @@ void trajectory_line_abs(struct trajectory *traj, * - Va_out: the angular speed to configure in quadramp * - remain_d_mm_out: remaining distance before start to turn */ -static uint8_t calc_clitoid(struct trajectory *traj, +static int8_t calc_clitoid(struct trajectory *traj, double x, double y, double a_rad, double alpha_deg, double beta_deg, double R_mm, double Vd, double Amax, double d_inter_mm, @@ -857,12 +855,14 @@ static uint8_t calc_clitoid(struct trajectory *traj, { double Vd_mm_s; double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; + double t, tau, d_mm, alpha_rad, beta_rad; double remain_d_mm; double Aa, Aa_rd_s2; line_t line1, line2; - point_t robot, intersect, pt2, center, proj; + line_t line1_int, line2_int; + point_t robot, intersect, pt2, center, proj, M; vect_t v; + double xm, ym, L, A; /* param check */ if (fabs(alpha_deg) <= fabs(beta_deg)) { @@ -896,11 +896,6 @@ static uint8_t calc_clitoid(struct trajectory *traj, return -1; } - /* the robot position */ -/* x = position_get_x_double(&mainboard.pos); */ -/* y = position_get_y_double(&mainboard.pos); */ -/* a_rad = position_get_a_rad_double(&mainboard.pos); */ - /* define line1 and line2 */ robot.x = x; robot.y = y; @@ -913,46 +908,74 @@ static uint8_t calc_clitoid(struct trajectory *traj, DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)", intersect.x, intersect.y); - /* the center of the circle is at (d_mm, d_mm) when we have to - * start the clothoid */ - d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * - sqrt(M_PI) / 2.; + /* L and A are the parameters of the clothoid, xm and ym are + * the relative coords (starting from the beginning of + * clothoid) of the crossing point between the clothoid and + * the circle. */ + L = Vd_mm_s * t; + A = R_mm * sqrt(fabs(alpha_rad - beta_rad)); + xm = + L + - (pow(L, 5) / (40. * pow(A, 4))) + + (pow(L, 9) / (3456. * pow(A, 8))) + - (pow(L, 13) / (599040. * pow(A, 12))); + ym = + (pow(L, 3) / (6. * pow(A, 2))) + - (pow(L, 7) / (336. * pow(A, 6))) + + (pow(L, 11) / (42240. * pow(A, 10))) + - (pow(L, 15) / (9676800. * pow(A, 14))); + DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)", + xm, ym); + + /* the center of the circle is at d_mm when we have to start + * the clothoid */ + tau = (alpha_rad - beta_rad) / 2.; + d_mm = ym + (R_mm * cos(tau)); DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm); /* translate line1 */ + memcpy(&line1_int, &line1, sizeof(line1_int)); + memcpy(&line2_int, &line2, sizeof(line2_int)); v.x = intersect.x - robot.x; v.y = intersect.y - robot.y; - if (a_rad > 0) + if (alpha_rad > 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line1, &v); + line_translate(&line1_int, &v); + DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y); - /* translate line2 */ + /* translate line2_int */ v.x = intersect.x - pt2.x; v.y = intersect.y - pt2.y; - if (a_rad > 0) + if (alpha_rad < 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line2, &v); + line_translate(&line2_int, &v); + DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y); /* find the center of the circle, at the intersection of the * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { + if (intersect_line(&line1_int, &line2_int, ¢er) != 1) { DEBUG(E_TRAJECTORY, "cannot find circle center"); return -1; } DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y); - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_TRAJECTORY, "proj=(%2.2f,%2.2f)", proj.x, proj.y); + /* M is the same point than xm, ym but in absolute coords */ + M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm; + DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y); + + /* project M on line 1 */ + proj_pt_line(&M, &line1, &proj); + DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y); /* process remaining distance before start turning */ - remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); + remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm); DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm); if (remain_d_mm < 0) { DEBUG(E_TRAJECTORY, "too late, cannot turn"); @@ -966,6 +989,7 @@ static uint8_t calc_clitoid(struct trajectory *traj, return 0; } +/* after the line, start the clothoid */ static void start_clitoid(struct trajectory *traj) { double Aa = traj->target.line.Aa; @@ -975,12 +999,14 @@ static void start_clitoid(struct trajectory *traj) double d; delete_event(traj); + DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f", + __FUNCTION__, Va, Aa); traj->state = RUNNING_CLITOID_CURVE; - set_quadramp_acc(traj, Aa, Aa); - set_quadramp_speed(traj, Va, Va); - d = R_mm * a_rad; - d *= 2.; /* margin to avoid deceleration */ + d = fabs(R_mm * a_rad); + d *= 3.; /* margin to avoid deceleration */ trajectory_d_a_rel(traj, d, DEG(a_rad)); + set_quadramp_acc(traj, traj->d_acc, Aa); + set_quadramp_speed(traj, traj->d_speed, Va); } @@ -1010,11 +1036,12 @@ static void start_clitoid(struct trajectory *traj) int8_t trajectory_clitoid(struct trajectory *traj, double x, double y, double a, double advance, double alpha_deg, double beta_deg, double R_mm, - double Vd, double d_inter_mm) + double d_inter_mm) { - double remain = 0, Aa = 0, Va = 0; + double remain = 0, Aa = 0, Va = 0, Vd; double turnx, turny; + Vd = traj->d_speed; if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm, Vd, traj->a_acc, d_inter_mm, &Aa, &Va, &remain) < 0) @@ -1029,6 +1056,9 @@ int8_t trajectory_clitoid(struct trajectory *traj, traj->target.line.R = R_mm; traj->target.line.turn_pt.x = turnx; traj->target.line.turn_pt.y = turny; + DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f", + __FUNCTION__, turnx, turny); + __trajectory_line_abs(traj, x, y, turnx, turny, advance); traj->state = RUNNING_CLITOID_LINE; diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h index a724f69..b5a1395 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h @@ -111,3 +111,33 @@ void trajectory_manager_event(void * param); /*********** *CIRCLE */ /* XXX TODO */ + +/*********** CLITOID */ + +/** + * do a superb curve joining line1 to line2 which is composed of: + * - a clothoid starting from line1 + * - a circle + * - another clothoid up to line2 + * this curve is called a clitoid (hehe) + * + * the function assumes that the initial linear speed is Vd and + * angular speed is 0. + * + * - x,y,a: starting position + * - advance: parameter for line following + * - alpha: total angle + * - beta: circular part of angle (lower than alpha) + * - R: the radius of the circle (must be != 0) + * - Vd: linear speed to use (in imp per cs period) + * - Amax: maximum angular acceleration + * - d_inter: distance in mm until the intersection of the + * 2 lines + * + * return 0 if trajectory can be loaded, then it is processed in + * background. + */ +int8_t trajectory_clitoid(struct trajectory *traj, + double x, double y, double a, double advance, + double alpha_deg, double beta_deg, double R_mm, + double d_inter_mm); diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c index 2509d7c..7365427 100644 --- a/projects/microb2010/ballboard/actuator.c +++ b/projects/microb2010/ballboard/actuator.c @@ -42,11 +42,11 @@ #include "main.h" -#define ROLLER_ON 800 +#define ROLLER_ON -1200 #define ROLLER_OFF 0 -#define ROLLER_REVERSE -800 +#define ROLLER_REVERSE 1200 -#define FORKROT_DEPLOYED 1000 +#define FORKROT_DEPLOYED -50000 #define FORKROT_PACKED 0 #define FORKTRANS_LEFT 0 diff --git a/projects/microb2010/ballboard/cmdline.h b/projects/microb2010/ballboard/cmdline.h index 57dfa49..04ce1f3 100644 --- a/projects/microb2010/ballboard/cmdline.h +++ b/projects/microb2010/ballboard/cmdline.h @@ -35,6 +35,6 @@ static inline uint8_t cmdline_keypressed(void) { return (uart_recv_nowait(CMDLINE_UART) != -1); } -static inline uint8_t cmdline_getchar(void) { +static inline int16_t cmdline_getchar(void) { return uart_recv_nowait(CMDLINE_UART); } diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c index fd45914..ba570a4 100644 --- a/projects/microb2010/ballboard/commands_ballboard.c +++ b/projects/microb2010/ballboard/commands_ballboard.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -63,22 +63,24 @@ static void cmd_event_parsed(void *parsed_result, void *data) u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) ballboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) ballboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & ballboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & ballboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & ballboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & ballboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & ballboard.flags) ? "on":"off"); } return; } @@ -92,6 +94,8 @@ static void cmd_event_parsed(void *parsed_result, void *data) bit = DO_BD; else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) @@ -104,13 +108,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } ballboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & ballboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -121,9 +125,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -161,8 +165,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -275,7 +279,7 @@ struct cmd_state3_result { }; /* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, +static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; @@ -300,9 +304,9 @@ parse_pgm_inst_t cmd_state3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, NULL, }, }; @@ -331,7 +335,7 @@ parse_pgm_inst_t cmd_state_machine = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_machine, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, + (prog_void *)&cmd_state_machine_arg0, NULL, }, }; @@ -363,8 +367,8 @@ parse_pgm_inst_t cmd_state_debug = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_debug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, NULL, }, }; @@ -406,8 +410,8 @@ parse_pgm_inst_t cmd_fork = { .data = NULL, /* 2nd arg of func */ .help_str = help_fork, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_fork_arg0, - (prog_void *)&cmd_fork_arg1, + (prog_void *)&cmd_fork_arg0, + (prog_void *)&cmd_fork_arg1, NULL, }, }; @@ -445,8 +449,8 @@ parse_pgm_inst_t cmd_roller = { .data = NULL, /* 2nd arg of func */ .help_str = help_roller, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_roller_arg0, - (prog_void *)&cmd_roller_arg1, + (prog_void *)&cmd_roller_arg0, + (prog_void *)&cmd_roller_arg1, NULL, }, }; @@ -474,7 +478,7 @@ parse_pgm_inst_t cmd_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg0, NULL, }, }; diff --git a/projects/microb2010/ballboard/commands_gen.c b/projects/microb2010/ballboard/commands_gen.c index b2a1e5d..66dccc1 100644 --- a/projects/microb2010/ballboard/commands_gen.c +++ b/projects/microb2010/ballboard/commands_gen.c @@ -357,9 +357,7 @@ static const prog_char i2c_log[] = "i2c"; static const prog_char i2cproto_log[] = "i2cproto"; static const prog_char sensor_log[] = "sensor"; static const prog_char block_log[] = "bd"; -static const prog_char beacon_log[] = "beacon"; -static const prog_char scanner_log[] = "scanner"; -static const prog_char imgprocess_log[] = "imgprocess"; +static const prog_char state_log[] = "state"; struct log_name_and_num { const prog_char * name; @@ -372,9 +370,7 @@ static const struct log_name_and_num log_name_and_num[] = { { i2cproto_log, E_USER_I2C_PROTO }, { sensor_log, E_USER_SENSOR }, { block_log, E_BLOCKING_DETECTION_MANAGER }, - { beacon_log, E_USER_BEACON }, - { scanner_log, E_USER_SCANNER }, - { imgprocess_log, E_USER_IMGPROCESS }, + { state_log, E_USER_ST_MACH }, }; static uint8_t @@ -523,7 +519,7 @@ static void cmd_log_type_parsed(void * parsed_result, void * data) prog_char str_log_arg1_type[] = "type"; parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type); /* keep it sync with log_name_and_num above */ -prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess"; +prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state"; parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type); prog_char str_log_arg3[] = "on#off"; parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3); diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index bab700d..cf122e6 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -72,10 +72,20 @@ static void do_cs(void *dummy) if (ballboard.forkrot.on) cs_manage(&ballboard.forkrot.cs); } - if (ballboard.flags & DO_BD) { + if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) { bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs); bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs); bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs); + + /* urgent case: stop power on blocking */ + if (ballboard.flags & DO_ERRBLOCKING) { + if (bd_get(&ballboard.roller.bd) || + bd_get(&ballboard.forktrans.bd) || + bd_get(&ballboard.forkrot.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } } if (ballboard.flags & DO_POWER) BRAKE_OFF(); @@ -86,7 +96,7 @@ static void do_cs(void *dummy) void dump_cs(const char *name, struct cs *cs) { printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld\r\n"), + "in=% .5ld out=% .5ld\r\n"), name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -119,10 +129,15 @@ void microb_cs_init(void) cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER); cs_set_consign(&ballboard.roller.cs, 0); + /* Blocking detection */ + bd_init(&ballboard.roller.bd); + bd_set_speed_threshold(&ballboard.roller.bd, 150); + bd_set_current_thresholds(&ballboard.roller.bd, 500, 8000, 1000000, 200); + /* ---- CS forktrans */ /* PID */ pid_init(&ballboard.forktrans.pid); - pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250); + pid_set_gains(&ballboard.forktrans.pid, 30, 5, 0); pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forktrans.pid, 6); pid_set_derivate_filter(&ballboard.forktrans.pid, 6); @@ -143,19 +158,19 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forktrans.bd); bd_set_speed_threshold(&ballboard.forktrans.bd, 150); - bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 200); /* ---- CS forkrot */ /* PID */ pid_init(&ballboard.forkrot.pid); - pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250); + pid_set_gains(&ballboard.forkrot.pid, 30, 5, 0); pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047); pid_set_out_shift(&ballboard.forkrot.pid, 6); pid_set_derivate_filter(&ballboard.forkrot.pid, 6); /* QUADRAMP */ quadramp_init(&ballboard.forkrot.qr); - quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */ + quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */ quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */ /* CS */ @@ -169,15 +184,15 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&ballboard.forkrot.bd); bd_set_speed_threshold(&ballboard.forkrot.bd, 150); - bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200); /* set them on !! */ - ballboard.roller.on = 0; + ballboard.roller.on = 1; ballboard.forktrans.on = 1; ballboard.forkrot.on = 1; scheduler_add_periodical_event_priority(do_cs, NULL, - 5000L / SCHEDULER_UNIT, + 5000L / SCHEDULER_UNIT, CS_PRIO); } diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index 082b658..ed8716b 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -112,16 +112,21 @@ void bootloader(void) void do_led_blink(__attribute__((unused)) void *dummy) { -#if 1 /* simple blink */ - static uint8_t a=0; + static uint8_t a = 0; - if(a) - LED1_ON(); - else - LED1_OFF(); - - a = !a; -#endif + if (ballboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -149,7 +154,8 @@ int main(void) LED1_OFF(); memset(&gen, 0, sizeof(gen)); memset(&ballboard, 0, sizeof(ballboard)); - ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD + ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER | + DO_ERRBLOCKING | DO_BD; /* UART */ uart_init(); @@ -239,6 +245,9 @@ int main(void) /* ax12 */ ax12_user_init(); + gen.logs[0] = E_USER_ST_MACH; + gen.log_level = 5; + sei(); printf_P(PSTR("\r\n")); diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index 3ebb1e7..cde4608 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -60,9 +60,7 @@ /** ERROR NUMS */ #define E_USER_I2C_PROTO 195 #define E_USER_SENSOR 196 -#define E_USER_BEACON 197 -#define E_USER_SCANNER 198 -#define E_USER_IMGPROCESS 199 +#define E_USER_ST_MACH 197 #define LED_PRIO 170 #define TIME_PRIO 160 @@ -115,6 +113,7 @@ struct ballboard { #define DO_CS 2 #define DO_BD 4 #define DO_POWER 8 +#define DO_ERRBLOCKING 16 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 9568be8..0492e18 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -94,6 +94,7 @@ static void state_debug_wait_key_pressed(void) int8_t state_set_mode(uint8_t mode) { state_mode = mode; + STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode); /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ @@ -135,6 +136,7 @@ static void state_do_harvest(void) static void state_do_eject(void) { roller_reverse(); + time_wait_ms(2000); } /* main state machine */ diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 6811854..1b6e91f 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -103,6 +103,14 @@ void cobroller_off(uint8_t side) cobboard.right_cobroller_speed = 0; } +void cobroller_reverse(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + cobboard.left_cobroller_speed = -COBROLLER_SPEED; + else + cobboard.right_cobroller_speed = COBROLLER_SPEED; +} + void actuator_init(void) { diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index 5b59a73..4896447 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -33,6 +33,7 @@ void servo_door_block(void); void cobroller_on(uint8_t side); void cobroller_off(uint8_t side); +void cobroller_reverse(uint8_t side); #endif diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 4ea3b80..bea27e9 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -68,20 +68,22 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * struct cmd_event_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) cobboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) cobboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & cobboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & cobboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & cobboard.flags) ? "on":"off"); } return; } @@ -95,6 +97,8 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * bit = DO_BD; else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) @@ -107,13 +111,13 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * } cobboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & cobboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -124,9 +128,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -164,8 +168,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -204,8 +208,8 @@ parse_pgm_inst_t cmd_state1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state1_arg0, - (prog_void *)&cmd_state1_arg1, + (prog_void *)&cmd_state1_arg0, + (prog_void *)&cmd_state1_arg1, NULL, }, }; @@ -272,9 +276,9 @@ parse_pgm_inst_t cmd_state2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state2_arg0, - (prog_void *)&cmd_state2_arg1, - (prog_void *)&cmd_state2_arg2, + (prog_void *)&cmd_state2_arg0, + (prog_void *)&cmd_state2_arg1, + (prog_void *)&cmd_state2_arg2, NULL, }, }; @@ -290,7 +294,7 @@ struct cmd_state3_result { }; /* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, +static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; @@ -315,9 +319,9 @@ parse_pgm_inst_t cmd_state3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, NULL, }, }; @@ -346,7 +350,7 @@ parse_pgm_inst_t cmd_state_machine = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_machine, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, + (prog_void *)&cmd_state_machine_arg0, NULL, }, }; @@ -378,8 +382,8 @@ parse_pgm_inst_t cmd_state_debug = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_debug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, NULL, }, }; @@ -417,8 +421,8 @@ parse_pgm_inst_t cmd_servo_door = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_door, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_door_arg0, - (prog_void *)&cmd_servo_door_arg1, + (prog_void *)&cmd_servo_door_arg0, + (prog_void *)&cmd_servo_door_arg1, NULL, }, }; @@ -464,9 +468,9 @@ parse_pgm_inst_t cmd_cobroller = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobroller, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobroller_arg0, - (prog_void *)&cmd_cobroller_arg1, - (prog_void *)&cmd_cobroller_arg2, + (prog_void *)&cmd_cobroller_arg0, + (prog_void *)&cmd_cobroller_arg1, + (prog_void *)&cmd_cobroller_arg2, NULL, }, }; @@ -504,8 +508,8 @@ parse_pgm_inst_t cmd_shovel = { .data = NULL, /* 2nd arg of func */ .help_str = help_shovel, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_shovel_arg0, - (prog_void *)&cmd_shovel_arg1, + (prog_void *)&cmd_shovel_arg0, + (prog_void *)&cmd_shovel_arg1, NULL, }, }; @@ -541,8 +545,8 @@ parse_pgm_inst_t cmd_servo_carry = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_carry, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_carry_arg0, - (prog_void *)&cmd_servo_carry_arg1, + (prog_void *)&cmd_servo_carry_arg0, + (prog_void *)&cmd_servo_carry_arg1, NULL, }, }; @@ -597,9 +601,9 @@ parse_pgm_inst_t cmd_spickle = { .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, - (prog_void *)&cmd_spickle_arg2, + (prog_void *)&cmd_spickle_arg0, + (prog_void *)&cmd_spickle_arg1, + (prog_void *)&cmd_spickle_arg2, NULL, }, }; @@ -680,8 +684,8 @@ parse_pgm_inst_t cmd_spickle_params_show = { .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, + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1_show, NULL, }, }; @@ -702,7 +706,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_spickle_params2_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("coef"))) { spickle_set_coefs(res->arg2, res->arg3); } @@ -728,10 +732,10 @@ parse_pgm_inst_t cmd_spickle_params2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1, - (prog_void *)&cmd_spickle_params2_arg2, - (prog_void *)&cmd_spickle_params2_arg3, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1, + (prog_void *)&cmd_spickle_params2_arg2, + (prog_void *)&cmd_spickle_params2_arg3, NULL, }, }; @@ -746,8 +750,8 @@ parse_pgm_inst_t cmd_spickle_params2_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1_show, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1_show, NULL, }, }; @@ -776,7 +780,7 @@ parse_pgm_inst_t cmd_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg0, NULL, }, }; diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index f9c0b7d..a718970 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -95,10 +95,20 @@ static void do_cs(__attribute__((unused)) void *dummy) } #endif - if (cobboard.flags & DO_BD) { + if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) { bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs); bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs); bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs); + + /* urgent case: stop power on blocking */ + if (cobboard.flags & DO_ERRBLOCKING) { + if (bd_get(&cobboard.left_spickle.bd) || + bd_get(&cobboard.right_spickle.bd) || + bd_get(&cobboard.shovel.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } } if (cobboard.flags & DO_POWER) BRAKE_OFF(); @@ -154,7 +164,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.left_spickle.bd); bd_set_speed_threshold(&cobboard.left_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS right_spickle */ /* PID */ @@ -174,7 +184,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.right_spickle.bd); bd_set_speed_threshold(&cobboard.right_spickle.bd, 150); - bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 200); /* ---- CS shovel */ /* PID */ @@ -200,7 +210,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&cobboard.shovel.bd); bd_set_speed_threshold(&cobboard.shovel.bd, 150); - bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40); + bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 200); /* set them on (or not) !! */ cobboard.left_spickle.on = 0; diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 9de2d4c..8d86b49 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -93,7 +93,7 @@ static void i2c_send_status(void) /* status */ ans.mode = state_get_mode(); - ans.status = 0x55; /* TODO */ + ans.status = state_get_status(); ans.left_cobroller_speed = cobboard.left_cobroller_speed; ans.right_cobroller_speed = cobboard.right_cobroller_speed; diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 24f14bc..1f4065b 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -115,16 +115,21 @@ void bootloader(void) void do_led_blink(__attribute__((unused)) void *dummy) { -#if 1 /* simple blink */ - static uint8_t a=0; + static uint8_t a = 0; - if(a) - LED1_ON(); - else - LED1_OFF(); - - a = !a; -#endif + if (cobboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -153,7 +158,8 @@ int main(void) memset(&gen, 0, sizeof(gen)); memset(&cobboard, 0, sizeof(cobboard)); /* cs is enabled after arm_calibrate() */ - cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD + cobboard.flags = DO_ENCODERS | DO_POWER | DO_BD | + DO_ERRBLOCKING; /* UART */ uart_init(); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 0d07a9c..699a5c1 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -119,6 +119,7 @@ struct cobboard { #define DO_CS 2 #define DO_BD 4 #define DO_POWER 8 +#define DO_ERRBLOCKING 16 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 9d082ba..a16675d 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -58,6 +58,7 @@ static struct vt100 local_vt100; static volatile uint8_t state_mode; +static volatile uint8_t state_status; static uint8_t cob_count; /* short aliases */ @@ -72,13 +73,6 @@ static uint8_t cob_count; uint8_t state_debug = 0; -#if 0 -static void state_dump_sensors(void) -{ - STMCH_DEBUG("TODO\n"); -} -#endif - uint8_t state_get_cob_count(void) { return cob_count; @@ -125,11 +119,30 @@ static uint8_t state_no_cob_inside(void) !sensor_get(S_COB_INSIDE_R); } +/* pack/deploy spickles depending on mode */ +static void spickle_prepare(uint8_t side) +{ + if (cob_count >= 5) + spickle_pack(side); + else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode)) + spickle_mid(side); + else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode)) + spickle_deploy(side); + else + spickle_pack(side); +} + /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { state_mode = mode; + /* preempt current action if not busy */ + if (state_status != I2C_COBBOARD_STATUS_LBUSY) + spickle_prepare(I2C_LEFT_SIDE); + if (state_status != I2C_COBBOARD_STATUS_RBUSY) + spickle_prepare(I2C_RIGHT_SIDE); + /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ /* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */ @@ -159,9 +172,19 @@ uint8_t state_get_mode(void) return state_mode; } +uint8_t state_get_status(void) +{ + return state_status; +} + /* harvest cobs from area */ static void state_do_harvest(uint8_t side) { + if (side == I2C_LEFT_SIDE) + state_status = I2C_COBBOARD_STATUS_LBUSY; + else + state_status = I2C_COBBOARD_STATUS_RBUSY; + /* if there is no cob, return */ if (state_cob_present(side)) return; @@ -178,13 +201,38 @@ static void state_do_harvest(uint8_t side) time_wait_ms(250); cobroller_on(side); + /* check that cob is correctly in place */ if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) { if (state_no_cob_inside()) { - printf_P(PSTR("NO COB\r\n")); + STMCH_DEBUG("no cob"); + return; + } + STMCH_DEBUG("bad cob state"); + + /* while cob is not correctly placed try to extract + * it as much as we can */ + while (state_cob_inside() == 0 && + state_no_cob_inside() == 0) { + uint8_t cpt = 0; + if (cpt == 0) + cobroller_reverse(side); + else if (cpt == 1) + cobroller_off(side); + else if (cpt == 2) + cobroller_on(side); + cpt ++; + cpt %= 3; + shovel_mid(); + time_wait_ms(250); + shovel_down(); + time_wait_ms(250); + } + + STMCH_DEBUG("cob removed"); + if (state_no_cob_inside()) { + STMCH_DEBUG("no cob"); return; } - printf_P(PSTR("BAD COB - press a key\r\n")); - while(!cmdline_keypressed()); } /* cob is inside, switch off roller */ @@ -208,10 +256,11 @@ static void state_do_harvest(uint8_t side) /* store it */ shovel_up(); - if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { - BRAKE_ON(); - printf_P(PSTR("BLOCKED\r\n")); - while(!cmdline_keypressed()); + while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_down(); + time_wait_ms(250); + shovel_up(); } state_debug_wait_key_pressed(); @@ -224,10 +273,11 @@ static void state_do_harvest(uint8_t side) shovel_down(); - if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { - BRAKE_ON(); - printf_P(PSTR("BLOCKED\r\n")); - while(!cmdline_keypressed()); + while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) { + STMCH_DEBUG("shovel blocked"); + shovel_up(); + time_wait_ms(250); + shovel_down(); } STMCH_DEBUG("end"); @@ -236,6 +286,7 @@ static void state_do_harvest(uint8_t side) /* eject cobs */ static void state_do_eject(void) { + state_status = I2C_COBBOARD_STATUS_EJECT; cob_count = 0; shovel_mid(); servo_carry_open(); @@ -251,33 +302,19 @@ void state_machine(void) { while (state_want_exit() == 0) { + state_status = I2C_COBBOARD_STATUS_READY; + /* init */ if (INIT(state_mode)) { state_mode &= (~I2C_COBBOARD_MODE_INIT); state_init(); } + /* pack/deply spickles, enable/disable roller */ cobroller_off(I2C_LEFT_SIDE); cobroller_off(I2C_RIGHT_SIDE); - - /* pack/deply spickles, enable/disable roller */ - if (cob_count >= 5) - spickle_pack(I2C_LEFT_SIDE); - else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode)) - spickle_mid(I2C_LEFT_SIDE); - else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) - spickle_deploy(I2C_LEFT_SIDE); - else - spickle_pack(I2C_LEFT_SIDE); - - if (cob_count >= 5) - spickle_pack(I2C_RIGHT_SIDE); - else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode)) - spickle_mid(I2C_RIGHT_SIDE); - else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) - spickle_deploy(I2C_RIGHT_SIDE); - else - spickle_pack(I2C_RIGHT_SIDE); + spickle_prepare(I2C_LEFT_SIDE); + spickle_prepare(I2C_RIGHT_SIDE); /* harvest */ if (cob_count < 5) { @@ -305,4 +342,5 @@ void state_init(void) spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; cob_count = 0; + state_status = I2C_COBBOARD_STATUS_READY; } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index 8dcb6c1..d9f95fc 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -27,6 +27,7 @@ int8_t state_set_mode(uint8_t mode); /* get current state */ uint8_t state_get_mode(void); +uint8_t state_get_status(void); uint8_t state_get_cob_count(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 87f98c7..d6794af 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -52,7 +52,7 @@ struct i2c_cmd_hdr { struct i2c_cmd_led_control { struct i2c_cmd_hdr hdr; uint8_t led_num:7; - uint8_t state:1; + uint8_t state:1; }; /****/ @@ -115,9 +115,10 @@ struct i2c_ans_cobboard_status { /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; -#define I2C_COBBOARD_STATUS_F_READY 0x00 -#define I2C_COBBOARD_STATUS_F_BUSY 0x01 -#define I2C_COBBOARD_STATUS_F_EXCPT 0x02 +#define I2C_COBBOARD_STATUS_READY 0x00 +#define I2C_COBBOARD_STATUS_LBUSY 0x01 +#define I2C_COBBOARD_STATUS_RBUSY 0x02 +#define I2C_COBBOARD_STATUS_EJECT 0x03 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index cdbeb6e..e5bb456 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -74,6 +74,10 @@ 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_ballboard_show; +extern parse_pgm_inst_t cmd_ballboard_setmode1; +extern parse_pgm_inst_t cmd_ballboard_setmode2; +extern parse_pgm_inst_t cmd_ballboard_setmode3; extern parse_pgm_inst_t cmd_beacon_start; extern parse_pgm_inst_t cmd_servo_balls; extern parse_pgm_inst_t cmd_clitoid; @@ -82,6 +86,8 @@ extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ extern parse_pgm_inst_t cmd_traj_speed; extern parse_pgm_inst_t cmd_traj_speed_show; +extern parse_pgm_inst_t cmd_traj_acc; +extern parse_pgm_inst_t cmd_traj_acc_show; extern parse_pgm_inst_t cmd_trajectory; extern parse_pgm_inst_t cmd_trajectory_show; extern parse_pgm_inst_t cmd_circle_coef; @@ -159,6 +165,10 @@ parse_pgm_ctx_t main_ctx[] = { (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_ballboard_show, + (parse_pgm_inst_t *)&cmd_ballboard_setmode1, + (parse_pgm_inst_t *)&cmd_ballboard_setmode2, + (parse_pgm_inst_t *)&cmd_ballboard_setmode3, (parse_pgm_inst_t *)&cmd_servo_balls, (parse_pgm_inst_t *)&cmd_clitoid, (parse_pgm_inst_t *)&cmd_test, @@ -166,6 +176,8 @@ parse_pgm_ctx_t main_ctx[] = { /* commands_traj.c */ (parse_pgm_inst_t *)&cmd_traj_speed, (parse_pgm_inst_t *)&cmd_traj_speed_show, + (parse_pgm_inst_t *)&cmd_traj_acc, + (parse_pgm_inst_t *)&cmd_traj_acc_show, (parse_pgm_inst_t *)&cmd_trajectory, (parse_pgm_inst_t *)&cmd_trajectory_show, (parse_pgm_inst_t *)&cmd_circle_coef, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fe4fabb..a69208b 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -81,29 +82,30 @@ static void cmd_event_parsed(void *parsed_result, void *data) u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS | - DO_BD | DO_TIMER | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) mainboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("rs is %s\r\n"), + printf_P(PSTR("rs is %s\r\n"), (DO_RS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("pos is %s\r\n"), + printf_P(PSTR("pos is %s\r\n"), (DO_POS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & mainboard.flags) ? "on":"off"); - printf_P(PSTR("timer is %s\r\n"), + printf_P(PSTR("timer is %s\r\n"), (DO_TIMER & mainboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & mainboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & mainboard.flags) ? "on":"off"); } return; } @@ -126,6 +128,8 @@ static void cmd_event_parsed(void *parsed_result, void *data) } else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; @@ -141,13 +145,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } mainboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & mainboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power"; +prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -158,9 +162,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -181,7 +185,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) printf("not implemented\n"); #else uint16_t i = 0, ret = 0, ret2 = 0; - + if (mainboard.flags & DO_ENCODERS) { printf_P(PSTR("Disable encoder event first\r\n")); return; @@ -211,7 +215,7 @@ parse_pgm_inst_t cmd_spi_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_spi_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spi_test_arg0, + (prog_void *)&cmd_spi_test_arg0, NULL, }, }; @@ -251,8 +255,8 @@ parse_pgm_inst_t cmd_opponent = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, - (prog_void *)&cmd_opponent_arg1, + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, NULL, }, }; @@ -269,10 +273,10 @@ parse_pgm_inst_t cmd_opponent_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg0, (prog_void *)&cmd_opponent_arg1_set, - (prog_void *)&cmd_opponent_arg2, - (prog_void *)&cmd_opponent_arg3, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, NULL, }, }; @@ -338,9 +342,9 @@ parse_pgm_inst_t cmd_start = { .data = NULL, /* 2nd arg of func */ .help_str = help_start, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_start_arg0, - (prog_void *)&cmd_start_color, - (prog_void *)&cmd_start_debug, + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, NULL, }, }; @@ -358,7 +362,7 @@ struct cmd_interact_result { static void print_cs(void) { printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " - "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -371,7 +375,7 @@ static void print_cs(void) static void print_pos(void) { - printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); @@ -484,7 +488,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) wait_ms(10); continue; } - + if (cmd == -1) { switch(c) { case '1': print ^= PRINT_POS; break; @@ -494,7 +498,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) case '5': print ^= PRINT_TIME; break; case '6': print ^= PRINT_BLOCKING; break; - case 'q': + case 'q': if (mainboard.flags & DO_CS) strat_hardstop(); pwm_set_and_save(LEFT_PWM, 0); @@ -504,21 +508,21 @@ static void cmd_interact_parsed(void * parsed_result, void * data) pwm_set_and_save(LEFT_PWM, 0); pwm_set_and_save(RIGHT_PWM, 0); break; - default: + default: break; } } else { switch(cmd) { - case KEY_UP_ARR: + case KEY_UP_ARR: pwm_set_and_save(LEFT_PWM, 1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_LEFT_ARR: + case KEY_LEFT_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_DOWN_ARR: + case KEY_DOWN_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, -1200); break; @@ -541,7 +545,7 @@ parse_pgm_inst_t cmd_interact = { .data = NULL, /* 2nd arg of func */ .help_str = help_interact, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_interact_arg0, + (prog_void *)&cmd_interact_arg0, NULL, }, }; @@ -588,8 +592,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -609,11 +613,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_feedback(&mainboard.angle.cs), cs_get_out(&mainboard.angle.cs)); - printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.distance.cs), cs_get_filtered_feedback(&mainboard.distance.cs), cs_get_out(&mainboard.distance.cs)); @@ -634,8 +638,8 @@ parse_pgm_inst_t cmd_rs = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_arg0, - (prog_void *)&cmd_rs_arg1, + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, NULL, }, }; @@ -668,7 +672,7 @@ parse_pgm_inst_t cmd_i2cdebug = { .data = NULL, /* 2nd arg of func */ .help_str = help_i2cdebug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_i2cdebug_arg0, + (prog_void *)&cmd_i2cdebug_arg0, NULL, }, }; @@ -707,8 +711,8 @@ parse_pgm_inst_t cmd_cobboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_show_arg0, - (prog_void *)&cmd_cobboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; @@ -725,16 +729,12 @@ struct cmd_cobboard_setmode1_result { /* function called when cmd_cobboard_setmode1 is parsed successfully */ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) i2c_cobboard_mode_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_mode_eject(); -#endif } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -748,8 +748,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode1_arg0, - (prog_void *)&cmd_cobboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; @@ -767,9 +767,6 @@ struct cmd_cobboard_setmode2_result { /* function called when cmd_cobboard_setmode2 is parsed successfully */ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -784,7 +781,6 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) i2c_cobboard_mode_harvest(side); else if (!strcmp_P(res->arg1, PSTR("pack"))) i2c_cobboard_mode_pack(side); -#endif } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; @@ -800,9 +796,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode2_arg0, - (prog_void *)&cmd_cobboard_setmode2_arg1, - (prog_void *)&cmd_cobboard_setmode2_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; @@ -820,13 +816,9 @@ struct cmd_cobboard_setmode3_result { /* function called when cmd_cobboard_setmode3 is parsed successfully */ static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_cobboard_setmode3_arg0[] = "cobboard"; @@ -841,9 +833,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode3_arg0, - (prog_void *)&cmd_cobboard_setmode3_arg1, - (prog_void *)&cmd_cobboard_setmode3_arg2, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; @@ -880,8 +872,8 @@ parse_pgm_inst_t cmd_ballboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_show_arg0, - (prog_void *)&cmd_ballboard_show_arg1, + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, NULL, }, }; @@ -898,9 +890,6 @@ struct cmd_ballboard_setmode1_result { /* function called when cmd_ballboard_setmode1 is parsed successfully */ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) @@ -913,7 +902,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* other commands */ -#endif } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; @@ -927,8 +915,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode1_arg0, - (prog_void *)&cmd_ballboard_setmode1_arg1, + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, NULL, }, }; @@ -946,9 +934,6 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode2_result *res = parsed_result; uint8_t mode = I2C_BALLBOARD_MODE_INIT; @@ -965,7 +950,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } i2c_ballboard_set_mode(mode); -#endif } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; @@ -981,9 +965,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode2_arg0, - (prog_void *)&cmd_ballboard_setmode2_arg1, - (prog_void *)&cmd_ballboard_setmode2_arg2, + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, NULL, }, }; @@ -1001,13 +985,9 @@ struct cmd_ballboard_setmode3_result { /* function called when cmd_ballboard_setmode3 is parsed successfully */ static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_ballboard_setmode3_arg0[] = "ballboard"; @@ -1022,9 +1002,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode3_arg0, - (prog_void *)&cmd_ballboard_setmode3_arg1, - (prog_void *)&cmd_ballboard_setmode3_arg2, + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, NULL, }, }; @@ -1063,8 +1043,8 @@ parse_pgm_inst_t cmd_servo_balls = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_balls, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_balls_arg0, - (prog_void *)&cmd_servo_balls_arg1, + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, NULL, }, }; @@ -1220,8 +1200,16 @@ uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, static void cmd_clitoid_parsed(void *parsed_result, void *data) { struct cmd_clitoid_result *res = parsed_result; - clitoid(res->alpha_deg, res->beta_deg, res->R_mm, - res->Vd, res->Amax, res->d_inter_mm); +/* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */ +/* res->Vd, res->Amax, res->d_inter_mm); */ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + strat_set_speed(res->Vd, SPEED_ANGLE_FAST); + trajectory_clitoid(&mainboard.traj, x, y, a, 150., + res->alpha_deg, res->beta_deg, res->R_mm, + res->d_inter_mm); } prog_char str_clitoid_arg0[] = "clitoid"; @@ -1264,127 +1252,6 @@ parse_pgm_inst_t cmd_clitoid = { }, }; -////////////////////// - -// 500 -- 5 -// 400 -- 3 -#define TEST_SPEED 400 -#define TEST_ACC 3 - -static void line2line(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist) -{ - uint8_t err; - double speed_d, speed_a; - double distance, angle; - double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); - double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = line2_angle - line1_angle; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), - distance, angle); - - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("line\r\n")); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - -static void halfturn(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist, double dir) -{ - uint8_t err; - double speed_d, speed_a; - double distance, angle; - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = dir * M_PI/2.; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - - /* take some margin on dist to avoid deceleration */ - DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", - distance, angle); - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - DEBUG(E_USER_STRAT, "miniline"); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < - dist, TRAJ_FLAGS_NO_NEAR); - DEBUG(E_USER_STRAT, "circle2"); - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - DEBUG(E_USER_STRAT, "line"); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - /**********************************************************/ /* Test */ @@ -1398,96 +1265,6 @@ struct cmd_test_result { /* 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; -#ifdef HOST_VERSION - strat_reset_pos(400, 400, 90); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - while (!cmdline_keypressed()) { - /****** PASS1 */ - -#define DIST_HARD_TURN 260 -#define RADIUS_HARD_TURN 100 -#define DIST_EASY_TURN 190 -#define RADIUS_EASY_TURN 190 -#define DIST_HALF_TURN 225 -#define RADIUS_HALF_TURN 130 - - /* hard turn */ - line2line(375, 597, 375, 1847, - 375, 1847, 1050, 1472, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy left and easy right !*/ - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* half turns */ - halfturn(2175, 1347, 2175, 722, - 2625, 722, 2625, 1597, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - halfturn(2625, 847, 2625, 1722, - 2175, 1722, 2175, 1097, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - - /* easy turns */ - line2line(2175, 1597, 2175, 1097, - 2175, 1097, 1500, 722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(2175, 1097, 1500, 722, - 1500, 722, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 722, 1050, 972, - 1050, 972, 375, 597, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1097, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /****** PASS2 */ - - /* easy turn */ - line2line(375, 597, 375, 1097, - 375, 1097, 1050, 1472, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1097, 1050, 1472, - 1050, 1472, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 1472, 375, 1847, - 375, 1847, 375, 1347, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy turn */ - line2line(375, 1847, 375, 1347, - 375, 1347, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1347, 1050, 972, - 1050, 972, 375, 597, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - } trajectory_hardstop(&mainboard.traj); } diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 7bf6057..8c59996 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -69,14 +69,14 @@ struct cmd_traj_speed_result { fixed_string_t arg0; fixed_string_t arg1; - uint16_t s; + float s; }; /* function called when cmd_traj_speed is parsed successfully */ static void cmd_traj_speed_parsed(void *parsed_result, void *data) { struct cmd_traj_speed_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("angle"))) { trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); } @@ -85,7 +85,7 @@ static void cmd_traj_speed_parsed(void *parsed_result, void *data) } /* else it is a "show" */ - printf_P(PSTR("angle %u, distance %u\r\n"), + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), mainboard.traj.a_speed, mainboard.traj.d_speed); } @@ -94,7 +94,7 @@ prog_char str_traj_speed_arg0[] = "traj_speed"; parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); prog_char str_traj_speed_arg1[] = "angle#distance"; parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); -parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT); prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; parse_pgm_inst_t cmd_traj_speed = { @@ -102,9 +102,9 @@ parse_pgm_inst_t cmd_traj_speed = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, - (prog_void *)&cmd_traj_speed_arg1, - (prog_void *)&cmd_traj_speed_s, + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, NULL, }, }; @@ -120,12 +120,76 @@ parse_pgm_inst_t cmd_traj_speed_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg0, (prog_void *)&cmd_traj_speed_show_arg, NULL, }, }; +/**********************************************************/ +/* Traj_Accs for trajectory_manager */ + +/* this structure is filled when cmd_traj_acc is parsed successfully */ +struct cmd_traj_acc_result { + fixed_string_t arg0; + fixed_string_t arg1; + float s; +}; + +/* function called when cmd_traj_acc is parsed successfully */ +static void cmd_traj_acc_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_acc_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), + mainboard.traj.a_acc, + mainboard.traj.d_acc); +} + +prog_char str_traj_acc_arg0[] = "traj_acc"; +parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0); +prog_char str_traj_acc_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1); +parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT); + +prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_arg1, + (prog_void *)&cmd_traj_acc_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_acc_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg); + +prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc_show = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_show_arg, + NULL, + }, +}; + /**********************************************************/ /* circle coef configuration */ @@ -179,7 +243,7 @@ parse_pgm_inst_t cmd_circle_coef_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_circle_coef_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg0, (prog_void *)&cmd_circle_coef_show_arg, NULL, }, @@ -202,7 +266,7 @@ struct cmd_trajectory_result { static void cmd_trajectory_parsed(void * parsed_result, void * data) { struct cmd_trajectory_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("set"))) { trajectory_set_windows(&mainboard.traj, res->d_win, res->a_win, res->a_start); @@ -226,11 +290,11 @@ parse_pgm_inst_t cmd_trajectory = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, - (prog_void *)&cmd_trajectory_arg1, - (prog_void *)&cmd_trajectory_d, - (prog_void *)&cmd_trajectory_a, - (prog_void *)&cmd_trajectory_as, + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, NULL, }, }; @@ -246,7 +310,7 @@ parse_pgm_inst_t cmd_trajectory_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg0, (prog_void *)&cmd_trajectory_show_arg, NULL, }, @@ -272,9 +336,9 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data) struct cmd_rs_gains_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("set"))) { - rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, LEFT_ENCODER, res->left); // en augmentant on tourne à gauche - rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + 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 %2.2f %2.2f\r\n"), @@ -295,10 +359,10 @@ parse_pgm_inst_t cmd_rs_gains = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, - (prog_void *)&cmd_rs_gains_arg1, - (prog_void *)&cmd_rs_gains_l, - (prog_void *)&cmd_rs_gains_r, + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, NULL, }, }; @@ -314,7 +378,7 @@ parse_pgm_inst_t cmd_rs_gains_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg0, (prog_void *)&cmd_rs_gains_show_arg, NULL, }, @@ -353,9 +417,9 @@ parse_pgm_inst_t cmd_track = { .data = NULL, /* 2nd arg of func */ .help_str = help_track, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, - (prog_void *)&cmd_track_arg1, - (prog_void *)&cmd_track_val, + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, NULL, }, }; @@ -371,7 +435,7 @@ parse_pgm_inst_t cmd_track_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_track_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg0, (prog_void *)&cmd_track_show_arg, NULL, }, @@ -400,7 +464,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) { struct cmd_pt_list_result * res = parsed_result; uint8_t i, why=0; - + if (!strcmp_P(res->arg1, PSTR("append"))) { res->arg2 = pt_list_len; } @@ -419,7 +483,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("List is too large\r\n")); return; } - memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], PT_LIST_SIZE-1-res->arg2); pt_list[res->arg2].x = res->arg3; pt_list[res->arg2].y = res->arg4; @@ -434,14 +498,14 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("Index too large\r\n")); return; } - memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); pt_list_len--; } else if (!strcmp_P(res->arg1, PSTR("reset"))) { pt_list_len = 0; } - + /* else it is a "show" or a "start" */ if (pt_list_len == 0) { printf_P(PSTR("List empty\r\n")); @@ -493,11 +557,11 @@ parse_pgm_inst_t cmd_pt_list = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1, - (prog_void *)&cmd_pt_list_arg2, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -513,10 +577,10 @@ parse_pgm_inst_t cmd_pt_list_append = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_append, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1_append, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -532,8 +596,8 @@ parse_pgm_inst_t cmd_pt_list_del = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_del, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, (prog_void *)&cmd_pt_list_arg2, NULL, }, @@ -549,7 +613,7 @@ parse_pgm_inst_t cmd_pt_list_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg0, (prog_void *)&cmd_pt_list_show_arg, NULL, }, @@ -654,9 +718,9 @@ parse_pgm_inst_t cmd_goto1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_a, - (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, NULL, }, }; @@ -672,10 +736,10 @@ parse_pgm_inst_t cmd_goto2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, (prog_void *)&cmd_goto_arg2, - (prog_void *)&cmd_goto_arg3, + (prog_void *)&cmd_goto_arg3, NULL, }, }; @@ -780,7 +844,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) } /* else it's just a "show" */ - printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), position_get_x_double(&mainboard.pos), position_get_y_double(&mainboard.pos), DEG(position_get_a_rad_double(&mainboard.pos))); @@ -797,8 +861,8 @@ parse_pgm_inst_t cmd_position = { .data = NULL, /* 2nd arg of func */ .help_str = help_position, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, NULL, }, }; @@ -816,11 +880,11 @@ parse_pgm_inst_t cmd_position_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_position_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1_set, - (prog_void *)&cmd_position_arg2, - (prog_void *)&cmd_position_arg3, - (prog_void *)&cmd_position_arg4, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, NULL, }, }; @@ -858,8 +922,8 @@ parse_pgm_inst_t cmd_strat_infos = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_infos, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_infos_arg0, - (prog_void *)&cmd_strat_infos_arg1, + (prog_void *)&cmd_strat_infos_arg0, + (prog_void *)&cmd_strat_infos_arg1, NULL, }, }; @@ -893,8 +957,8 @@ parse_pgm_inst_t cmd_strat_conf = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf_arg0, - (prog_void *)&cmd_strat_conf_arg1, + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, NULL, }, }; @@ -919,7 +983,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) on = 1; else on = 0; - + #if 0 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) bit = STRAT_CONF_ONLY_ONE_ON_DISC; @@ -962,9 +1026,9 @@ parse_pgm_inst_t cmd_strat_conf2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf2_arg0, - (prog_void *)&cmd_strat_conf2_arg1, - (prog_void *)&cmd_strat_conf2_arg2, + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, NULL, }, }; @@ -1028,9 +1092,9 @@ parse_pgm_inst_t cmd_strat_conf3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf3_arg0, - (prog_void *)&cmd_strat_conf3_arg1, - (prog_void *)&cmd_strat_conf3_arg2, + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, NULL, }, }; @@ -1039,6 +1103,224 @@ parse_pgm_inst_t cmd_strat_conf3 = { /**********************************************************/ /* Subtraj */ +////////////////////// + +// 500 -- 5 +// 400 -- 3 +#define TEST_SPEED 400 +#define TEST_ACC 3 + +static void line2line(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist) +{ + uint8_t err; + double speed_d, speed_a; + double distance, angle; + double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); + double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); + + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = line2_angle - line1_angle; + distance = angle * radius; + if (distance < 0) + distance = -distance; + angle = simple_modulo_2pi(angle); + angle = DEG(angle); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), + distance, angle); + + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + printf_P(PSTR("line\r\n")); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + +static void halfturn(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist, double dir) +{ + uint8_t err; + double speed_d, speed_a; + double distance, angle; + + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + + strat_set_speed(TEST_SPEED, TEST_SPEED); + quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); + + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = dir * M_PI/2.; + distance = angle * radius; + if (distance < 0) + distance = -distance; + angle = simple_modulo_2pi(angle); + angle = DEG(angle); + + /* take some margin on dist to avoid deceleration */ + DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", + distance, angle); + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + /* circle exit condition */ + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + DEBUG(E_USER_STRAT, "miniline"); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < + dist, TRAJ_FLAGS_NO_NEAR); + DEBUG(E_USER_STRAT, "circle2"); + /* take some margin on dist to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); + + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + DEBUG(E_USER_STRAT, "line"); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + + +/* function called when cmd_test is parsed successfully */ +static void subtraj_test(void) +{ +#ifdef HOST_VERSION + strat_reset_pos(400, 400, 90); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + printf_P(PSTR("%s()\r\n"), __FUNCTION__); + while (!cmdline_keypressed()) { + /****** PASS1 */ + +#define DIST_HARD_TURN 260 +#define RADIUS_HARD_TURN 100 +#define DIST_EASY_TURN 190 +#define RADIUS_EASY_TURN 190 +#define DIST_HALF_TURN 225 +#define RADIUS_HALF_TURN 130 + + /* hard turn */ + line2line(375, 597, 375, 1847, + 375, 1847, 1050, 1472, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy left and easy right !*/ + line2line(825, 1596, 1050, 1472, + 1050, 1472, 1500, 1722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1050, 1472, 1500, 1722, + 1500, 1722, 2175, 1347, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 1722, 2175, 1347, + 2175, 1347, 2175, 847, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* half turns */ + halfturn(2175, 1347, 2175, 722, + 2625, 722, 2625, 1597, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + halfturn(2625, 847, 2625, 1722, + 2175, 1722, 2175, 1097, + RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); + + /* easy turns */ + line2line(2175, 1597, 2175, 1097, + 2175, 1097, 1500, 722, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(2175, 1097, 1500, 722, + 1500, 722, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + line2line(1500, 722, 1050, 972, + 1050, 972, 375, 597, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1097, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /****** PASS2 */ + + /* easy turn */ + line2line(375, 597, 375, 1097, + 375, 1097, 1050, 1472, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1097, 1050, 1472, + 1050, 1472, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 1472, 375, 1847, + 375, 1847, 375, 1347, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* easy turn */ + line2line(375, 1847, 375, 1347, + 375, 1347, 1050, 972, + RADIUS_EASY_TURN, DIST_EASY_TURN); + + /* hard turn */ + line2line(375, 1347, 1050, 972, + 1050, 972, 375, 597, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + /* hard turn */ + line2line(1050, 972, 375, 597, + 375, 597, 375, 1847, + RADIUS_HARD_TURN, DIST_HARD_TURN); + + } + trajectory_hardstop(&mainboard.traj); +} + + /* this structure is filled when cmd_subtraj is parsed successfully */ struct cmd_subtraj_result { fixed_string_t arg0; @@ -1052,9 +1334,11 @@ struct cmd_subtraj_result { /* function called when cmd_subtraj is parsed successfully */ static void cmd_subtraj_parsed(void *parsed_result, void *data) { -/* struct cmd_subtraj_result *res = parsed_result; */ + struct cmd_subtraj_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("test"))) + subtraj_test(); - printf_P(PSTR("TODO\r\n")); trajectory_hardstop(&mainboard.traj); } @@ -1073,12 +1357,13 @@ parse_pgm_inst_t cmd_subtraj = { .data = NULL, /* 2nd arg of func */ .help_str = help_subtraj, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_subtraj_arg0, - (prog_void *)&cmd_subtraj_arg1, - (prog_void *)&cmd_subtraj_arg2, - (prog_void *)&cmd_subtraj_arg3, - (prog_void *)&cmd_subtraj_arg4, - (prog_void *)&cmd_subtraj_arg5, + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, NULL, }, }; + diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 905d533..b8d6e08 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -126,12 +126,19 @@ static void do_cs(void *dummy) * compensation) */ position_manage(&mainboard.pos); } - if (mainboard.flags & DO_BD) { + if ((mainboard.flags & DO_BD) && (mainboard.flags & DO_POWER)) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); #ifndef HOST_VERSION 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_ERRBLOCKING) { + if (bd_get(&mainboard.left_cobroller.bd) || + bd_get(&mainboard.left_cobroller.bd)) { + printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); + mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); + } + } #endif } #ifndef HOST_VERSION @@ -226,7 +233,7 @@ void microb_cs_init(void) &mainboard.angle.cs); trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos); trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */ - trajectory_set_speed(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ + trajectory_set_acc(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */ /* distance window, angle window, angle start */ trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index ad8cff2..64fa521 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -1,6 +1,9 @@ -import math, sys, time, os, random +import math, sys, time, os, random, re from visual import * +FLOAT = "([-+]?[0-9]*\.?[0-9]+)" +INT = "([-+]?[0-9][0-9]*)" + AREA_X = 3000. AREA_Y = 2100. @@ -16,11 +19,11 @@ scene.autoscale=0 # all positions of robot every 5ms save_pos = [] -robot = box(pos = (0, 0, ROBOT_HEIGHT/2), - size = (250,320,ROBOT_HEIGHT), - color = (0.3, 0.3, 0.3) ) +robot = box(color=(0.4, 0.4, 0.4)) +lspickle = box(color=(0.4, 0.4, 0.4)) +rspickle = box(color=(0.4, 0.4, 0.4)) -last_pos = robot.pos.x, robot.pos.y, robot.pos.z +last_pos = (0.,0.,0.) hcenter_line = curve() hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)] vcenter_line = curve() @@ -50,6 +53,11 @@ def square(sz): sq1 = square(250) sq2 = square(500) +robot_x = 0. +robot_y = 0. +robot_a = 0. +robot_lspickle = 0 +robot_rspickle = 0 robot_trail = curve() robot_trail_list = [] max_trail = 500 @@ -181,6 +189,21 @@ def init_waypoints(): def toggle_obj_disp(): global area_objects + """ + if area_objects == []: + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5)) + area_objects.append(c) + c = sphere(radius=5, color=(0., 0.,1.), + pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5)) + area_objects.append(c) +""" if area_objects == []: i = 0 j = 0 @@ -223,18 +246,32 @@ def toggle_obj_disp(): o.visible = 1 -def set_robot(x, y, a): +def set_robot(): global robot, last_pos, robot_trail, robot_trail_list - global save_pos + global save_pos, robot_x, robot_y, robot_a - robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2) - robot.axis = (math.cos(a*math.pi/180), - math.sin(a*math.pi/180), - 0) + axis = (math.cos(robot_a*math.pi/180), + math.sin(robot_a*math.pi/180), + 0) + + robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2) + robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) + lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*70) * math.cos((robot_a-90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_a-90)*math.pi/180), + ROBOT_HEIGHT/2) + lspickle.axis = axis + lspickle.size = (20, 320, 5) + + rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*70) * math.cos((robot_a+90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_a+90)*math.pi/180), + ROBOT_HEIGHT/2) + rspickle.axis = axis + rspickle.size = (20, 320, 5) + # save position - save_pos.append((robot.pos.x, robot.pos, a)) + save_pos.append((robot.pos.x, robot.pos, robot_a)) pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: @@ -272,11 +309,30 @@ while True: fw = open("/tmp/.robot_dis2sim", "w", 0) while True: l = fr.readline() - try: - x,y,a = map(lambda x:int(x), l[:-1].split(" ")) - set_robot(x,y,a) - except ValueError: - pass + m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l) + if m: + robot_x = int(m.groups()[0]) + robot_y = int(m.groups()[1]) + robot_a = int(m.groups()[2]) + set_robot() + m = re.match("ballboard=%s"%(INT), l) + if m: + print int(m.groups()[0]) + m = re.match("cobboard=%s"%(INT), l) + if m: + mode = int(m.groups()[0]) + if (mode & 0x01) == 0: + robot_lspickle = 0 + elif (mode & 0x02) == 0: + robot_lspickle = 1 + else: + robot_lspickle = 2 + if (mode & 0x04) == 0: + robot_rspickle = 0 + elif (mode & 0x08) == 0: + robot_rspickle = 1 + else: + robot_rspickle = 2 if scene.kb.keys == 0: continue diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 4e5c174..a4aa028 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -53,6 +53,9 @@ #include "main.h" #include "sensor.h" #include "i2c_protocol.h" +#ifdef HOST_VERSION +#include "robotsim.h" +#endif #define I2C_STATE_MAX 4 @@ -321,7 +324,7 @@ static int8_t i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) { #ifdef HOST_VERSION - return 0; + return robotsim_i2c(addr, buf, size); #else uint8_t flags; microseconds us = time_get_us2(); @@ -458,6 +461,6 @@ int8_t i2c_ballboard_set_mode(uint8_t mode) struct i2c_cmd_ballboard_set_mode buf; buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE; buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 88d4f97..98d3749 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -133,9 +133,21 @@ void do_time_monitor(void *dummy) void do_led_blink(void *dummy) { -#if 1 /* simple blink */ - LED1_TOGGLE(); -#endif + static uint8_t a = 0; + + if (mainboard.flags & DO_ERRBLOCKING) { + if (a & 1) + LED1_ON(); + else + LED1_OFF(); + } + else { + if (a & 4) + LED1_ON(); + else + LED1_OFF(); + } + a++; } static void main_timer_interrupt(void) @@ -171,7 +183,7 @@ int main(void) memset(&gen, 0, sizeof(gen)); memset(&mainboard, 0, sizeof(mainboard)); mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | - DO_POS | DO_POWER | DO_BD; + DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING; /* UART */ uart_init(); @@ -262,13 +274,13 @@ int main(void) /* all cs management */ microb_cs_init(); + /* TIME */ + time_init(TIME_PRIO); + #ifndef HOST_VERSION /* sensors, will also init hardware adc */ sensor_init(); - /* TIME */ - time_init(TIME_PRIO); - /* start i2c slave polling */ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); @@ -281,7 +293,7 @@ int main(void) /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, - 25000L / SCHEDULER_UNIT, + 10000L / SCHEDULER_UNIT, STRAT_PRIO); #ifndef HOST_VERSION diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 48655eb..6e3f8ad 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -168,6 +168,7 @@ struct mainboard { #define DO_BD 16 #define DO_TIMER 32 #define DO_POWER 64 +#define DO_ERRBLOCKING 128 uint8_t flags; /* misc flags */ /* control systems */ diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index d0aa3ae..5fc2e37 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -48,6 +48,7 @@ #include #include +#include "../common/i2c_commands.h" #include "strat.h" #include "strat_utils.h" #include "main.h" @@ -67,7 +68,7 @@ void robotsim_dump(void) char buf[BUFSIZ]; int len; - len = snprintf(buf, sizeof(buf), "%d %d %d\n", + len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n", position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); @@ -76,6 +77,81 @@ void robotsim_dump(void) hostsim_unlock(); } +static int8_t +robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) +{ + char buf[BUFSIZ]; + int len; + + ballboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +{ + char buf[BUFSIZ]; + int len; + + cobboard.mode = cmd->mode; + len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); + return 0; +} + +static int8_t +robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + case I2C_CMD_BALLBOARD_SET_MODE: + { + struct i2c_cmd_ballboard_set_mode *cmd = void_cmd; + robotsim_i2c_ballboard_set_mode(cmd); + break; + } + + default: + break; + } + return 0; +} + +static int8_t +robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size) +{ + void *void_cmd = buf; + + switch (buf[0]) { + case I2C_CMD_COBBOARD_SET_MODE: + { + struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; + robotsim_i2c_cobboard_set_mode(cmd); + break; + } + default: + break; + } + return 0; +} + +int8_t +robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size) +{ + if (addr == I2C_BALLBOARD_ADDR) + return robotsim_i2c_ballboard(addr, buf, size); + else if (addr == I2C_COBBOARD_ADDR) + return robotsim_i2c_cobboard(addr, buf, size); + return 0; +} + /* must be called periodically */ void robotsim_update(void) { diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h index 8777de8..a9316b9 100644 --- a/projects/microb2010/mainboard/robotsim.h +++ b/projects/microb2010/mainboard/robotsim.h @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * 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 @@ -19,6 +19,7 @@ * */ +int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size); void robotsim_update(void); void robotsim_pwm(void *arg, int32_t val); int32_t robotsim_encoder_get(void *arg); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 6743d37..97d159c 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -70,7 +70,10 @@ struct strat_infos strat_infos = { .conf = { .flags = 0, }, - + /* status */ + .status = { + .flags = 0, + }, }; /*************************************************************/ @@ -150,7 +153,7 @@ void strat_init(void) interrupt_traj_reset(); /* used in strat_base for END_TIMER */ - mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | + mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_TIMER | DO_POWER; } @@ -174,6 +177,17 @@ void strat_exit(void) /* called periodically */ void strat_event(void *dummy) { +#if 0 + /* pack or deploy spickle */ + if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) { + if (sensor_get(S_LCOB_PRESENT)) { + if (sensor_get(S_LCOB_WHITE)) + i2c_ballboard_set_mode(); + else + ; + } + } +#endif /* limit speed when opponent is close */ strat_limit_speed(); } @@ -197,7 +211,7 @@ uint8_t strat_main(void) { uint8_t err; - /* do static cols + first temple */ + /* */ err = strat_beginning(); return err; diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 53d9192..d852396 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -83,23 +83,30 @@ /* strat infos structures */ -struct bbox { +struct strat_bbox { int32_t x1; int32_t y1; int32_t x2; int32_t y2; }; -struct conf { +struct strat_conf { #define STRAT_CONF_XXX 0x01 uint8_t flags; }; +struct strat_status { +#define STRAT_STATUS_LHARVEST 0x01 +#define STRAT_STATUS_RHARVEST 0x02 + uint8_t flags; +}; + /* all infos related to strat */ struct strat_infos { uint8_t dump_enabled; - struct conf conf; - struct bbox area_bbox; + struct strat_conf conf; + struct strat_bbox area_bbox; + struct strat_status status; }; extern struct strat_infos strat_infos; -- 2.20.1 From aed049ab6be4e1916457743d53f6b610a21b4854 Mon Sep 17 00:00:00 2001 From: zer0 Date: Wed, 14 Apr 2010 00:04:10 +0200 Subject: [PATCH 09/16] clothoid --- .../trajectory_manager_core.c | 25 +- .../microb2010/mainboard/commands_mainboard.c | 276 +++++++++--------- projects/microb2010/mainboard/display.py | 66 +++-- 3 files changed, 197 insertions(+), 170 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 5197300..c6c70fc 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -882,8 +882,8 @@ static int8_t calc_clitoid(struct trajectory *traj, alpha_rad = RAD(alpha_deg); beta_rad = RAD(beta_deg); t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); - DEBUG(E_TRAJECTORY, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", - R_mm, alpha_rad, beta_rad, t); + DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", + R_mm, a_rad, alpha_rad, beta_rad, t); /* process the angular acceleration */ Aa_rd_s2 = Va_rd_s / t; @@ -966,8 +966,14 @@ static int8_t calc_clitoid(struct trajectory *traj, DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y); /* M is the same point than xm, ym but in absolute coords */ - M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm; - M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm; + if (alpha_rad < 0) { + M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm; + } + else { + M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm; + } DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y); /* project M on line 1 */ @@ -1020,7 +1026,7 @@ static void start_clitoid(struct trajectory *traj) * the function assumes that the initial linear speed is Vd and * angular speed is 0. * - * - x,y,a: starting position + * - x,y,a_deg: starting position * - advance: parameter for line following * - alpha: total angle * - beta: circular part of angle (lower than alpha) @@ -1034,22 +1040,23 @@ static void start_clitoid(struct trajectory *traj) * background. */ int8_t trajectory_clitoid(struct trajectory *traj, - double x, double y, double a, double advance, + double x, double y, double a_deg, double advance, double alpha_deg, double beta_deg, double R_mm, double d_inter_mm) { double remain = 0, Aa = 0, Va = 0, Vd; double turnx, turny; + double a_rad = RAD(a_deg); Vd = traj->d_speed; - if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm, + if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm, Vd, traj->a_acc, d_inter_mm, &Aa, &Va, &remain) < 0) return -1; delete_event(traj); - turnx = x + cos(a) * remain; - turny = y + sin(a) * remain; + turnx = x + cos(a_rad) * remain; + turny = y + sin(a_rad) * remain; traj->target.line.Aa = Aa; traj->target.line.Va = Va; traj->target.line.alpha = RAD(alpha_deg); diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index a69208b..8a79c35 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1063,139 +1063,6 @@ struct cmd_clitoid_result { float d_inter_mm; }; -/** - * do a superb curve joining line1 to line2 which is composed of: - * - a clothoid starting from line1 - * - a circle - * - another clothoid up to line2 - * - * the function assumes that the initial linear speed is Vd and - * angular speed is 0. - * - * - alpha: total angle - * - beta: circular part of angle (lower than alpha) - * - R: the radius of the circle (must be != 0) - * - Vd: linear speed to use (in imp per cs period) - * - Amax: maximum angular acceleration - * - d_inter: distance in mm until the intersection of the - * 2 lines - * - * return 0 on success: in this case these parameters are filled: - * - Aa_out: the angular acceleration to configure in quadramp - * - remain_d_mm_out: remaining distance before start to turn - */ -uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, - double Vd, double Amax, double d_inter_mm) -{ - double Vd_mm_s; - double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; - double remain_d_mm; - double Aa, Aa_rd_s2; - line_t line1, line2; - double x, y, a_rad; - point_t robot, intersect, pt2, center, proj; - vect_t v; - - /* param check */ - if (fabs(alpha_deg) <= fabs(beta_deg)) { - DEBUG(E_USER_STRAT, "alpha is smaller than beta"); - return END_ERROR; - } - - /* get angular speed Va */ - Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM); - DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s); - Va_rd_s = Vd_mm_s / R_mm; - Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ)); - DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va); - - /* process 't', the time in seconds that we will take to do - * the first clothoid */ - alpha_rad = RAD(alpha_deg); - beta_rad = RAD(beta_deg); - t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); - DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", - R_mm, alpha_rad, beta_rad, t); - - /* process the angular acceleration */ - Aa_rd_s2 = Va_rd_s / t; - Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM / - (2 * CS_HZ * CS_HZ)); - DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa); - - /* exit if the robot cannot physically do it */ - if (Aa > Amax) { - DEBUG(E_USER_STRAT, "greater than max acceleration"); - return END_ERROR; - } - - /* the robot position */ - x = position_get_x_double(&mainboard.pos); - y = position_get_y_double(&mainboard.pos); - a_rad = position_get_a_rad_double(&mainboard.pos); - - /* define line1 and line2 */ - robot.x = x; - robot.y = y; - intersect.x = x + cos(a_rad) * d_inter_mm; - intersect.y = y + sin(a_rad) * d_inter_mm; - pts2line(&robot, &intersect, &line1); - pt2.x = intersect.x + cos(a_rad + alpha_rad); - pt2.y = intersect.y + sin(a_rad + alpha_rad); - pts2line(&intersect, &pt2, &line2); - DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)", - intersect.x, intersect.y); - - /* the center of the circle is at (d_mm, d_mm) when we have to - * start the clothoid */ - d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * - sqrt(M_PI) / 2.; - DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm); - - /* translate line1 */ - v.x = intersect.x - robot.x; - v.y = intersect.y - robot.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line1, &v); - - /* translate line2 */ - v.x = intersect.x - pt2.x; - v.y = intersect.y - pt2.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line2, &v); - - /* find the center of the circle, at the intersection of the - * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { - DEBUG(E_USER_STRAT, "cannot find circle center"); - return END_ERROR; - } - DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y); - - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y); - - /* process remaining distance before start turning */ - remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); - DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm); - if (remain_d_mm < 0) { - DEBUG(E_USER_STRAT, "too late, cannot turn"); - return END_ERROR; - } - - return END_TRAJ; -} - /* function called when cmd_test is parsed successfully */ static void cmd_clitoid_parsed(void *parsed_result, void *data) { @@ -1262,9 +1129,152 @@ struct cmd_test_result { int32_t dist; }; +#define LINE_UP 0 +#define LINE_DOWN 1 +#define LINE_R_UP 2 +#define LINE_L_DOWN 3 +#define LINE_L_UP 4 +#define LINE_R_DOWN 5 + +struct line_2pts { + point_t p1; + point_t p2; +}; + +static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +{ + float n = num; + + switch (dir) { + + case LINE_UP: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(0); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(2100); + break; + case LINE_DOWN: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(2100); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(0); + break; + case LINE_R_UP: + l->p1.x = 150; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 2850; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_L_DOWN: + l->p1.x = 2850; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 150; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + case LINE_L_UP: + l->p1.x = 2850; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 150; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_R_DOWN: + l->p1.x = 150; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 2850; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + default: + break; + } +} + +#if 0 +static void reverse_line(struct line_2pts *l) +{ + point_t tmp; + + tmp.x = l->p1.x; + tmp.y = l->p1.y; + l->p1.x = l->p2.x; + l->p1.y = l->p2.y; + l->p2.x = tmp.x; + l->p2.y = tmp.y; +} +#endif + +static void line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2) +{ + double line1_a_rad, line1_a_deg, line2_a_rad; + double diff_a_deg, diff_a_deg_abs, beta_deg; + double radius; + struct line_2pts l1, l2; + line_t ll1, ll2; + point_t p; + + /* convert to 2 points */ + num2line(&l1, dir1, num1); + num2line(&l2, dir2, num2); + + printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); + printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + + /* convert to line eq and find intersection */ + pts2line(&l1.p1, &l1.p2, &ll1); + pts2line(&l2.p1, &l2.p2, &ll2); + intersect_line(&ll1, &ll2, &p); + + line1_a_rad = atan2(l1.p2.y - l1.p1.y, + l1.p2.x - l1.p1.x); + line1_a_deg = DEG(line1_a_rad); + line2_a_rad = atan2(l2.p2.y - l2.p1.y, + l2.p2.x - l2.p1.x); + diff_a_deg = DEG(line2_a_rad - line1_a_rad); + diff_a_deg_abs = fabs(diff_a_deg); + + if (diff_a_deg_abs < 70.) { + radius = 200; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else if (diff_a_deg_abs < 100.) { + radius = 100; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else { + radius = 120; + if (diff_a_deg > 0) + beta_deg = 60; + else + beta_deg = -60; + } + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, + line1_a_deg, 150., diff_a_deg, beta_deg, + radius, xy_norm(l1.p1.x, l1.p1.y, + p.x, p.y)); + wait_traj_end(0xFF); +} + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + strat_reset_pos(298.48, 309.21, 70.02); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + time_wait_ms(100); + + line2line(LINE_UP, 0, LINE_R_DOWN, 2); + line2line(LINE_R_DOWN, 2, LINE_R_UP, 1); + trajectory_hardstop(&mainboard.traj); } diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 64fa521..edcf494 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -258,14 +258,14 @@ def set_robot(): robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*70) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_a-90)*math.pi/180), + lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis lspickle.size = (20, 320, 5) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*70) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_a+90)*math.pi/180), + rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis rspickle.size = (20, 320, 5) @@ -308,31 +308,41 @@ while True: fr = open("/tmp/.robot_sim2dis", "r") fw = open("/tmp/.robot_dis2sim", "w", 0) while True: + m = None l = fr.readline() - m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l) - if m: - robot_x = int(m.groups()[0]) - robot_y = int(m.groups()[1]) - robot_a = int(m.groups()[2]) - set_robot() - m = re.match("ballboard=%s"%(INT), l) - if m: - print int(m.groups()[0]) - m = re.match("cobboard=%s"%(INT), l) - if m: - mode = int(m.groups()[0]) - if (mode & 0x01) == 0: - robot_lspickle = 0 - elif (mode & 0x02) == 0: - robot_lspickle = 1 - else: - robot_lspickle = 2 - if (mode & 0x04) == 0: - robot_rspickle = 0 - elif (mode & 0x08) == 0: - robot_rspickle = 1 - else: - robot_rspickle = 2 + + # parse position + if not m: + m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l) + if m: + robot_x = int(m.groups()[0]) + robot_y = int(m.groups()[1]) + robot_a = int(m.groups()[2]) + set_robot() + + # parse ballboard + if not m: + m = re.match("ballboard=%s"%(INT), l) + if m: + print int(m.groups()[0]) + + # parse cobboard + if not m: + m = re.match("cobboard=%s"%(INT), l) + if m: + mode = int(m.groups()[0]) + if (mode & 0x01) == 0: + robot_lspickle = 0 + elif (mode & 0x02) == 0: + robot_lspickle = 1 + else: + robot_lspickle = 2 + if (mode & 0x04) == 0: + robot_rspickle = 0 + elif (mode & 0x08) == 0: + robot_rspickle = 1 + else: + robot_rspickle = 2 if scene.kb.keys == 0: continue -- 2.20.1 From b022f257a5ee568737e1a684a83d0154397fffcb Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 16 Apr 2010 00:15:22 +0200 Subject: [PATCH 10/16] 20100416 --- projects/microb2010/ballboard/main.h | 12 +- projects/microb2010/cobboard/actuator.c | 4 +- projects/microb2010/cobboard/cs.c | 12 +- projects/microb2010/cobboard/i2c_protocol.c | 18 +- projects/microb2010/cobboard/main.c | 7 +- projects/microb2010/cobboard/main.h | 12 +- projects/microb2010/cobboard/sensor.c | 71 ++- projects/microb2010/cobboard/sensor.h | 2 + projects/microb2010/cobboard/spickle.c | 20 +- projects/microb2010/cobboard/state.c | 26 +- projects/microb2010/common/i2c_commands.h | 5 +- projects/microb2010/mainboard/Makefile | 2 +- .../microb2010/mainboard/commands_mainboard.c | 79 ++- projects/microb2010/mainboard/display.py | 11 +- projects/microb2010/mainboard/i2c_protocol.c | 45 +- projects/microb2010/mainboard/main.h | 12 +- projects/microb2010/mainboard/robotsim.c | 15 +- projects/microb2010/mainboard/robotsim.h | 1 + projects/microb2010/mainboard/sensor.c | 14 +- projects/microb2010/mainboard/sensor.h | 4 +- projects/microb2010/mainboard/strat.c | 3 + projects/microb2010/mainboard/strat.h | 33 +- projects/microb2010/mainboard/strat_base.h | 5 +- projects/microb2010/mainboard/strat_corn.c | 548 ++++++++++++++++++ projects/microb2010/mainboard/strat_corn.h | 36 ++ 25 files changed, 853 insertions(+), 144 deletions(-) create mode 100644 projects/microb2010/mainboard/strat_corn.c create mode 100644 projects/microb2010/mainboard/strat_corn.h diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index cde4608..9f10c35 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -30,13 +30,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 1b6e91f..8dfe8bb 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -49,8 +49,8 @@ //#define COBROLLER_SPEED 400 #define SERVO_DOOR_OPEN 260 -#define SERVO_DOOR_CLOSED 510 -#define SERVO_DOOR_BLOCK 510 +#define SERVO_DOOR_CLOSED 500 +#define SERVO_DOOR_BLOCK 500 #define SERVO_CARRY_L_OPEN 295 #define SERVO_CARRY_L_CLOSED 400 // 510 diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index a718970..0ed2f1a 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -102,8 +102,8 @@ static void do_cs(__attribute__((unused)) void *dummy) /* urgent case: stop power on blocking */ if (cobboard.flags & DO_ERRBLOCKING) { - if (bd_get(&cobboard.left_spickle.bd) || - bd_get(&cobboard.right_spickle.bd) || + if (/* bd_get(&cobboard.left_spickle.bd) || */ + /* bd_get(&cobboard.right_spickle.bd) || */ bd_get(&cobboard.shovel.bd)) { printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n")); cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING); @@ -149,8 +149,8 @@ void microb_cs_init(void) /* ---- CS left_spickle */ /* PID */ pid_init(&cobboard.left_spickle.pid); - pid_set_gains(&cobboard.left_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.left_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.left_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.left_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.left_spickle.pid, 10); pid_set_derivate_filter(&cobboard.left_spickle.pid, 4); @@ -169,8 +169,8 @@ void microb_cs_init(void) /* ---- CS right_spickle */ /* PID */ pid_init(&cobboard.right_spickle.pid); - pid_set_gains(&cobboard.right_spickle.pid, 300, 10, 1500); - pid_set_maximums(&cobboard.right_spickle.pid, 0, 10000, 2400); /* max is 12 V */ + pid_set_gains(&cobboard.right_spickle.pid, 400, 10, 1500); + pid_set_maximums(&cobboard.right_spickle.pid, 0, 25000, 4095); pid_set_out_shift(&cobboard.right_spickle.pid, 10); pid_set_derivate_filter(&cobboard.right_spickle.pid, 4); diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 8d86b49..9b2a9e7 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -99,17 +99,11 @@ static void i2c_send_status(void) ans.right_cobroller_speed = cobboard.right_cobroller_speed; ans.cob_count = state_get_cob_count(); -; + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); } -static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) -{ - state_set_mode(cmd->mode); - return 0; -} - void i2c_recvevent(uint8_t * buf, int8_t size) { void *void_cmd = buf; @@ -136,6 +130,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } +#if 0 case I2C_CMD_COBBOARD_SET_MODE: { struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; @@ -144,6 +139,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) i2c_set_mode(cmd); break; } +#endif case I2C_CMD_GENERIC_SET_COLOR: { @@ -164,16 +160,18 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } #endif - + /* Add other commands here ...*/ case I2C_REQ_COBBOARD_STATUS: { - //struct i2c_req_cobboard_status *cmd = void_cmd; + struct i2c_req_cobboard_status *cmd = void_cmd; if (size != sizeof (struct i2c_req_cobboard_status)) goto error; - + + /* mode is in req */ + state_set_mode(cmd->mode); i2c_send_status(); break; } diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 1f4065b..d21e7b7 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -207,8 +207,8 @@ int main(void) PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); - PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | - PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4); + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, @@ -271,6 +271,9 @@ int main(void) gen.log_level = 5; cobboard.flags |= DO_CS; + spickle_pack(I2C_LEFT_SIDE); + spickle_pack(I2C_RIGHT_SIDE); + state_machine(); cmdline_interact(); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 699a5c1..0c12fdc 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -30,13 +30,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index 5bbf43f..c471033 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -38,6 +38,7 @@ #include #include +#include "../common/i2c_commands.h" #include "main.h" #include "sensor.h" @@ -75,7 +76,7 @@ int16_t rii_strong(struct adc_infos *adc, int16_t val) #define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) /* define which ADC to poll, see in sensor.h */ -static struct adc_infos adc_infos[ADC_MAX] = { +static struct adc_infos adc_infos[ADC_MAX] = { [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium }, [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium }, [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium }, @@ -91,7 +92,7 @@ static struct adc_infos adc_infos[ADC_MAX] = { static void adc_event(int16_t result); /* called every 10 ms, see init below */ -static void do_adc(__attribute__((unused)) void *dummy) +static void do_adc(__attribute__((unused)) void *dummy) { /* launch first conversion */ adc_launch(adc_infos[0].config); @@ -146,10 +147,10 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP2] = { 10, 0, 3, 7, 0, 0 }, /* 1 */ [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ - [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ - [S_RCOB] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ + [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used + [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used + [S_RCOB] = { 1, 0, 0, 1, 0, 1 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ @@ -163,7 +164,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { /* value of filtered sensors */ static uint16_t sensor_filtered = 0; -/* sensor mapping : +/* sensor mapping : * 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13) * 4-5: PORTL 0->1 (cap5 -> cap6) * 6-7: PORTE 3->4 (cap7 -> cap8) @@ -218,7 +219,7 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy) if (sensor_filter[i].cpt <= sensor_filter[i].thres_off) sensor_filter[i].prev = 0; } - + if (sensor_filter[i].prev && !sensor_filter[i].invert) { tmp |= (1UL << i); } @@ -231,6 +232,51 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy) IRQ_UNLOCK(flags); } +static uint8_t lcob_cpt = 0, rcob_cpt = 0; +uint8_t cob_falling_edge(uint8_t side) +{ + uint8_t flags; + + if (side == I2C_LEFT_SIDE) { + IRQ_LOCK(flags); + if (lcob_cpt == 0) { + IRQ_UNLOCK(flags); + return 0; + } + lcob_cpt = 0; + IRQ_UNLOCK(flags); + return 1; + } + else { + IRQ_LOCK(flags); + if (rcob_cpt == 0) { + IRQ_UNLOCK(flags); + return 0; + } + rcob_cpt = 0; + IRQ_UNLOCK(flags); + return 1; + } +} + +static void cob_edge_manage(void) +{ + static uint8_t lprev, rprev; + uint8_t l, r; + l = sensor_get(S_LCOB); + r = sensor_get(S_RCOB); + /* falling edge */ + if (lprev != 0 && l == 0) + lcob_cpt = 10; + if (rprev != 0 && r == 0) + rcob_cpt = 10; + if (lcob_cpt > 0) + lcob_cpt --; + if (rcob_cpt > 0) + rcob_cpt --; + lprev = l; + rprev = r; +} /************ global sensor init */ @@ -240,6 +286,7 @@ static void do_sensors(__attribute__((unused)) void *dummy) { do_adc(NULL); do_boolean_sensors(NULL); + cob_edge_manage(); } void sensor_init(void) @@ -247,8 +294,8 @@ void sensor_init(void) adc_init(); adc_register_event(adc_event); /* CS EVENT */ - scheduler_add_periodical_event_priority(do_sensors, NULL, - 10000L / SCHEDULER_UNIT, + scheduler_add_periodical_event_priority(do_sensors, NULL, + 10000L / SCHEDULER_UNIT, ADC_PRIO); } diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 0ba6b0c..c24af73 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -54,3 +54,5 @@ int16_t sensor_get_adc(uint8_t i); /* get filtered values of boolean sensors */ uint16_t sensor_get_all(void); uint8_t sensor_get(uint8_t i); + +uint8_t cob_falling_edge(uint8_t side); diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index e1d9a7b..dcee125 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -60,23 +60,23 @@ struct spickle_params { }; static struct spickle_params spickle = { - .k1 = 500, + .k1 = 1000, .k2 = 20, .csb = { &cobboard.left_spickle, &cobboard.right_spickle, }, .pos_deployed = { - 40000, /* left */ - -40000, /* right */ + 0, /* left */ + 0, /* right */ }, .pos_mid = { - 20000, /* left */ - -20000, /* right */ + 25000, /* left */ + 26000, /* right */ }, .pos_packed = { - 0, /* left */ - 0, /* right */ + 53000, /* left */ + 54500, /* right */ }, }; @@ -84,9 +84,9 @@ static struct spickle_params spickle = { static void spickle_autopos(void) { printf_P(PSTR("spickle autopos...")); - pwm_ng_set(LEFT_SPICKLE_PWM, -500); - pwm_ng_set(RIGHT_SPICKLE_PWM, 500); - wait_ms(1000); + pwm_ng_set(LEFT_SPICKLE_PWM, -700); + pwm_ng_set(RIGHT_SPICKLE_PWM, -700); + wait_ms(2500); pwm_ng_set(LEFT_SPICKLE_PWM, 0); pwm_ng_set(RIGHT_SPICKLE_PWM, 0); encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0); diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index a16675d..389411e 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -86,25 +86,6 @@ static void state_debug_wait_key_pressed(void) while(!cmdline_keypressed()); } -/* return true if cob is present */ -static uint8_t state_cob_present(uint8_t side) -{ - if (side == I2C_LEFT_SIDE) - return sensor_get(S_LCOB); - else - return sensor_get(S_RCOB); -} - -/* return the detected color of the cob (only valid if present) */ -static uint8_t state_cob_color(uint8_t side) -{ - /* XXX no color sensor for now */ - if (side == I2C_LEFT_SIDE) - return I2C_COB_WHITE; - else - return I2C_COB_WHITE; -} - /* return true if the cob is correctly inside */ static uint8_t state_cob_inside(void) { @@ -186,11 +167,7 @@ static void state_do_harvest(uint8_t side) state_status = I2C_COBBOARD_STATUS_RBUSY; /* if there is no cob, return */ - if (state_cob_present(side)) - return; - - /* if it is black, nothing to do */ - if (state_cob_color(side) == I2C_COB_BLACK) + if (cob_falling_edge(side) == 0) return; STMCH_DEBUG("start"); @@ -330,6 +307,7 @@ void state_machine(void) state_do_eject(); } } + state_status = I2C_COBBOARD_STATUS_READY; } void state_init(void) diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index d6794af..4b8aea5 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -68,6 +68,7 @@ struct i2c_cmd_generic_color { #define I2C_CMD_COBBOARD_SET_MODE 0x02 +/* XXX disabled, use memory sync instead */ struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; @@ -77,7 +78,7 @@ struct i2c_cmd_cobboard_set_mode { #define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ #define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ #define I2C_COBBOARD_MODE_INIT 0x20 /* init state machine */ - uint8_t mode; + //uint8_t mode; }; #define I2C_CMD_BALLBOARD_SET_MODE 0x10 @@ -105,7 +106,7 @@ struct i2c_cmd_ballboard_set_mode { struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - int16_t sickle_left1_current; + uint8_t mode; }; #define I2C_ANS_COBBOARD_STATUS 0x81 diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 141943f..33b5c52 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -6,7 +6,7 @@ AVERSIVE_DIR = ../../.. SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c -SRC += strat_utils.c strat_base.c strat.c +SRC += strat_utils.c strat_base.c strat.c strat_corn.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 8a79c35..7c42411 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,7 @@ #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -1202,6 +1203,65 @@ static void reverse_line(struct line_2pts *l) } #endif +/* return 1 if there is a corn near, and fill the index ptr */ +static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +{ +#define SENSOR_CORN_DIST 225 +#define SENSOR_CORN_ANGLE 90 + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a_rad = position_get_a_rad_double(&mainboard.pos); + double x_corn, y_corn; + int16_t x_corn_int, y_corn_int; + + if (side == I2C_LEFT_SIDE) { + x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + else { + x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + x_corn_int = x_corn; + y_corn_int = y_corn; + + *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (*corn_idx < 0) + return 0; + return 1; +} + +/* + * - send the correct commands to the spickles + * - return 1 if we need to stop (cobboard is stucked) +*/ +static uint8_t handle_spickles(void) +{ + int8_t corn_idx; + + if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) + i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + else + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + } +/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ +/* time_wait_ms(100); */ + + if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) + i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + else + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + } + + return 0; +} + static void line2line(uint8_t dir1, uint8_t num1, uint8_t dir2, uint8_t num2) { @@ -1211,6 +1271,7 @@ static void line2line(uint8_t dir1, uint8_t num1, struct line_2pts l1, l2; line_t ll1, ll2; point_t p; + uint8_t err; /* convert to 2 points */ num2line(&l1, dir1, num1); @@ -1255,11 +1316,22 @@ static void line2line(uint8_t dir1, uint8_t num1, else beta_deg = -60; } + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, p.x, p.y)); - wait_traj_end(0xFF); + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return; /* XXX do something */ + } + err = test_traj_end(0xFF); + } + return; } /* function called when cmd_test is parsed successfully */ @@ -1269,11 +1341,14 @@ static void cmd_test_parsed(void *parsed_result, void *data) strat_reset_pos(298.48, 309.21, 70.02); mainboard.angle.on = 1; mainboard.distance.on = 1; + strat_set_speed(250, SPEED_ANGLE_FAST); #endif + init_corn_table(0, 0); time_wait_ms(100); line2line(LINE_UP, 0, LINE_R_DOWN, 2); - line2line(LINE_R_DOWN, 2, LINE_R_UP, 1); + line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + line2line(LINE_R_UP, 2, LINE_UP, 5); trajectory_hardstop(&mainboard.traj); } diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index edcf494..eb63f03 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -258,14 +258,14 @@ def set_robot(): robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180), + lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis lspickle.size = (20, 320, 5) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180), + rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180), + robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis rspickle.size = (20, 320, 5) @@ -297,7 +297,8 @@ def silent_mkfifo(f): except: pass -init_corn_table(random.randint(0,8), random.randint(0,3)) +#init_corn_table(random.randint(0,8), random.randint(0,3)) +init_corn_table(0, 0) waypoints = init_waypoints() toggle_obj_disp() diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index a4aa028..aed92c7 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -272,7 +272,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* status */ - cobboard.mode = ans->mode; + //cobboard.mode = ans->mode; cobboard.status = ans->status; cobboard.left_cobroller_speed = ans->left_cobroller_speed; cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed); @@ -354,6 +354,7 @@ static int8_t i2c_req_cobboard_status(void) int8_t err; buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS; + buf.mode = cobboard.mode; err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); @@ -390,20 +391,38 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_cobboard_mode_eject(void) +static int8_t i2c_cobboard_set_mode(uint8_t mode) { +#ifdef HOST_VERSION + return robotsim_i2c_cobboard_set_mode(mode); +#else + cobboard.mode = mode; + return 0; +#endif + +#if 0 /* old */ struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +#endif +} + +int8_t i2c_cobboard_mode_eject(void) +{ + /* XXXXXXXXX bad bad bad */ + uint8_t mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; + i2c_cobboard_set_mode(mode); + time_wait_ms(500); + mode = cobboard.mode & (~I2C_COBBOARD_MODE_EJECT); + i2c_cobboard_set_mode(mode); + return 0; } int8_t i2c_cobboard_mode_harvest(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) { mode |= I2C_COBBOARD_MODE_L_DEPLOY; mode |= I2C_COBBOARD_MODE_L_HARVEST; @@ -412,16 +431,13 @@ int8_t i2c_cobboard_mode_harvest(uint8_t side) mode |= I2C_COBBOARD_MODE_R_DEPLOY; mode |= I2C_COBBOARD_MODE_R_HARVEST; } - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_deploy(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) { mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); mode |= I2C_COBBOARD_MODE_L_DEPLOY; @@ -430,30 +446,23 @@ int8_t i2c_cobboard_mode_deploy(uint8_t side) mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); mode |= I2C_COBBOARD_MODE_R_DEPLOY; } - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_pack(uint8_t side) { - struct i2c_cmd_cobboard_set_mode buf; uint8_t mode = cobboard.mode; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; if (side == I2C_LEFT_SIDE) mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); else mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - buf.mode = mode; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(mode); } int8_t i2c_cobboard_mode_init(void) { - struct i2c_cmd_cobboard_set_mode buf; - buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = I2C_COBBOARD_MODE_INIT; - return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); + return i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); } int8_t i2c_ballboard_set_mode(uint8_t mode) diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 6e3f8ad..21c0065 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -53,13 +53,13 @@ #define LED1_OFF() cbi(PORTJ, 2) #define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) -#define LED2_ON() sbi(PORTL, 7) -#define LED2_OFF() cbi(PORTL, 7) -#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTJ, 3) -#define LED3_ON() sbi(PORTJ, 3) -#define LED3_OFF() cbi(PORTJ, 3) -#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) +#define LED3_ON() sbi(PORTL, 7) +#define LED3_OFF() cbi(PORTL, 7) +#define LED3_TOGGLE() LED_TOGGLE(PORTL, 7) #define LED4_ON() sbi(PORTL, 6) #define LED4_OFF() cbi(PORTL, 6) diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 5fc2e37..a0fa5a0 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -91,14 +91,17 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd) return 0; } -static int8_t -robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +int8_t +robotsim_i2c_cobboard_set_mode(uint8_t mode) { char buf[BUFSIZ]; int len; - cobboard.mode = cmd->mode; - len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode); + if (cobboard.mode == mode) + return 0; + + cobboard.mode = mode; + len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode); hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); @@ -127,15 +130,17 @@ robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size) static int8_t robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size) { - void *void_cmd = buf; + // void *void_cmd = buf; switch (buf[0]) { +#if 0 /* deleted */ case I2C_CMD_COBBOARD_SET_MODE: { struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; robotsim_i2c_cobboard_set_mode(cmd); break; } +#endif default: break; } diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h index a9316b9..8246500 100644 --- a/projects/microb2010/mainboard/robotsim.h +++ b/projects/microb2010/mainboard/robotsim.h @@ -25,3 +25,4 @@ void robotsim_pwm(void *arg, int32_t val); int32_t robotsim_encoder_get(void *arg); int robotsim_init(void); void robotsim_dump(void); +int8_t robotsim_i2c_cobboard_set_mode(uint8_t mode); diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index a6f85de..473ed76 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -158,14 +158,14 @@ struct sensor_filter { * CAP 1,5,6,7,8 */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { - [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ - [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ - [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ - [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ - [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ - [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 6 */ - [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 4 */ + [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ + [S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ + [S_LCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h index 0df5567..0ab7a3b 100644 --- a/projects/microb2010/mainboard/sensor.h +++ b/projects/microb2010/mainboard/sensor.h @@ -36,8 +36,8 @@ #define S_COLUMN_LEFT 3 #define S_START_SWITCH 4 #define S_DISP_LEFT 5 -#define S_DISP_RIGHT 6 -#define S_CAP8 7 +#define S_RCOB_WHITE 6 +#define S_LCOB_WHITE 7 #define S_RESERVED1 8 #define S_RESERVED2 9 #define S_RESERVED3 10 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 97d159c..1a525e5 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -58,6 +58,7 @@ #include "main.h" #include "strat.h" #include "strat_base.h" +#include "strat_corn.h" #include "strat_utils.h" #include "sensor.h" #include "actuator.h" @@ -138,12 +139,14 @@ void strat_dump_infos(const char *caller) * here */ void strat_reset_infos(void) { + init_corn_table(-1, -1); } /* call it just before launching the strat */ void strat_init(void) { /* XXX init rollers, .. */ + strat_reset_infos(); /* we consider that the color is correctly set */ diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index d852396..0a31073 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -45,20 +45,21 @@ /* * * - * - * - * +------------------------------------+ - * | | - * | | - * | | - * | | - * y | | - * | | - * |------ ------| - * | | | | - * | | | | - * +-----+------------------------+-----+ - * x + * vertical lines + * O 1 2 3 4 5 + * 2100 +-----|-----|-----|-----|-----|-----|-----+ + * | o o o | + * | o o o o | diag + * | o o o | lines + * 0/ o o o o \0 + * y | o o | + * 1/ o o \1 + * | | + * 2/------ ------\2 + * | | | | + * | | | | + * 0 +-----+-----------------------------+-----+ + * 0 x 3000 */ /* useful traj flags */ @@ -70,8 +71,8 @@ #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) /* default acc */ -#define ACC_DIST 5. -#define ACC_ANGLE 5. +#define ACC_DIST 10. +#define ACC_ANGLE 10. /* default speeds */ #define SPEED_DIST_FAST 2500. diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h index 6ff0666..d8b61e5 100644 --- a/projects/microb2010/mainboard/strat_base.h +++ b/projects/microb2010/mainboard/strat_base.h @@ -26,8 +26,9 @@ #define END_OBSTACLE 8 /* There is an obstacle in front of us */ #define END_ERROR 16 /* Cannot do the command */ #define END_INTR 32 /* interrupted by user */ -#define END_TIMER 64 /* we don't a lot of time */ -#define END_RESERVED 128 /* reserved */ +#define END_TIMER 64 /* we don't have a lot of time */ +#define END_RESERVED 128 /* reserved... be careful, sometimes error is + coded on a int8_t */ /* only valid after a END_OBSTACLE */ struct opponent_obstacle { diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c new file mode 100644 index 0000000..72029b3 --- /dev/null +++ b/projects/microb2010/mainboard/strat_corn.c @@ -0,0 +1,548 @@ +/* + * Copyright Droids, Microb Technology (2010) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + + +#include +#include +#include +#include + +#include +#include + +#include + +/* XXX TODO +static +const +change x,y -> i,j to avoid confusion with coords +could be optimized in mem space: it is not needed to store the x,y coord, + we can process it from idx. however it will be less optimized for speed + +*/ + +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 +#define TYPE_UNKNOWN 5 + +/* XXX enum possible ? else just rename */ +#define START 0 +#define UP 1 +#define UP_RIGHT 2 +#define DOWN_RIGHT 3 +#define DOWN 4 +#define DOWN_LEFT 5 +#define UP_LEFT 6 +#define END 7 + +struct point { + int32_t x; + int32_t y; +}; + +struct djpoint { + struct point pos; + uint16_t weight; + struct djpoint *parent; + + uint8_t type:3; + uint8_t parent_pos:3; + uint8_t updated:1; + uint8_t todo:1; +}; + +uint8_t corn_table[CORN_NB]; + +const uint8_t corn_coord_i[CORN_NB] = { + 0, 0, 0, 2, 2, 2, 4, 4, 6, + 6, 8, 8, 10, 10, 10, 12, 12, 12, +}; + +const uint8_t corn_coord_j[CORN_NB] = { + 2, 4, 6, 3, 5, 7, 4, 6, 5, + 7, 4, 6, 3, 5, 7, 2, 4, 6, +}; + +static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; + +/* table to find the symetric idx */ +uint8_t corn_sym[] = { + 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 +}; + +uint8_t corn_side_confs[9][2] = { + { 1, 4 }, + { 0, 4 }, + { 2, 4 }, + { 2, 3 }, + { 0, 3 }, + { 1, 3 }, + { 1, 6 }, + { 0, 6 }, + { 2, 6 }, +}; +uint8_t corn_center_confs[4][2] = { + { 5, 8 }, + { 7, 8 }, + { 5, 9 }, + { 7, 8 }, +}; + + +/* return index from neigh pointer */ +#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) + +void dump(void) +{ + int8_t i, j; + struct djpoint *pt; + + printf_P(PSTR(" ")); + for (i=0; i=0; j--) { + printf_P(PSTR("%3d "), j/2); + + if ((j&1) == 0) + printf_P(PSTR(" ")); + + for (i=0; itype == TYPE_OBSTACLE) + printf_P(PSTR(" X ")); + else if (pt->type == TYPE_DANGEROUS) + printf_P(PSTR(" D ")); + else if (pt->type == TYPE_WHITE_CORN) + printf_P(PSTR(" W ")); + else if (pt->type == TYPE_BLACK_CORN) + printf_P(PSTR(" B ")); + else if (pt->type == TYPE_WAYPOINT) + printf_P(PSTR(" %5d "), pt->weight); + else + printf_P(PSTR(" ? ")); + } + printf_P(PSTR("\r\n")); + } +} + +static inline uint8_t opposite_position(uint8_t pos) +{ + pos += 3; + if (pos > UP_LEFT) + pos -= 6; + return pos; +} + +/* return coord of the entry in the table from the pointer */ +static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) +{ + int8_t idx = PT2IDX(pt); + *x = idx / WAYPOINTS_NBY; + *y = idx % WAYPOINTS_NBY; +} + +/* get the neighbour of the point at specified position */ +static struct djpoint *get_neigh(struct djpoint *pt, + uint8_t position) +{ + int8_t i,j; + struct djpoint *neigh; + + djpoint2ij(pt, &i, &j); + + switch (position) { + case UP: + j++; + break; + case UP_RIGHT: + if (!(i & 1)) j++; + i++; + break; + case DOWN_RIGHT: + if (i & 1) j--; + i++; + break; + case DOWN: + j--; + break; + case DOWN_LEFT: + if (i & 1) j--; + i--; + break; + case UP_LEFT: + if (!(i & 1)) j++; + i--; + break; + default: + return NULL; + } + if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return NULL; + + neigh = &djpoints[i][j]; + + if (neigh->type != TYPE_WAYPOINT) + return NULL; + + return neigh; +} + +static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) +{ + struct djpoint *neigh = NULL; + uint8_t pos = *position + 1; + + for (pos = *position + 1; pos < END; pos++) { + neigh = get_neigh(pt, pos); + if (neigh != NULL) + break; + } + + *position = pos; + return neigh; +} + +/* browse all points */ +#define POINT_FOREACH(cur) \ + for (cur = &djpoints[0][0]; \ + cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ + cur ++) + +/* XXX comment */ +#define NEIGH_FOREACH(neigh, pos, point) \ + for (pos = START, neigh = get_next_neigh(point, &pos); \ + neigh; \ + neigh = get_next_neigh(point, &pos)) + +int dijkstra_init(void) +{ + return 0; +} + +static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +{ + double vx, vy; + vx = p2->pos.x - p1->pos.x; + vy = p2->pos.y - p1->pos.y; + return sqrt(vx * vx + vy * vy); +} + +void dijkstra_process_neighs(struct djpoint *pt) +{ + struct djpoint *neigh; + uint8_t pos, parent_pos; + uint16_t weight; + int8_t i,j; + + djpoint2ij(pt, &i, &j); + printf_P(PSTR("at %d %d:\r\n"), i, j); + + NEIGH_FOREACH(neigh, pos, pt) { + weight = pt->weight + dist(pt, neigh); + parent_pos = opposite_position(pos); + + /* bonus if we keep the same direction */ + if (parent_pos == pt->parent_pos || + pt->parent_pos == END) + weight = weight - 1; + + printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, + pt->parent_pos, parent_pos, + neigh->weight, weight); + if (neigh->weight == 0 || weight < neigh->weight) { + djpoint2ij(neigh, &i, &j); + //printf_P(PSTR(" %d,%d updated\r\n"), i, j); + neigh->weight = weight; + neigh->parent_pos = parent_pos; + neigh->updated = 1; + } + } +} + +int dijkstra(struct djpoint *start) +{ + struct djpoint *cur; + uint8_t todolist = 1; + + start->todo = 1; + + while (todolist) { + printf_P(PSTR("\r\n")); + dump(); + /* process all neighbours of todo list */ + POINT_FOREACH(cur) { + if (!cur->todo) + continue; + dijkstra_process_neighs(cur); + cur->todo = 0; + } + + /* convert updated list in todo list */ + todolist = 0; + POINT_FOREACH(cur) { + if (!cur->updated) + continue; + todolist = 1; + cur->todo = 1; + cur->updated = 0; + } + } + return 0; /* XXX */ +} + +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) +{ + uint8_t n; + for (n = 0; n < CORN_NB; n ++) { + if (i == corn_coord_i[n] && + j == corn_coord_j[n]) + return n; + } + return -1; +} + +int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j) +{ + if (idx >= CORN_NB) + return -1; + *i = corn_coord_i[idx]; + *j = corn_coord_j[idx]; + return 0; +} + +/* return the index of the closest corn at these coordinates. If the + * corn is really too far (~20cm), return -1. The x and y pointer are + * updated with the real position of the corn */ +int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y) +{ + uint8_t idx = -1, n, i, j; + int16_t d, x_corn, y_corn; + int16_t x_corn_min = 0, y_corn_min = 0; + int16_t d_min = 0; + + for (n = 0; n < CORN_NB; n ++) { + corn_idx_to_coordij(n, &i, &j); + x_corn = (OFFSET_CORN_X + i*STEP_CORN_X); + y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y); + d = xy_norm(x_corn, y_corn, *x, *y); + if (d < 200 && (d_min == 0 || d < d_min)) { + d_min = d; + idx = n; + x_corn_min = x_corn; + y_corn_min = y_corn; + } + } + if (d_min == 0) + return -1; + + *x = x_corn_min; + *y = y_corn_min; + + return idx; +} + +int8_t corn_get_sym(int8_t i) +{ + if (i >= CORN_NB) + return -1; + return corn_sym[i]; +} + +void init_corn_table(int8_t conf_side, int8_t conf_center) +{ + int8_t sym, i; + + /* before match */ + if (conf_side == -1) { + for (i=0; ipos.x = x; + pt->pos.y = y; + + pt->type = TYPE_WAYPOINT; + pt->parent_pos = END; + pt->updated = 0; + pt->todo = 0; + + y += STEP_CORN_Y; + } + + x += STEP_CORN_X; + } +} + +/* update the type and weight of waypoints, before starting + * dijkstra */ +void update_waypoints(void) +{ + int8_t i, j, c; + struct djpoint *pt; + + for (i=0; iweight = 0; + + /* corn */ + c = ijcoord_to_corn_idx(i, j); + if (c >= 0) { + pt->type = corn_table[c]; + continue; + } + /* too close of border */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + pt->type = TYPE_WAYPOINT; + } + } +} + +int get_path(struct djpoint *start, struct djpoint *end) +{ + struct djpoint *cur; + uint8_t prev_direction = 0; + + for (cur=start; + cur != NULL && cur->parent_pos != END && cur != end; + cur = get_neigh(cur, cur->parent_pos)) { + if (prev_direction != cur->parent_pos) { + printf_P(PSTR("%d %d (%d)\r\n"), + cur->pos.x, cur->pos.y, + cur->parent_pos); + } + prev_direction = cur->parent_pos; + } + printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y); + + return 0; /* XXX */ +} + +#if 0 +int main(void) +{ + struct djpoint *start; + struct djpoint *end; + + start = &djpoints[1][1]; + end = &djpoints[12][1]; + + init_corn_table(0, 0); + init_waypoints(); + update_waypoints(); + + dijkstra(end); + dump(); + + get_path(start, end); + + return 0; +} +#endif diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h new file mode 100644 index 0000000..8ab2398 --- /dev/null +++ b/projects/microb2010/mainboard/strat_corn.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids, Microb Technology (2010) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#define CORN_NB 18 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 + +extern uint8_t corn_table[CORN_NB]; + +int8_t ijcoord_to_corn_idx(int8_t i, int8_t j); +int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); +void init_corn_table(int8_t conf_side, int8_t conf_center); -- 2.20.1 From 78150017ab8c5615af414df706a0525fe7c262ae Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 17 Apr 2010 11:46:41 +0200 Subject: [PATCH 11/16] avant la coupe de belgique --- .../trajectory_manager_core.c | 2 +- projects/microb2010/ballboard/main.c | 3 + projects/microb2010/ballboard/state.c | 6 +- projects/microb2010/cobboard/spickle.c | 8 +- projects/microb2010/cobboard/state.c | 2 +- .../microb2010/mainboard/commands_mainboard.c | 225 ++---------------- projects/microb2010/mainboard/commands_traj.c | 12 +- projects/microb2010/mainboard/cs.c | 12 +- projects/microb2010/mainboard/display.py | 38 ++- projects/microb2010/mainboard/main.c | 4 +- projects/microb2010/mainboard/main.h | 6 +- projects/microb2010/mainboard/robotsim.c | 11 +- projects/microb2010/mainboard/strat.c | 35 ++- projects/microb2010/mainboard/strat_base.c | 43 ++-- projects/microb2010/mainboard/strat_corn.c | 214 +++++++++++++++++ projects/microb2010/mainboard/strat_corn.h | 18 +- 16 files changed, 382 insertions(+), 257 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index c6c70fc..18f5c90 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -1007,10 +1007,10 @@ static void start_clitoid(struct trajectory *traj) delete_event(traj); DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f", __FUNCTION__, Va, Aa); - traj->state = RUNNING_CLITOID_CURVE; d = fabs(R_mm * a_rad); d *= 3.; /* margin to avoid deceleration */ trajectory_d_a_rel(traj, d, DEG(a_rad)); + traj->state = RUNNING_CLITOID_CURVE; set_quadramp_acc(traj, traj->d_acc, Aa); set_quadramp_speed(traj, traj->d_speed, Va); } diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index ed8716b..de8bc0c 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -58,6 +58,7 @@ #include "actuator.h" #include "cs.h" #include "i2c_protocol.h" +#include "state.h" /* 0 means "programmed" * ---- with 16 Mhz quartz @@ -252,6 +253,8 @@ int main(void) printf_P(PSTR("\r\n")); printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); + + state_machine(); cmdline_interact(); return 0; diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c index 0492e18..57d44ff 100644 --- a/projects/microb2010/ballboard/state.c +++ b/projects/microb2010/ballboard/state.c @@ -56,7 +56,7 @@ static struct vt100 local_vt100; static volatile uint8_t state_mode; -static uint8_t ball_count; +static volatile uint8_t ball_count; /* short aliases */ #define INIT I2C_BALLBOARD_MODE_INIT @@ -82,6 +82,7 @@ uint8_t state_get_ball_count(void) return ball_count; } +#if 0 static void state_debug_wait_key_pressed(void) { if (!state_debug) @@ -89,6 +90,7 @@ static void state_debug_wait_key_pressed(void) printf_P(PSTR("press a key\r\n")); while (!cmdline_keypressed()); } +#endif /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) @@ -128,7 +130,7 @@ uint8_t state_get_mode(void) /* harvest balls from area */ static void state_do_harvest(void) { - state_debug_wait_key_pressed(); + //state_debug_wait_key_pressed(); roller_on(); } diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index dcee125..cbcb65c 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -67,16 +67,16 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 0, /* left */ - 0, /* right */ + 200, /* left */ + 200, /* right */ }, .pos_mid = { 25000, /* left */ 26000, /* right */ }, .pos_packed = { - 53000, /* left */ - 54500, /* right */ + 55800, /* left */ + 55800, /* right */ }, }; diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 389411e..d310168 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -233,7 +233,7 @@ static void state_do_harvest(uint8_t side) /* store it */ shovel_up(); - while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) { + while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) { STMCH_DEBUG("shovel blocked"); shovel_down(); time_wait_ms(250); diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 7c42411..de73518 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -296,9 +296,6 @@ struct cmd_start_result { /* function called when cmd_start is parsed successfully */ static void cmd_start_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_start_result *res = parsed_result; uint8_t old_level = gen.log_level; @@ -327,7 +324,6 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -1130,65 +1126,6 @@ struct cmd_test_result { int32_t dist; }; -#define LINE_UP 0 -#define LINE_DOWN 1 -#define LINE_R_UP 2 -#define LINE_L_DOWN 3 -#define LINE_L_UP 4 -#define LINE_R_DOWN 5 - -struct line_2pts { - point_t p1; - point_t p2; -}; - -static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) -{ - float n = num; - - switch (dir) { - - case LINE_UP: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(0); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(2100); - break; - case LINE_DOWN: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(2100); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(0); - break; - case LINE_R_UP: - l->p1.x = 150; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 2850; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_L_DOWN: - l->p1.x = 2850; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 150; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - case LINE_L_UP: - l->p1.x = 2850; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 150; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_R_DOWN: - l->p1.x = 150; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 2850; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - default: - break; - } -} - #if 0 static void reverse_line(struct line_2pts *l) { @@ -1203,142 +1140,14 @@ static void reverse_line(struct line_2pts *l) } #endif -/* return 1 if there is a corn near, and fill the index ptr */ -static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) -{ -#define SENSOR_CORN_DIST 225 -#define SENSOR_CORN_ANGLE 90 - double x = position_get_x_double(&mainboard.pos); - double y = position_get_y_double(&mainboard.pos); - double a_rad = position_get_a_rad_double(&mainboard.pos); - double x_corn, y_corn; - int16_t x_corn_int, y_corn_int; - - if (side == I2C_LEFT_SIDE) { - x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - } - else { - x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; - } - x_corn_int = x_corn; - y_corn_int = y_corn; - - *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); - if (*corn_idx < 0) - return 0; - return 1; -} - -/* - * - send the correct commands to the spickles - * - return 1 if we need to stop (cobboard is stucked) -*/ -static uint8_t handle_spickles(void) -{ - int8_t corn_idx; - - if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) - i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); - else - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - } -/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ -/* time_wait_ms(100); */ - - if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) - i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); - else - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); - } - - return 0; -} - -static void line2line(uint8_t dir1, uint8_t num1, - uint8_t dir2, uint8_t num2) -{ - double line1_a_rad, line1_a_deg, line2_a_rad; - double diff_a_deg, diff_a_deg_abs, beta_deg; - double radius; - struct line_2pts l1, l2; - line_t ll1, ll2; - point_t p; - uint8_t err; - - /* convert to 2 points */ - num2line(&l1, dir1, num1); - num2line(&l2, dir2, num2); - - printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); - printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); - - /* convert to line eq and find intersection */ - pts2line(&l1.p1, &l1.p2, &ll1); - pts2line(&l2.p1, &l2.p2, &ll2); - intersect_line(&ll1, &ll2, &p); - - line1_a_rad = atan2(l1.p2.y - l1.p1.y, - l1.p2.x - l1.p1.x); - line1_a_deg = DEG(line1_a_rad); - line2_a_rad = atan2(l2.p2.y - l2.p1.y, - l2.p2.x - l2.p1.x); - diff_a_deg = DEG(line2_a_rad - line1_a_rad); - diff_a_deg_abs = fabs(diff_a_deg); - - if (diff_a_deg_abs < 70.) { - radius = 200; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else if (diff_a_deg_abs < 100.) { - radius = 100; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else { - radius = 120; - if (diff_a_deg > 0) - beta_deg = 60; - else - beta_deg = -60; - } - - trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, - line1_a_deg, 150., diff_a_deg, beta_deg, - radius, xy_norm(l1.p1.x, l1.p1.y, - p.x, p.y)); - err = 0; - while (err == 0) { - err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); - if (err == 0) { - /* cobboard is stucked */ - trajectory_hardstop(&mainboard.traj); - return; /* XXX do something */ - } - err = test_traj_end(0xFF); - } - return; -} - /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { + uint8_t err; + #ifdef HOST_VERSION - strat_reset_pos(298.48, 309.21, 70.02); + strat_reset_pos(298.48, COLOR_Y(309.21), + COLOR_A(70.02)); mainboard.angle.on = 1; mainboard.distance.on = 1; strat_set_speed(250, SPEED_ANGLE_FAST); @@ -1346,11 +1155,29 @@ static void cmd_test_parsed(void *parsed_result, void *data) init_corn_table(0, 0); time_wait_ms(100); - line2line(LINE_UP, 0, LINE_R_DOWN, 2); - line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); - line2line(LINE_R_UP, 2, LINE_UP, 5); - + err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + err = line2line(LINE_R_UP, 2, LINE_UP, 5); trajectory_hardstop(&mainboard.traj); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -100); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_eject(); + time_wait_ms(2000); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 8c59996..be9150b 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -766,14 +766,13 @@ static void auto_position(void) strat_get_speed(&old_spdd, &old_spda); strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); - strat_reset_pos(ROBOT_WIDTH/2, + strat_reset_pos(ROBOT_WIDTH/2 + 100, COLOR_Y(ROBOT_HALF_LENGTH_FRONT), COLOR_A(-90)); + strat_hardstop(); trajectory_d_rel(&mainboard.traj, -180); err = wait_traj_end(END_INTR|END_TRAJ); @@ -785,14 +784,13 @@ static void auto_position(void) if (err == END_INTR) goto intr; - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, DO_NOT_SET_POS, 180); + strat_hardstop(); trajectory_d_rel(&mainboard.traj, -170); err = wait_traj_end(END_INTR|END_TRAJ); diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index b8d6e08..516f00b 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -213,9 +213,9 @@ void microb_cs_init(void) RIGHT_ENCODER, IMP_COEF * 1.); #else rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, - LEFT_ENCODER, IMP_COEF * -1.036); + LEFT_ENCODER, IMP_COEF * -1.011718); rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * 1.037); + RIGHT_ENCODER, IMP_COEF * 1.012695); #endif /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -261,7 +261,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.angle.bd); bd_set_speed_threshold(&mainboard.angle.bd, 80); - bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 20); /* ---- CS distance */ /* PID */ @@ -287,7 +287,7 @@ void microb_cs_init(void) /* Blocking detection */ bd_init(&mainboard.distance.bd); bd_set_speed_threshold(&mainboard.distance.bd, 60); - bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); #ifndef HOST_VERSION /* ---- CS left_cobroller */ @@ -332,8 +332,8 @@ void microb_cs_init(void) #endif /* !HOST_VERSION */ /* set them on !! */ - mainboard.angle.on = 0; - mainboard.distance.on = 0; + mainboard.angle.on = 1; + mainboard.distance.on = 1; mainboard.left_cobroller.on = 1; mainboard.right_cobroller.on = 1; diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index eb63f03..6106ff0 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -41,6 +41,10 @@ greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ] greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea) greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3)) +YELLOW = 0 +BLUE = 1 +color = YELLOW + def square(sz): sq = curve() sq.pos = [(-sz, -sz, 0.3), @@ -245,33 +249,49 @@ def toggle_obj_disp(): else: o.visible = 1 +def toggle_color(): + global color + global BLUE, YELLOW + if color == YELLOW: + color = BLUE + else: + color = YELLOW def set_robot(): global robot, last_pos, robot_trail, robot_trail_list global save_pos, robot_x, robot_y, robot_a - axis = (math.cos(robot_a*math.pi/180), - math.sin(robot_a*math.pi/180), + if color == YELLOW: + tmp_x = robot_x - AREA_X/2 + tmp_y = robot_y - AREA_Y/2 + tmp_a = robot_a + else: + tmp_x = -robot_x + AREA_X/2 + tmp_y = -robot_y + AREA_Y/2 + tmp_a = robot_a + + robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2) + axis = (math.cos(tmp_a*math.pi/180), + math.sin(tmp_a*math.pi/180), 0) - robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2) robot.axis = axis robot.size = (250, 320, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180), + lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180), + tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180), ROBOT_HEIGHT/2) lspickle.axis = axis lspickle.size = (20, 320, 5) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180), + rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180), + tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180), ROBOT_HEIGHT/2) rspickle.axis = axis rspickle.size = (20, 320, 5) # save position - save_pos.append((robot.pos.x, robot.pos, robot_a)) + save_pos.append((robot.pos.x, robot.pos.y, tmp_a)) pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: @@ -372,6 +392,8 @@ while True: save() elif k == "h": toggle_obj_disp() + elif k == "i": + toggle_color() # EOF if l == "": diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 98d3749..633a35d 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -83,8 +83,8 @@ struct genboard gen; struct mainboard mainboard; -struct cobboard cobboard; -struct ballboard ballboard; +volatile struct cobboard cobboard; +volatile struct ballboard ballboard; #ifndef HOST_VERSION /***********************/ diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 21c0065..367718f 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -201,15 +201,15 @@ struct cobboard { /* state of ballboard, synchronized through i2c */ struct ballboard { - uint8_t mode; + volatile uint8_t mode; uint8_t status; uint8_t ball_count; }; extern struct genboard gen; extern struct mainboard mainboard; -extern struct cobboard cobboard; -extern struct ballboard ballboard; +extern volatile struct cobboard cobboard; +extern volatile struct ballboard ballboard; /* start the bootloader */ void bootloader(void); diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index a0fa5a0..db161b8 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -67,11 +67,16 @@ void robotsim_dump(void) { char buf[BUFSIZ]; int len; + int16_t x, y, a; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + a = position_get_a_deg_s16(&mainboard.pos); +/* y = COLOR_Y(y); */ +/* a = COLOR_A(a); */ len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n", - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); + x, y, a); hostsim_lock(); write(fdw, buf, len); hostsim_unlock(); diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 1a525e5..c253e5f 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -112,7 +112,7 @@ void strat_preinit(void) mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_POWER; - i2c_cobboard_mode_init(); + //i2c_cobboard_mode_init(); strat_dump_conf(); strat_dump_infos(__FUNCTION__); } @@ -155,6 +155,10 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + /* used in strat_base for END_TIMER */ mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_TIMER | DO_POWER; @@ -197,6 +201,35 @@ void strat_event(void *dummy) static uint8_t strat_beginning(void) { + uint8_t err; + + strat_set_speed(250, SPEED_ANGLE_FAST); + //init_corn_table(0, 0); + + err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + err = line2line(LINE_R_UP, 2, LINE_UP, 5); + trajectory_hardstop(&mainboard.traj); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -100); + err = wait_traj_end(END_INTR|END_TRAJ); + i2c_cobboard_mode_eject(); + time_wait_ms(2000); + return END_TRAJ; } diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 2c52778..a6724ad 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -101,7 +101,7 @@ const char *get_err(uint8_t err) return "END_UNKNOWN"; } -void strat_hardstop(void) +void strat_hardstop(void) { trajectory_hardstop(&mainboard.traj); pid_reset(&mainboard.angle.pid); @@ -140,7 +140,7 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) return END_BLOCKING; } -/* reset position */ +/* reset position */ void strat_reset_pos(int16_t x, int16_t y, int16_t a) { int16_t posx = position_get_x_s16(&mainboard.pos); @@ -162,7 +162,7 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a) DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); } -/* +/* * decrease gain on angle PID, and go forward until we reach the * border. */ @@ -171,12 +171,17 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) int32_t p = pid_get_gain_P(&mainboard.angle.pid); int32_t i = pid_get_gain_I(&mainboard.angle.pid); int32_t d = pid_get_gain_D(&mainboard.angle.pid); + int32_t max_in = pid_get_max_in(&mainboard.angle.pid); + int32_t max_i = pid_get_max_I(&mainboard.angle.pid); + int32_t max_out = pid_get_max_out(&mainboard.angle.pid); uint8_t err; - pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000); + pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); trajectory_d_rel(&mainboard.traj, dist); err = wait_traj_end(flags); pid_set_gains(&mainboard.angle.pid, p, i, d); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); return err; } @@ -190,7 +195,7 @@ static void strat_update_traj_speed(void) a = strat_speed_a; if (strat_limit_speed_a && a > strat_limit_speed_a) a = strat_limit_speed_a; - + trajectory_set_speed(&mainboard.traj, d, a); } @@ -272,7 +277,7 @@ void strat_limit_speed(void) /* start the strat */ void strat_start(void) -{ +{ int8_t i; uint8_t err; @@ -294,7 +299,7 @@ void strat_start(void) break; } } - + /* if start sw plugged */ if (!sensor_get(S_START_SWITCH)) { printf_P(PSTR("Ready, unplug start switch to start\r\n")); @@ -344,14 +349,14 @@ uint8_t strat_obstacle(void) /* opponent is in front of us */ if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " - "xrel=%d yrel=%d (speed_d=%d)", + "xrel=%d yrel=%d (speed_d=%d)", opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); sensor_obstacle_disable(); return 1; } /* opponent is behind us */ if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { - DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", + DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", opp_d, opp_a, x_rel, y_rel); sensor_obstacle_disable(); return 1; @@ -371,7 +376,7 @@ void interrupt_traj_reset(void) } uint8_t test_traj_end(uint8_t why) -{ +{ uint16_t cur_timer; point_t robot_pt; @@ -389,26 +394,26 @@ uint8_t test_traj_end(uint8_t why) } if ((why & END_INTR) && traj_intr) { - interrupt_traj_reset(); + interrupt_traj_reset(); return END_INTR; } if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) return END_TRAJ; - + /* we are near the destination point (depends on current * speed) AND the robot is in the area bounding box. */ if (why & END_NEAR) { - int16_t d_near = 100; - + int16_t d_near = 100; + if (mainboard.speed_d > 2000) d_near = 150; - + if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && is_in_boundingbox(&robot_pt)) return END_NEAR; } - + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { strat_hardstop(); return END_BLOCKING; @@ -443,7 +448,7 @@ uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) line, opp_x, opp_y, opp_d, opp_a); } else { - DEBUG(E_USER_STRAT, "Got %s at line %d", + DEBUG(E_USER_STRAT, "Got %s at line %d", get_err(ret), line); } return ret; diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 72029b3..4262e34 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -29,7 +29,40 @@ #include #include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_corn.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" /* XXX TODO static @@ -525,6 +558,187 @@ int get_path(struct djpoint *start, struct djpoint *end) return 0; /* XXX */ } +/* return 1 if there is a corn near, and fill the index ptr */ +static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +{ +#define SENSOR_CORN_DIST 225 +#define SENSOR_CORN_ANGLE 90 + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a_rad = position_get_a_rad_double(&mainboard.pos); + double x_corn, y_corn; + int16_t x_corn_int, y_corn_int; + + if (side == I2C_LEFT_SIDE) { + x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + else { + x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; + } + x_corn_int = x_corn; + y_corn_int = y_corn; + + *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (*corn_idx < 0) + return 0; + return 1; +} + +/* + * - send the correct commands to the spickles + * - return 1 if we need to stop (cobboard is stucked) +*/ +static uint8_t handle_spickles(void) +{ + int8_t corn_idx; + + return 0; + + if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) + i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + else + i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + } +/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ +/* time_wait_ms(100); */ + + if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) + i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); + else { + if (corn_table[corn_idx] == TYPE_WHITE_CORN) + i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + else + i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + } + + return 0; +} + +uint8_t line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2) +{ + double line1_a_rad, line1_a_deg, line2_a_rad; + double diff_a_deg, diff_a_deg_abs, beta_deg; + double radius; + struct line_2pts l1, l2; + line_t ll1, ll2; + point_t p; + uint8_t err; + + /* convert to 2 points */ + num2line(&l1, dir1, num1); + num2line(&l2, dir2, num2); + + printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); + printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + + /* convert to line eq and find intersection */ + pts2line(&l1.p1, &l1.p2, &ll1); + pts2line(&l2.p1, &l2.p2, &ll2); + intersect_line(&ll1, &ll2, &p); + + line1_a_rad = atan2(l1.p2.y - l1.p1.y, + l1.p2.x - l1.p1.x); + line1_a_deg = DEG(line1_a_rad); + line2_a_rad = atan2(l2.p2.y - l2.p1.y, + l2.p2.x - l2.p1.x); + diff_a_deg = DEG(line2_a_rad - line1_a_rad); + diff_a_deg_abs = fabs(diff_a_deg); + + if (diff_a_deg_abs < 70.) { + radius = 200; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else if (diff_a_deg_abs < 100.) { + radius = 100; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else { + radius = 120; + if (diff_a_deg > 0) + beta_deg = 60; + else + beta_deg = -60; + } + + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, + line1_a_deg, 150., diff_a_deg, beta_deg, + radius, xy_norm(l1.p1.x, l1.p1.y, + p.x, p.y)); + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return err; /* XXX do something */ + } + err = test_traj_end(0xFF); + } + return err; +} + +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +{ + float n = num; + + switch (dir) { + + case LINE_UP: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(0); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(2100); + break; + case LINE_DOWN: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(2100); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(0); + break; + case LINE_R_UP: + l->p1.x = 150; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 2850; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_L_DOWN: + l->p1.x = 2850; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 150; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + case LINE_L_UP: + l->p1.x = 2850; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 150; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_R_DOWN: + l->p1.x = 150; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 2850; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + default: + break; + } +} + + #if 0 int main(void) { diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index 8ab2398..def2237 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -29,8 +29,24 @@ #define TYPE_BLACK_CORN 3 #define TYPE_OBSTACLE 4 +#define LINE_UP 0 +#define LINE_DOWN 1 +#define LINE_R_UP 2 +#define LINE_L_DOWN 3 +#define LINE_L_UP 4 +#define LINE_R_DOWN 5 + +struct line_2pts { + point_t p1; + point_t p2; +}; + extern uint8_t corn_table[CORN_NB]; -int8_t ijcoord_to_corn_idx(int8_t i, int8_t j); +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j); int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); void init_corn_table(int8_t conf_side, int8_t conf_center); + +uint8_t line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2); +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); -- 2.20.1 From 4e7801883ed4076cb14b63a0571467747894c0f8 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 22 Apr 2010 22:35:44 +0200 Subject: [PATCH 12/16] strat db --- projects/microb2010/ballboard/main.c | 2 +- projects/microb2010/ballboard/main.h | 2 + projects/microb2010/cobboard/main.c | 2 +- projects/microb2010/cobboard/main.h | 2 + projects/microb2010/cobboard/sensor.c | 6 +- projects/microb2010/cobboard/sensor.h | 4 +- projects/microb2010/cobboard/spickle.c | 4 +- projects/microb2010/common/i2c_commands.h | 2 + projects/microb2010/mainboard/Makefile | 1 + projects/microb2010/mainboard/commands.c | 4 +- .../microb2010/mainboard/commands_mainboard.c | 54 +- projects/microb2010/mainboard/commands_traj.c | 66 +-- projects/microb2010/mainboard/main.c | 3 +- projects/microb2010/mainboard/main.h | 4 +- projects/microb2010/mainboard/sensor.c | 4 +- projects/microb2010/mainboard/sensor.h | 4 +- projects/microb2010/mainboard/strat.c | 90 ++- projects/microb2010/mainboard/strat.h | 74 +-- projects/microb2010/mainboard/strat_avoid.c | 369 +++++++++++- projects/microb2010/mainboard/strat_base.c | 26 +- projects/microb2010/mainboard/strat_corn.c | 552 +----------------- projects/microb2010/mainboard/strat_corn.h | 16 +- 22 files changed, 549 insertions(+), 742 deletions(-) diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index de8bc0c..725a89d 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -170,7 +170,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_BALLBOARD) { diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index 9f10c35..0758279 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -19,6 +19,8 @@ * */ +/* was sensorboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index d21e7b7..7a3d240 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -173,7 +173,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_COBBOARD) { diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 0c12fdc..105ca7d 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -19,6 +19,8 @@ * */ +/* was mainboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index c471033..eb1f42e 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -140,7 +140,7 @@ struct sensor_filter { }; /* pullup mapping: - * CAP 1,5,6,7,8 + * CAP 1,5,6,7,8 (notation elec) */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */ @@ -148,8 +148,8 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ - [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used - [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used + [S_CAP6] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ + [S_CAP7] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ [S_RCOB] = { 1, 0, 0, 1, 0, 1 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index c24af73..a603e97 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -33,8 +33,8 @@ #define S_COB_INSIDE_R 2 #define S_CAP4 3 #define S_LCOB 4 -#define S_LEFT 5 -#define S_RIGHT 6 +#define S_CAP6 5 +#define S_CAP7 6 #define S_RCOB 7 #define S_RESERVED1 8 #define S_RESERVED2 9 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index cbcb65c..f8e1613 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -67,8 +67,8 @@ static struct spickle_params spickle = { &cobboard.right_spickle, }, .pos_deployed = { - 200, /* left */ - 200, /* right */ + 7000, // 200, /* left */ + 7000, // 200, /* right */ }, .pos_mid = { 25000, /* left */ diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 4b8aea5..c0ff8e1 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -38,6 +38,8 @@ #define I2C_COB_BLACK 0 #define I2C_COB_WHITE 1 +#define I2C_COB_UNKNOWN 2 +#define I2C_COB_REMOVED 3 struct i2c_cmd_hdr { uint8_t cmd; diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 33b5c52..2ebfff2 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,6 +7,7 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c strat_corn.c +SRC += strat_db.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index e5bb456..879297a 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -105,7 +105,7 @@ extern parse_pgm_inst_t cmd_goto2; extern parse_pgm_inst_t cmd_goto3; extern parse_pgm_inst_t cmd_position; extern parse_pgm_inst_t cmd_position_set; -extern parse_pgm_inst_t cmd_strat_infos; +extern parse_pgm_inst_t cmd_strat_db; extern parse_pgm_inst_t cmd_strat_conf; extern parse_pgm_inst_t cmd_strat_conf2; extern parse_pgm_inst_t cmd_strat_conf3; @@ -194,7 +194,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_goto2, (parse_pgm_inst_t *)&cmd_position, (parse_pgm_inst_t *)&cmd_position_set, - (parse_pgm_inst_t *)&cmd_strat_infos, + (parse_pgm_inst_t *)&cmd_strat_db, (parse_pgm_inst_t *)&cmd_strat_conf, (parse_pgm_inst_t *)&cmd_strat_conf2, (parse_pgm_inst_t *)&cmd_strat_conf3, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index de73518..e4eee57 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,7 @@ #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_db.h" #include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -301,11 +302,11 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = E_USER_STRAT; if (!strcmp_P(res->debug, PSTR("debug"))) { - strat_infos.dump_enabled = 1; + strat_db.dump_enabled = 1; gen.log_level = 5; } else { - strat_infos.dump_enabled = 0; + strat_db.dump_enabled = 0; gen.log_level = 0; } @@ -1126,58 +1127,9 @@ struct cmd_test_result { int32_t dist; }; -#if 0 -static void reverse_line(struct line_2pts *l) -{ - point_t tmp; - - tmp.x = l->p1.x; - tmp.y = l->p1.y; - l->p1.x = l->p2.x; - l->p1.y = l->p2.y; - l->p2.x = tmp.x; - l->p2.y = tmp.y; -} -#endif - /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - uint8_t err; - -#ifdef HOST_VERSION - strat_reset_pos(298.48, COLOR_Y(309.21), - COLOR_A(70.02)); - mainboard.angle.on = 1; - mainboard.distance.on = 1; - strat_set_speed(250, SPEED_ANGLE_FAST); -#endif - init_corn_table(0, 0); - time_wait_ms(100); - - err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); - err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); - err = line2line(LINE_R_UP, 2, LINE_UP, 5); - trajectory_hardstop(&mainboard.traj); - - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* half turn */ - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); - trajectory_a_rel(&mainboard.traj, COLOR_A(180)); - err = wait_traj_end(END_INTR|END_TRAJ); - - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, -100); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_eject(); - time_wait_ms(2000); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index be9150b..d63f57a 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -59,6 +59,7 @@ #include "strat_utils.h" #include "strat_base.h" #include "strat.h" +#include "strat_db.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" @@ -891,37 +892,37 @@ parse_pgm_inst_t cmd_position_set = { /**********************************************************/ /* strat configuration */ -/* this structure is filled when cmd_strat_infos is parsed successfully */ -struct cmd_strat_infos_result { +/* this structure is filled when cmd_strat_db is parsed successfully */ +struct cmd_strat_db_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_strat_infos is parsed successfully */ -static void cmd_strat_infos_parsed(void *parsed_result, void *data) +/* function called when cmd_strat_db is parsed successfully */ +static void cmd_strat_db_parsed(void *parsed_result, void *data) { - struct cmd_strat_infos_result *res = parsed_result; + struct cmd_strat_db_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("reset"))) { - strat_reset_infos(); + strat_db_init(); } - strat_infos.dump_enabled = 1; - strat_dump_infos(__FUNCTION__); + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); } -prog_char str_strat_infos_arg0[] = "strat_infos"; -parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); -prog_char str_strat_infos_arg1[] = "show#reset"; -parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); +prog_char str_strat_db_arg0[] = "strat_db"; +parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0); +prog_char str_strat_db_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1); -prog_char help_strat_infos[] = "reset/show strat_infos"; -parse_pgm_inst_t cmd_strat_infos = { - .f = cmd_strat_infos_parsed, /* function to call */ +prog_char help_strat_db[] = "reset/show strat_db"; +parse_pgm_inst_t cmd_strat_db = { + .f = cmd_strat_db_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_strat_infos, + .help_str = help_strat_db, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_infos_arg0, - (prog_void *)&cmd_strat_infos_arg1, + (prog_void *)&cmd_strat_db_arg0, + (prog_void *)&cmd_strat_db_arg1, NULL, }, }; @@ -939,9 +940,8 @@ struct cmd_strat_conf_result { static void cmd_strat_conf_parsed(void *parsed_result, void *data) { // struct cmd_strat_conf_result *res = parsed_result; - - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf_arg0[] = "strat_conf"; @@ -1002,12 +1002,12 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) #endif if (on) - strat_infos.conf.flags |= bit; + strat_conf.flags |= bit; else - strat_infos.conf.flags &= (~bit); + strat_conf.flags &= (~bit); - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf2_arg0[] = "strat_conf"; @@ -1050,32 +1050,32 @@ static void cmd_strat_conf3_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.scan_opp_min_time = res->arg2; + strat_conf.scan_opp_min_time = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.delay_between_opp_scan = res->arg2; + strat_conf.delay_between_opp_scan = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.scan_our_min_time = res->arg2; + strat_conf.scan_our_min_time = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.delay_between_our_scan = res->arg2; + strat_conf.delay_between_our_scan = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { - strat_infos.conf.wait_opponent = res->arg2; + strat_conf.wait_opponent = res->arg2; } else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { - strat_infos.conf.lintel_min_time = res->arg2; + strat_conf.lintel_min_time = res->arg2; } #endif - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf3_arg0[] = "strat_conf"; diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 633a35d..14a2aad 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -67,6 +67,7 @@ #include "actuator.h" #include "cs.h" #include "strat_base.h" +#include "strat_db.h" #include "i2c_protocol.h" /* 0 means "programmed" @@ -289,7 +290,7 @@ int main(void) /* strat */ gen.logs[0] = E_USER_STRAT; gen.log_level = 5; - strat_reset_infos(); + strat_db_init(); /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 367718f..ff86dd2 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -19,6 +19,8 @@ * */ +/* was mechboard in 2009 */ + #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ port &= ~_BV(bit); \ @@ -119,7 +121,7 @@ #define I2C_POLL_PRIO 20 #define EEPROM_TIME_PRIO 10 -#define CS_PERIOD 5000L /* in microsecond */ +#define CS_PERIOD 5000L /* in microsecond */ #define CS_HZ (1000000. / CS_PERIOD) #define NB_LOGS 4 diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index 473ed76..fb0bfd9 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -160,8 +160,8 @@ struct sensor_filter { static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */ [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ - [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ - [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_LFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_RFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 4 */ [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ [S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h index 0ab7a3b..9a4cd96 100644 --- a/projects/microb2010/mainboard/sensor.h +++ b/projects/microb2010/mainboard/sensor.h @@ -32,8 +32,8 @@ /* synchronize with sensor.c */ #define S_CAP1 0 #define S_CAP2 1 -#define S_COLUMN_RIGHT 2 -#define S_COLUMN_LEFT 3 +#define S_LFRONT 2 +#define S_RFRONT 3 #define S_START_SWITCH 4 #define S_DISP_LEFT 5 #define S_RCOB_WHITE 6 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index c253e5f..a5a952c 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -57,6 +57,7 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" @@ -66,16 +67,7 @@ #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 -struct strat_infos strat_infos = { - /* conf */ - .conf = { - .flags = 0, - }, - /* status */ - .status = { - .flags = 0, - }, -}; +struct strat_conf strat_conf; /*************************************************************/ @@ -83,27 +75,6 @@ struct strat_infos strat_infos = { /*************************************************************/ -void strat_set_bounding_box(void) -{ - if (get_color() == I2C_COLOR_YELLOW) { - strat_infos.area_bbox.x1 = 300; - strat_infos.area_bbox.y1 = 200; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1800; - } - else { - strat_infos.area_bbox.x1 = 200; - strat_infos.area_bbox.y1 = 300; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1900; - } - - polygon_set_boundingbox(strat_infos.area_bbox.x1, - strat_infos.area_bbox.y1, - strat_infos.area_bbox.x2, - strat_infos.area_bbox.y2); -} - /* called before each strat, and before the start switch */ void strat_preinit(void) { @@ -113,41 +84,25 @@ void strat_preinit(void) DO_POS | DO_BD | DO_POWER; //i2c_cobboard_mode_init(); - strat_dump_conf(); - strat_dump_infos(__FUNCTION__); + strat_conf_dump(__FUNCTION__); + strat_db_dump(__FUNCTION__); } -void strat_dump_conf(void) +void strat_conf_dump(const char *caller) { - if (!strat_infos.dump_enabled) + if (!strat_conf.dump_enabled) return; printf_P(PSTR("-- conf --\r\n")); } -/* display current information about the state of the game */ -void strat_dump_infos(const char *caller) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR("%s() dump strat infos:\r\n"), caller); -} - -/* init current area state before a match. Dump update user conf - * here */ -void strat_reset_infos(void) -{ - init_corn_table(-1, -1); -} - /* call it just before launching the strat */ void strat_init(void) { /* XXX init rollers, .. */ - strat_reset_infos(); + strat_db_init(); /* we consider that the color is correctly set */ @@ -168,17 +123,21 @@ void strat_init(void) /* call it after each strat */ void strat_exit(void) { +#ifndef HOST_VERSION uint8_t flags; +#endif mainboard.flags &= ~(DO_TIMER); strat_hardstop(); time_reset(); - wait_ms(1000); + wait_ms(100); +#ifndef HOST_VERSION IRQ_LOCK(flags); mainboard.flags &= ~(DO_CS); + IRQ_UNLOCK(flags); pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); - IRQ_UNLOCK(flags); +#endif } /* called periodically */ @@ -204,11 +163,31 @@ static uint8_t strat_beginning(void) uint8_t err; strat_set_speed(250, SPEED_ANGLE_FAST); - //init_corn_table(0, 0); + l1: err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l1; + } + + l2: err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l2; + } + + l3: err = line2line(LINE_R_UP, 2, LINE_UP, 5); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + time_wait_ms(2000); + goto l3; + } + trajectory_hardstop(&mainboard.traj); /* ball ejection */ @@ -230,6 +209,7 @@ static uint8_t strat_beginning(void) i2c_cobboard_mode_eject(); time_wait_ms(2000); + trajectory_hardstop(&mainboard.traj); return END_TRAJ; } diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 0a31073..42b47dd 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -42,24 +42,51 @@ #define CORNER_X 3000 #define CORNER_Y COLOR_Y(2100) +/* XXX these offset are not related to corn, but to waypoints */ +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 +#define TOMATO_NB 14 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + /* - * + * Corn position and lines * * vertical lines * O 1 2 3 4 5 * 2100 +-----|-----|-----|-----|-----|-----|-----+ - * | o o o | - * | o o o o | diag - * | o o o | lines - * 0/ o o o o \0 - * y | o o | - * 1/ o o \1 - * | | - * 2/------ ------\2 + * | c5 c9 c14 | + * | c2 c7 c11 c17 | diag + * | c4 c8 c13 | lines + * 0/ c1 c6 c10 c16 \0 + * y | c3 c12 | + * 1/ c0 c15 \1 + * |-----+ +-----| + * 2/ | | \2 * | | | | + * 0 +-----+-----------------------------+-----+ + * 0 x 3000 + * + * Ball (tomato) and i,j coords (for waypoints) + * + * 2100 +--0--1--2--3--4--5--6--7--8--9-10-11-12--+ + * 7 t5 t9 | + * 6 t3 t7 t11 | + * 5 t1 t4 t8 t13 | + * 4 t2 t6 t10 | + * y 3 t0 t12 | + * 2 | + * 1-----+ +-----| + * 0 | | | * | | | | * 0 +-----+-----------------------------+-----+ * 0 x 3000 + * */ /* useful traj flags */ @@ -83,38 +110,17 @@ #define SPEED_ANGLE_VERY_SLOW 400. /* strat infos structures */ - -struct strat_bbox { - int32_t x1; - int32_t y1; - int32_t x2; - int32_t y2; -}; - struct strat_conf { -#define STRAT_CONF_XXX 0x01 - uint8_t flags; -}; + uint8_t dump_enabled; -struct strat_status { -#define STRAT_STATUS_LHARVEST 0x01 -#define STRAT_STATUS_RHARVEST 0x02 +#define STRAT_CONF_XXX 0x01 uint8_t flags; }; -/* all infos related to strat */ -struct strat_infos { - uint8_t dump_enabled; - struct strat_conf conf; - struct strat_bbox area_bbox; - struct strat_status status; -}; -extern struct strat_infos strat_infos; +extern struct strat_conf strat_conf; /* in strat.c */ -void strat_dump_infos(const char *caller); /* show current known state - of area */ -void strat_dump_conf(void); +void strat_conf_dump(const char *caller); void strat_reset_infos(void); /* reset current known state */ void strat_preinit(void); void strat_init(void); diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 60dead3..3c128bf 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -1,5 +1,5 @@ /* - * Copyright Droids Corporation, Microb Technology (2009) + * Copyright Droids, Microb Technology (2010) * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -15,18 +15,19 @@ * 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_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * + * Olivier MATZ */ + #include #include -#include +#include #include +#include #include -#include -#include #include #include @@ -38,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -46,10 +49,364 @@ #include #include +#include + #include #include #include "../common/i2c_commands.h" - +#include "i2c_protocol.h" #include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_corn.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" + +/* XXX TODO +static +const +change x,y -> i,j to avoid confusion with coords +could be optimized in mem space: it is not needed to store the x,y coord, + we can process it from idx. however it will be less optimized for speed + +*/ + +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 +#define TYPE_UNKNOWN 5 + +/* XXX enum possible ? else just rename */ +#define START 0 +#define UP 1 +#define UP_RIGHT 2 +#define DOWN_RIGHT 3 +#define DOWN 4 +#define DOWN_LEFT 5 +#define UP_LEFT 6 +#define END 7 + +struct djpoint { + uint16_t weight; + struct djpoint *parent; + + uint8_t type:3; + uint8_t parent_pos:3; + uint8_t updated:1; + uint8_t todo:1; +}; + +uint8_t corn_table[CORN_NB]; + +static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; + +/* return index from neigh pointer */ +#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) + +void dump(void) +{ + int8_t i, j; + struct djpoint *pt; + + printf_P(PSTR(" ")); + for (i=0; i=0; j--) { + printf_P(PSTR("%3d "), j/2); + + if ((j&1) == 0) + printf_P(PSTR(" ")); + + for (i=0; itype == TYPE_OBSTACLE) + printf_P(PSTR(" X ")); + else if (pt->type == TYPE_DANGEROUS) + printf_P(PSTR(" D ")); + else if (pt->type == TYPE_WHITE_CORN) + printf_P(PSTR(" W ")); + else if (pt->type == TYPE_BLACK_CORN) + printf_P(PSTR(" B ")); + else if (pt->type == TYPE_WAYPOINT) + printf_P(PSTR(" %5d "), pt->weight); + else + printf_P(PSTR(" ? ")); + } + printf_P(PSTR("\r\n")); + } +} + +static inline uint8_t opposite_position(uint8_t pos) +{ + pos += 3; + if (pos > UP_LEFT) + pos -= 6; + return pos; +} + +/* return coord of the entry in the table from the pointer */ +static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) +{ + int8_t idx = PT2IDX(pt); + *x = idx / WAYPOINTS_NBY; + *y = idx % WAYPOINTS_NBY; +} + +/* get the neighbour of the point at specified position */ +static struct djpoint *get_neigh(struct djpoint *pt, + uint8_t position) +{ + int8_t i,j; + struct djpoint *neigh; + + djpoint2ij(pt, &i, &j); + + switch (position) { + case UP: + j++; + break; + case UP_RIGHT: + if (!(i & 1)) j++; + i++; + break; + case DOWN_RIGHT: + if (i & 1) j--; + i++; + break; + case DOWN: + j--; + break; + case DOWN_LEFT: + if (i & 1) j--; + i--; + break; + case UP_LEFT: + if (!(i & 1)) j++; + i--; + break; + default: + return NULL; + } + if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return NULL; + + neigh = &djpoints[i][j]; + + if (neigh->type != TYPE_WAYPOINT) + return NULL; + + return neigh; +} + +static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) +{ + struct djpoint *neigh = NULL; + uint8_t pos = *position + 1; + + for (pos = *position + 1; pos < END; pos++) { + neigh = get_neigh(pt, pos); + if (neigh != NULL) + break; + } + + *position = pos; + return neigh; +} + +/* browse all points */ +#define POINT_FOREACH(cur) \ + for (cur = &djpoints[0][0]; \ + cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ + cur ++) + +/* XXX comment */ +#define NEIGH_FOREACH(neigh, pos, point) \ + for (pos = START, neigh = get_next_neigh(point, &pos); \ + neigh; \ + neigh = get_next_neigh(point, &pos)) + +int dijkstra_init(void) +{ + return 0; +} + +static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +{ + double vx, vy; + vx = p2->pos.x - p1->pos.x; + vy = p2->pos.y - p1->pos.y; + return sqrt(vx * vx + vy * vy); +} + +void dijkstra_process_neighs(struct djpoint *pt) +{ + struct djpoint *neigh; + uint8_t pos, parent_pos; + uint16_t weight; + int8_t i,j; + + djpoint2ij(pt, &i, &j); + printf_P(PSTR("at %d %d:\r\n"), i, j); + + NEIGH_FOREACH(neigh, pos, pt) { + weight = pt->weight + dist(pt, neigh); + parent_pos = opposite_position(pos); + + /* bonus if we keep the same direction */ + if (parent_pos == pt->parent_pos || + pt->parent_pos == END) + weight = weight - 1; + + printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, + pt->parent_pos, parent_pos, + neigh->weight, weight); + if (neigh->weight == 0 || weight < neigh->weight) { + djpoint2ij(neigh, &i, &j); + //printf_P(PSTR(" %d,%d updated\r\n"), i, j); + neigh->weight = weight; + neigh->parent_pos = parent_pos; + neigh->updated = 1; + } + } +} + +int dijkstra(struct djpoint *start) +{ + struct djpoint *cur; + uint8_t todolist = 1; + + start->todo = 1; + + while (todolist) { + printf_P(PSTR("\r\n")); + dump(); + /* process all neighbours of todo list */ + POINT_FOREACH(cur) { + if (!cur->todo) + continue; + dijkstra_process_neighs(cur); + cur->todo = 0; + } + + /* convert updated list in todo list */ + todolist = 0; + POINT_FOREACH(cur) { + if (!cur->updated) + continue; + todolist = 1; + cur->todo = 1; + cur->updated = 0; + } + } + return 0; /* XXX */ +} + +/* init waypoints position */ +void init_djpoints(void) +{ + int8_t i, j; + struct djpoint *pt; + + for (i=0; itype = TYPE_WAYPOINT; + pt->parent_pos = END; + pt->updated = 0; + pt->todo = 0; + } + } +} + +/* update the type and weight of waypoints, before starting + * dijkstra */ +void update_djpoints(void) +{ + int8_t i, j, c; + struct djpoint *pt; + + for (i=0; iweight = 0; + + /* corn */ + c = ijcoord_to_corn_idx(i, j); + if (c >= 0) { + pt->type = corn_table[c]; + continue; + } + /* too close of border */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + pt->type = TYPE_WAYPOINT; + } + } +} + +int get_path(struct djpoint *start, struct djpoint *end) +{ + struct djpoint *cur; + uint8_t prev_direction = 0; + int8_t idx; + int16_t x, y; + + for (cur = start; + cur != NULL && cur->parent_pos != END && cur != end; + cur = get_neigh(cur, cur->parent_pos)) { + if (prev_direction != cur->parent_pos) { + idx = PT2IDX(cur); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d (%d)\r\n"), + x, y, cur->parent_pos); + } + prev_direction = cur->parent_pos; + } + idx = PT2IDX(end); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d\r\n"), x, y); + return 0; /* XXX */ +} diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index a6724ad..1f3b6e5 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -278,13 +278,15 @@ void strat_limit_speed(void) /* start the strat */ void strat_start(void) { - int8_t i; uint8_t err; strat_preinit(); +#ifndef HOST_VERSION /* if start sw not plugged */ if (sensor_get(S_START_SWITCH)) { + int8_t i; + printf_P(PSTR("No start switch, press a key or plug it\r\n")); /* while start sw not plugged */ @@ -306,9 +308,11 @@ void strat_start(void) /* while start sw plugged */ while (!sensor_get(S_START_SWITCH)); } +#endif strat_init(); err = strat_main(); + printf("coucou\n"); NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err)); strat_exit(); } @@ -316,6 +320,7 @@ void strat_start(void) /* return true if we have to brake due to an obstacle */ uint8_t strat_obstacle(void) { +#if 0 int16_t x_rel, y_rel; int16_t opp_x, opp_y, opp_d, opp_a; @@ -333,6 +338,25 @@ uint8_t strat_obstacle(void) opponent_obstacle.y = opp_y; opponent_obstacle.d = opp_d; opponent_obstacle.a = opp_a; +#else /* belgium cup only */ + int16_t x_rel, y_rel; + int16_t opp_d, opp_a; + double opp_x, opp_y; + +#ifdef HOST_VERSION + if (time_get_s() >= 12 && time_get_s() <= 30) + return 1; +#endif + if (!sensor_get(S_RCOB_WHITE)) + return 0; + + opp_a = 0; + opp_d = 300; + + rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y); + if (!is_in_area(opp_x, opp_y, 250)) + return 0; +#endif /* sensor are temporarily disabled */ if (sensor_obstacle_is_disabled()) diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 4262e34..3694bcd 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -58,508 +58,16 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" #include "sensor.h" #include "actuator.h" -/* XXX TODO -static -const -change x,y -> i,j to avoid confusion with coords -could be optimized in mem space: it is not needed to store the x,y coord, - we can process it from idx. however it will be less optimized for speed - -*/ - -#define OFFSET_CORN_X 150 -#define OFFSET_CORN_Y 222 -#define STEP_CORN_X 225 -#define STEP_CORN_Y 250 - -#define CORN_NB 18 - -#define WAYPOINTS_NBX 13 -#define WAYPOINTS_NBY 8 - -/* enum is better */ -#define TYPE_WAYPOINT 0 -#define TYPE_DANGEROUS 1 -#define TYPE_WHITE_CORN 2 -#define TYPE_BLACK_CORN 3 -#define TYPE_OBSTACLE 4 -#define TYPE_UNKNOWN 5 - -/* XXX enum possible ? else just rename */ -#define START 0 -#define UP 1 -#define UP_RIGHT 2 -#define DOWN_RIGHT 3 -#define DOWN 4 -#define DOWN_LEFT 5 -#define UP_LEFT 6 -#define END 7 - -struct point { - int32_t x; - int32_t y; -}; - -struct djpoint { - struct point pos; - uint16_t weight; - struct djpoint *parent; - - uint8_t type:3; - uint8_t parent_pos:3; - uint8_t updated:1; - uint8_t todo:1; -}; - -uint8_t corn_table[CORN_NB]; - -const uint8_t corn_coord_i[CORN_NB] = { - 0, 0, 0, 2, 2, 2, 4, 4, 6, - 6, 8, 8, 10, 10, 10, 12, 12, 12, -}; - -const uint8_t corn_coord_j[CORN_NB] = { - 2, 4, 6, 3, 5, 7, 4, 6, 5, - 7, 4, 6, 3, 5, 7, 2, 4, 6, -}; - -static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; - -/* table to find the symetric idx */ -uint8_t corn_sym[] = { - 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 -}; - -uint8_t corn_side_confs[9][2] = { - { 1, 4 }, - { 0, 4 }, - { 2, 4 }, - { 2, 3 }, - { 0, 3 }, - { 1, 3 }, - { 1, 6 }, - { 0, 6 }, - { 2, 6 }, -}; -uint8_t corn_center_confs[4][2] = { - { 5, 8 }, - { 7, 8 }, - { 5, 9 }, - { 7, 8 }, -}; - - -/* return index from neigh pointer */ -#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) - -void dump(void) -{ - int8_t i, j; - struct djpoint *pt; - - printf_P(PSTR(" ")); - for (i=0; i=0; j--) { - printf_P(PSTR("%3d "), j/2); - - if ((j&1) == 0) - printf_P(PSTR(" ")); - - for (i=0; itype == TYPE_OBSTACLE) - printf_P(PSTR(" X ")); - else if (pt->type == TYPE_DANGEROUS) - printf_P(PSTR(" D ")); - else if (pt->type == TYPE_WHITE_CORN) - printf_P(PSTR(" W ")); - else if (pt->type == TYPE_BLACK_CORN) - printf_P(PSTR(" B ")); - else if (pt->type == TYPE_WAYPOINT) - printf_P(PSTR(" %5d "), pt->weight); - else - printf_P(PSTR(" ? ")); - } - printf_P(PSTR("\r\n")); - } -} - -static inline uint8_t opposite_position(uint8_t pos) -{ - pos += 3; - if (pos > UP_LEFT) - pos -= 6; - return pos; -} - -/* return coord of the entry in the table from the pointer */ -static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) -{ - int8_t idx = PT2IDX(pt); - *x = idx / WAYPOINTS_NBY; - *y = idx % WAYPOINTS_NBY; -} - -/* get the neighbour of the point at specified position */ -static struct djpoint *get_neigh(struct djpoint *pt, - uint8_t position) -{ - int8_t i,j; - struct djpoint *neigh; - - djpoint2ij(pt, &i, &j); - - switch (position) { - case UP: - j++; - break; - case UP_RIGHT: - if (!(i & 1)) j++; - i++; - break; - case DOWN_RIGHT: - if (i & 1) j--; - i++; - break; - case DOWN: - j--; - break; - case DOWN_LEFT: - if (i & 1) j--; - i--; - break; - case UP_LEFT: - if (!(i & 1)) j++; - i--; - break; - default: - return NULL; - } - if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) - return NULL; - - neigh = &djpoints[i][j]; - - if (neigh->type != TYPE_WAYPOINT) - return NULL; - - return neigh; -} - -static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) -{ - struct djpoint *neigh = NULL; - uint8_t pos = *position + 1; - - for (pos = *position + 1; pos < END; pos++) { - neigh = get_neigh(pt, pos); - if (neigh != NULL) - break; - } - - *position = pos; - return neigh; -} - -/* browse all points */ -#define POINT_FOREACH(cur) \ - for (cur = &djpoints[0][0]; \ - cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ - cur ++) - -/* XXX comment */ -#define NEIGH_FOREACH(neigh, pos, point) \ - for (pos = START, neigh = get_next_neigh(point, &pos); \ - neigh; \ - neigh = get_next_neigh(point, &pos)) - -int dijkstra_init(void) -{ - return 0; -} - -static uint16_t dist(struct djpoint *p1, struct djpoint *p2) -{ - double vx, vy; - vx = p2->pos.x - p1->pos.x; - vy = p2->pos.y - p1->pos.y; - return sqrt(vx * vx + vy * vy); -} - -void dijkstra_process_neighs(struct djpoint *pt) -{ - struct djpoint *neigh; - uint8_t pos, parent_pos; - uint16_t weight; - int8_t i,j; - - djpoint2ij(pt, &i, &j); - printf_P(PSTR("at %d %d:\r\n"), i, j); - - NEIGH_FOREACH(neigh, pos, pt) { - weight = pt->weight + dist(pt, neigh); - parent_pos = opposite_position(pos); - - /* bonus if we keep the same direction */ - if (parent_pos == pt->parent_pos || - pt->parent_pos == END) - weight = weight - 1; - - printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, - pt->parent_pos, parent_pos, - neigh->weight, weight); - if (neigh->weight == 0 || weight < neigh->weight) { - djpoint2ij(neigh, &i, &j); - //printf_P(PSTR(" %d,%d updated\r\n"), i, j); - neigh->weight = weight; - neigh->parent_pos = parent_pos; - neigh->updated = 1; - } - } -} - -int dijkstra(struct djpoint *start) -{ - struct djpoint *cur; - uint8_t todolist = 1; - - start->todo = 1; - - while (todolist) { - printf_P(PSTR("\r\n")); - dump(); - /* process all neighbours of todo list */ - POINT_FOREACH(cur) { - if (!cur->todo) - continue; - dijkstra_process_neighs(cur); - cur->todo = 0; - } - - /* convert updated list in todo list */ - todolist = 0; - POINT_FOREACH(cur) { - if (!cur->updated) - continue; - todolist = 1; - cur->todo = 1; - cur->updated = 0; - } - } - return 0; /* XXX */ -} - -int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) -{ - uint8_t n; - for (n = 0; n < CORN_NB; n ++) { - if (i == corn_coord_i[n] && - j == corn_coord_j[n]) - return n; - } - return -1; -} - -int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j) -{ - if (idx >= CORN_NB) - return -1; - *i = corn_coord_i[idx]; - *j = corn_coord_j[idx]; - return 0; -} - -/* return the index of the closest corn at these coordinates. If the - * corn is really too far (~20cm), return -1. The x and y pointer are - * updated with the real position of the corn */ -int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y) -{ - uint8_t idx = -1, n, i, j; - int16_t d, x_corn, y_corn; - int16_t x_corn_min = 0, y_corn_min = 0; - int16_t d_min = 0; - - for (n = 0; n < CORN_NB; n ++) { - corn_idx_to_coordij(n, &i, &j); - x_corn = (OFFSET_CORN_X + i*STEP_CORN_X); - y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y); - d = xy_norm(x_corn, y_corn, *x, *y); - if (d < 200 && (d_min == 0 || d < d_min)) { - d_min = d; - idx = n; - x_corn_min = x_corn; - y_corn_min = y_corn; - } - } - if (d_min == 0) - return -1; - - *x = x_corn_min; - *y = y_corn_min; - - return idx; -} - -int8_t corn_get_sym(int8_t i) -{ - if (i >= CORN_NB) - return -1; - return corn_sym[i]; -} - -void init_corn_table(int8_t conf_side, int8_t conf_center) -{ - int8_t sym, i; - - /* before match */ - if (conf_side == -1) { - for (i=0; ipos.x = x; - pt->pos.y = y; - - pt->type = TYPE_WAYPOINT; - pt->parent_pos = END; - pt->updated = 0; - pt->todo = 0; - - y += STEP_CORN_Y; - } - - x += STEP_CORN_X; - } -} - -/* update the type and weight of waypoints, before starting - * dijkstra */ -void update_waypoints(void) -{ - int8_t i, j, c; - struct djpoint *pt; - - for (i=0; iweight = 0; - - /* corn */ - c = ijcoord_to_corn_idx(i, j); - if (c >= 0) { - pt->type = corn_table[c]; - continue; - } - /* too close of border */ - if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* hill */ - if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* dangerous points */ - if (i == 0 || i == (WAYPOINTS_NBX-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - pt->type = TYPE_WAYPOINT; - } - } -} - -int get_path(struct djpoint *start, struct djpoint *end) -{ - struct djpoint *cur; - uint8_t prev_direction = 0; - - for (cur=start; - cur != NULL && cur->parent_pos != END && cur != end; - cur = get_neigh(cur, cur->parent_pos)) { - if (prev_direction != cur->parent_pos) { - printf_P(PSTR("%d %d (%d)\r\n"), - cur->pos.x, cur->pos.y, - cur->parent_pos); - } - prev_direction = cur->parent_pos; - } - printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y); - - return 0; /* XXX */ -} - +#if 0 /* return 1 if there is a corn near, and fill the index ptr */ -static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) { #define SENSOR_CORN_DIST 225 #define SENSOR_CORN_ANGLE 90 @@ -568,6 +76,7 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) double a_rad = position_get_a_rad_double(&mainboard.pos); double x_corn, y_corn; int16_t x_corn_int, y_corn_int; + struct waypoint_db *wp; if (side == I2C_LEFT_SIDE) { x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST; @@ -580,11 +89,12 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) x_corn_int = x_corn; y_corn_int = y_corn; - *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); - if (*corn_idx < 0) + wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); + if (wp->corn.idx < 0) return 0; return 1; } +#endif /* * - send the correct commands to the spickles @@ -592,9 +102,9 @@ static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) */ static uint8_t handle_spickles(void) { - int8_t corn_idx; - return 0; +#if 0 + int8_t corn_idx; if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); @@ -617,6 +127,7 @@ static uint8_t handle_spickles(void) } return 0; +#endif } uint8_t line2line(uint8_t dir1, uint8_t num1, @@ -678,16 +189,20 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, p.x, p.y)); - err = 0; - while (err == 0) { - err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); - if (err == 0) { - /* cobboard is stucked */ - trajectory_hardstop(&mainboard.traj); - return err; /* XXX do something */ + if (0) { + err = 0; + while (err == 0) { + err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF); + if (err == 0) { + /* cobboard is stucked */ + trajectory_hardstop(&mainboard.traj); + return err; /* XXX do something */ + } + err = test_traj_end(0xFF); } - err = test_traj_end(0xFF); } + + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); return err; } @@ -737,26 +252,3 @@ void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) break; } } - - -#if 0 -int main(void) -{ - struct djpoint *start; - struct djpoint *end; - - start = &djpoints[1][1]; - end = &djpoints[12][1]; - - init_corn_table(0, 0); - init_waypoints(); - update_waypoints(); - - dijkstra(end); - dump(); - - get_path(start, end); - - return 0; -} -#endif diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index def2237..ca4a7d6 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -20,15 +20,6 @@ * Olivier MATZ */ -#define CORN_NB 18 - -/* enum is better */ -#define TYPE_WAYPOINT 0 -#define TYPE_DANGEROUS 1 -#define TYPE_WHITE_CORN 2 -#define TYPE_BLACK_CORN 3 -#define TYPE_OBSTACLE 4 - #define LINE_UP 0 #define LINE_DOWN 1 #define LINE_R_UP 2 @@ -41,12 +32,7 @@ struct line_2pts { point_t p2; }; -extern uint8_t corn_table[CORN_NB]; - -int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j); -int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y); -void init_corn_table(int8_t conf_side, int8_t conf_center); +void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); uint8_t line2line(uint8_t dir1, uint8_t num1, uint8_t dir2, uint8_t num2); -void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); -- 2.20.1 From 06813a0566192d90e1ad8c891dfb7cc55dedd525 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 22 Apr 2010 23:49:57 +0200 Subject: [PATCH 13/16] strat db + avoid --- projects/microb2010/mainboard/Makefile | 2 +- .../microb2010/mainboard/commands_mainboard.c | 12 + projects/microb2010/mainboard/strat_avoid.c | 136 ++--- projects/microb2010/mainboard/strat_db.c | 474 ++++++++++++++++++ projects/microb2010/mainboard/strat_db.h | 139 +++++ 5 files changed, 679 insertions(+), 84 deletions(-) create mode 100644 projects/microb2010/mainboard/strat_db.c create mode 100644 projects/microb2010/mainboard/strat_db.h diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 2ebfff2..7df1dc5 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,7 +7,7 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c strat_corn.c -SRC += strat_db.c +SRC += strat_db.c strat_avoid.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index e4eee57..e552583 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1130,6 +1130,18 @@ struct cmd_test_result { /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK); + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 3c128bf..e661ac1 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -58,6 +58,7 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" @@ -73,24 +74,6 @@ could be optimized in mem space: it is not needed to store the x,y coord, */ -#define OFFSET_CORN_X 150 -#define OFFSET_CORN_Y 222 -#define STEP_CORN_X 225 -#define STEP_CORN_Y 250 - -#define CORN_NB 18 - -#define WAYPOINTS_NBX 13 -#define WAYPOINTS_NBY 8 - -/* enum is better */ -#define TYPE_WAYPOINT 0 -#define TYPE_DANGEROUS 1 -#define TYPE_WHITE_CORN 2 -#define TYPE_BLACK_CORN 3 -#define TYPE_OBSTACLE 4 -#define TYPE_UNKNOWN 5 - /* XXX enum possible ? else just rename */ #define START 0 #define UP 1 @@ -105,14 +88,13 @@ struct djpoint { uint16_t weight; struct djpoint *parent; - uint8_t type:3; uint8_t parent_pos:3; uint8_t updated:1; uint8_t todo:1; + uint8_t reserved:3; }; -uint8_t corn_table[CORN_NB]; - +/* database for dijkstra */ static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; /* return index from neigh pointer */ @@ -122,6 +104,7 @@ void dump(void) { int8_t i, j; struct djpoint *pt; + struct waypoint_db *wp; printf_P(PSTR(" ")); for (i=0; itype == TYPE_OBSTACLE) + if (wp->type == WP_TYPE_OBSTACLE) printf_P(PSTR(" X ")); - else if (pt->type == TYPE_DANGEROUS) + else if (wp->dangerous) printf_P(PSTR(" D ")); - else if (pt->type == TYPE_WHITE_CORN) + else if (wp->type == WP_TYPE_CORN && + wp->corn.color == I2C_COB_WHITE) printf_P(PSTR(" W ")); - else if (pt->type == TYPE_BLACK_CORN) + else if (wp->type == WP_TYPE_CORN && + wp->corn.color == I2C_COB_BLACK) printf_P(PSTR(" B ")); - else if (pt->type == TYPE_WAYPOINT) + else if (wp->type == WP_TYPE_CORN && + wp->corn.color == I2C_COB_UNKNOWN) + printf_P(PSTR(" U ")); + else if (wp->type == WP_TYPE_WAYPOINT || + wp->type == WP_TYPE_TOMATO) printf_P(PSTR(" %5d "), pt->weight); else printf_P(PSTR(" ? ")); @@ -166,19 +156,35 @@ static inline uint8_t opposite_position(uint8_t pos) return pos; } +/* is point reachable by the robot ? */ +static uint8_t is_reachable(uint8_t i, uint8_t j) +{ + struct waypoint_db *wp; + + wp = &strat_db.wp_table[i][j]; + if (wp->type == WP_TYPE_WAYPOINT) + return 1; + if (wp->type == WP_TYPE_TOMATO) + return 1; + if (wp->type == WP_TYPE_CORN && + wp->present == 0) + return 1; + return 0; +} + /* return coord of the entry in the table from the pointer */ -static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) +static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j) { int8_t idx = PT2IDX(pt); - *x = idx / WAYPOINTS_NBY; - *y = idx % WAYPOINTS_NBY; + *i = idx / WAYPOINTS_NBY; + *j = idx % WAYPOINTS_NBY; } /* get the neighbour of the point at specified position */ static struct djpoint *get_neigh(struct djpoint *pt, uint8_t position) { - int8_t i,j; + uint8_t i,j; struct djpoint *neigh; djpoint2ij(pt, &i, &j); @@ -212,11 +218,10 @@ static struct djpoint *get_neigh(struct djpoint *pt, if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) return NULL; - neigh = &djpoints[i][j]; - - if (neigh->type != TYPE_WAYPOINT) + if (is_reachable(i, j) == 0) return NULL; + neigh = &djpoints[i][j]; return neigh; } @@ -252,11 +257,21 @@ int dijkstra_init(void) return 0; } +/* return distance between p1 and p2 */ static uint16_t dist(struct djpoint *p1, struct djpoint *p2) { + int16_t x1, y1, x2, y2; double vx, vy; - vx = p2->pos.x - p1->pos.x; - vy = p2->pos.y - p1->pos.y; + uint8_t i, j; + + djpoint2ij(p1, &i, &j); + ijcoord_to_xycoord(i, j, &x1, &y1); + + djpoint2ij(p2, &i, &j); + ijcoord_to_xycoord(i, j, &x2, &y2); + + vx = x2 - x1; + vy = y2 - y1; return sqrt(vx * vx + vy * vy); } @@ -265,7 +280,7 @@ void dijkstra_process_neighs(struct djpoint *pt) struct djpoint *neigh; uint8_t pos, parent_pos; uint16_t weight; - int8_t i,j; + uint8_t i,j; djpoint2ij(pt, &i, &j); printf_P(PSTR("at %d %d:\r\n"), i, j); @@ -333,55 +348,10 @@ void init_djpoints(void) for (j=0; jtype = TYPE_WAYPOINT; pt->parent_pos = END; pt->updated = 0; pt->todo = 0; - } - } -} - -/* update the type and weight of waypoints, before starting - * dijkstra */ -void update_djpoints(void) -{ - int8_t i, j, c; - struct djpoint *pt; - - for (i=0; iweight = 0; - - /* corn */ - c = ijcoord_to_corn_idx(i, j); - if (c >= 0) { - pt->type = corn_table[c]; - continue; - } - /* too close of border */ - if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* hill */ - if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { - pt->type = TYPE_OBSTACLE; - continue; - } - /* dangerous points */ - if (i == 0 || i == (WAYPOINTS_NBX-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { - pt->type = TYPE_DANGEROUS; - continue; - } - pt->type = TYPE_WAYPOINT; } } } @@ -398,14 +368,14 @@ int get_path(struct djpoint *start, struct djpoint *end) cur = get_neigh(cur, cur->parent_pos)) { if (prev_direction != cur->parent_pos) { idx = PT2IDX(cur); - corn_idx_to_coordxy(idx, &x, &y); + corn_idx_to_xycoord(idx, &x, &y); printf_P(PSTR("%d %d (%d)\r\n"), x, y, cur->parent_pos); } prev_direction = cur->parent_pos; } idx = PT2IDX(end); - corn_idx_to_coordxy(idx, &x, &y); + corn_idx_to_xycoord(idx, &x, &y); printf_P(PSTR("%d %d\r\n"), x, y); return 0; /* XXX */ diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c new file mode 100644 index 0000000..57c20ba --- /dev/null +++ b/projects/microb2010/mainboard/strat_db.c @@ -0,0 +1,474 @@ +/* + * Copyright Droids, Microb Technology (2010) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_corn.h" +#include "strat_db.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" + +/* status of objects on area */ +struct strat_db strat_db; + +/* given an index, give the i coord */ +static const uint8_t corn_coord_i[CORN_NB] = { + 0, 0, 0, 2, 2, 2, 4, 4, 6, + 6, 8, 8, 10, 10, 10, 12, 12, 12, +}; + +/* given an index, give the j coord */ +static const uint8_t corn_coord_j[CORN_NB] = { + 2, 4, 6, 3, 5, 7, 4, 6, 5, + 7, 4, 6, 3, 5, 7, 2, 4, 6, +}; + +/* table to find the symetric idx */ +static const uint8_t corn_sym[] = { + 15, 16, 17, 12, 13, 14, 10, 11, + 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 +}; + +#if 0 /* XXX maybe useless */ +/* the 10 possible configurations for corn on the side */ +static const uint8_t corn_side_confs[9][2] = { + { 1, 4 }, + { 0, 4 }, + { 2, 4 }, + { 2, 3 }, + { 0, 3 }, + { 1, 3 }, + { 1, 6 }, + { 0, 6 }, + { 2, 6 }, +}; + +/* the 4 possible configurations for corn on center */ +static const uint8_t corn_center_confs[4][2] = { + { 5, 8 }, + { 7, 8 }, + { 5, 9 }, + { 7, 8 }, +}; +#endif + +/* in these groups, only one black cob */ +static const int8_t corn_group1[] = { 0, 1, 2, -1, }; +static const int8_t corn_group2[] = { 3, 4, 6, -1, }; +static const int8_t corn_group3[] = { 5, 7, -1, }; +static const int8_t corn_group4[] = { 8, 9, -1, }; +static const int8_t corn_group5[] = { 11, 14, -1, }; +static const int8_t corn_group6[] = { 10, 12, 13, -1, }; +static const int8_t corn_group7[] = { 15, 16, 17, -1, }; + +static const int8_t *corn_groups[] = { + corn_group1, + corn_group2, + corn_group3, + corn_group4, + corn_group5, + corn_group6, + corn_group7, + NULL, +}; + +/* given an index, give the i coord */ +static const uint8_t tomato_coord_i[TOMATO_NB] = { + 0, 0, 2, 2, 4, 4, 6, 6, + 8, 8, 10, 10, 12, 12, +}; + +/* given an index, give the j coord */ +static const uint8_t tomato_coord_j[TOMATO_NB] = { + 3, 5, 4, 6, 5, 7, 4, 6, 5, 7, 4, 6, 3, 5, +}; + +/******** Generic waypoint */ + +/* return the xy coords of a waypoint given its ij coords. */ +int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y) +{ + if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY) + return -1; + *x = (OFFSET_CORN_X + i*STEP_CORN_X); + *y = (OFFSET_CORN_Y + j*STEP_CORN_Y); + return 0; +} + +/******** CORN */ + +/* return the index of a corn given its i,j coords. */ +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) +{ + uint8_t n; + for (n = 0; n < CORN_NB; n ++) { + if (i == corn_coord_i[n] && + j == corn_coord_j[n]) + return n; + } + return -1; +} + +/* return the i,j coords of a corn given its index */ +int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j) +{ + if (idx >= CORN_NB) + return -1; + *i = corn_coord_i[idx]; + *j = corn_coord_j[idx]; + return 0; +} + +/* return the index of a corn given its x,y coords. */ +int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y) +{ + uint8_t i, j; + if (corn_idx_to_ijcoord(idx, &i, &j) < 0) + return -1; + if (ijcoord_to_xycoord(i, j, x, y) < 0) + return -1; + return 0; +} + +/* return the index of the closest corn at these coordinates. If the + * corn is really too far (~20cm), return NULL. The x and y pointer are + * updated with the real position of the corn */ +struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y) +{ + uint8_t idx = -1, n; + int16_t d, x_corn, y_corn; + int16_t x_corn_min = 0, y_corn_min = 0; + int16_t d_min = 0; + + /* XXX does it work when we are blue ? */ + for (n = 0; n < CORN_NB; n ++) { + corn_idx_to_xycoord(n, &x_corn, &y_corn); + d = xy_norm(x_corn, y_corn, *x, *y); + /* XXX 200 -> constant */ + if (d < 200 && (d_min == 0 || d < d_min)) { + d_min = d; + idx = n; + x_corn_min = x_corn; + y_corn_min = y_corn; + } + } + if (d_min == 0) + return NULL; + + *x = x_corn_min; + *y = y_corn_min; + + return strat_db.corn_table[idx]; +} + +/* return true if 'idx' is in group */ +static uint8_t is_in_group(const int8_t *group, uint8_t idx) +{ + const int8_t *pidx; + for (pidx = group; *pidx != -1; pidx++) { + if (*pidx == idx) { + return 1; + } + } + return 0; +} + +/* return the number of cob of that color in the group */ +static uint8_t count_in_group(const int8_t *group, uint8_t color) +{ + const int8_t *pidx; + struct waypoint_db *wp; + uint8_t count = 0; + + for (pidx = &group[0]; *pidx != -1; pidx++) { + wp = strat_db.corn_table[*pidx]; + if (wp->corn.color == color) + count ++; + } + return count; +} + +/* set all unkown cobs to specified color */ +static void set_unknown_in_group(const int8_t *group, uint8_t color) +{ + const int8_t *pidx; + struct waypoint_db *wp; + + for (pidx = &group[0]; *pidx != -1; pidx++) { + wp = strat_db.corn_table[*pidx]; + if (wp->corn.color == I2C_COB_UNKNOWN) + wp->corn.color = color; + } +} + +/* depending on which cob is set (and its color), set the color of + * other cobs */ +static void corn_deduct_other(uint8_t idx, uint8_t color) +{ + const int8_t **pgroup; + + for (pgroup = &corn_groups[0]; *pgroup; pgroup++) { + if (!is_in_group(*pgroup, idx)) + continue; + if (color == I2C_COB_BLACK) { + set_unknown_in_group(*pgroup, I2C_COB_WHITE); + } + else if (color == I2C_COB_WHITE) { + if (count_in_group(*pgroup, I2C_COB_UNKNOWN) == 1) + set_unknown_in_group(*pgroup, I2C_COB_BLACK); + } + } +} + +/* set color of a corn + * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN + * it will update the symetric corn if != UNKOWN + * it will also deduct color of some other cobs */ +void corn_set_color(struct waypoint_db *wp, uint8_t color) +{ + uint8_t symidx; + + wp->corn.color = color; + if (color == I2C_COB_UNKNOWN) + return; + corn_deduct_other(wp->corn.idx, color); + symidx = corn_get_sym_idx(wp->corn.idx); + strat_db.corn_table[symidx]->corn.color = color; + corn_deduct_other(symidx, color); +} + + +/* return the idx of the symetric corn */ +int8_t corn_get_sym_idx(int8_t i) +{ + if (i >= CORN_NB) + return -1; + return corn_sym[i]; +} + +/*********** TOMATO */ + +/* return the index of a tomato given its i,j coords. */ +int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j) +{ + uint8_t n; + for (n = 0; n < TOMATO_NB; n ++) { + if (i == tomato_coord_i[n] && + j == tomato_coord_j[n]) + return n; + } + return -1; +} + +/* return the i,j coords of a tomato given its index */ +int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j) +{ + if (idx >= TOMATO_NB) + return -1; + *i = tomato_coord_i[idx]; + *j = tomato_coord_j[idx]; + return 0; +} + +/* return the index of a tomato given its x,y coords. */ +int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y) +{ + uint8_t i, j; + if (tomato_idx_to_ijcoord(idx, &i, &j) < 0) + return -1; + if (ijcoord_to_xycoord(i, j, x, y) < 0) + return -1; + return 0; +} + +/* return the index of the closest tomato at these coordinates. If the + * tomato is really too far (~20cm), return NULL. The x and y pointer are + * updated with the real position of the tomato */ +struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y) +{ + uint8_t idx = -1, n; + int16_t d, x_tomato, y_tomato; + int16_t x_tomato_min = 0, y_tomato_min = 0; + int16_t d_min = 0; + + /* XXX does it work when we are blue ? */ + for (n = 0; n < TOMATO_NB; n ++) { + tomato_idx_to_xycoord(n, &x_tomato, &y_tomato); + d = xy_norm(x_tomato, y_tomato, *x, *y); + /* XXX 200 -> constant */ + if (d < 200 && (d_min == 0 || d < d_min)) { + d_min = d; + idx = n; + x_tomato_min = x_tomato; + y_tomato_min = y_tomato; + } + } + if (d_min == 0) + return NULL; + + *x = x_tomato_min; + *y = y_tomato_min; + + return strat_db.tomato_table[idx]; +} + +/* + * Init internal database. The initialization is done with UNKNOWN + * corn with all objects present + */ +void strat_db_init(void) +{ + struct waypoint_db *wp; + int8_t idx; + int8_t i, j; + + memset(&strat_db, 0, sizeof(strat_db)); + + /* corn table */ + for (i=0; itype = WP_TYPE_WAYPOINT; + + /* mark dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) + wp->dangerous = 1; + if ((i&1) == 0 && j == (WAYPOINTS_NBY-1)) + wp->dangerous = 1; + + /* too close of border, unreachable wp */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + wp->type = WP_TYPE_OBSTACLE; + continue; + } + + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + wp->type = WP_TYPE_OBSTACLE; + continue; + } + + /* corn */ + idx = ijcoord_to_corn_idx(i, j); + if (idx >= 0) { + wp->type = WP_TYPE_CORN; + wp->present = 1; + wp->corn.idx = idx; + wp->corn.color = I2C_COB_UNKNOWN; + continue; + } + + /* tomato */ + idx = ijcoord_to_tomato_idx(i, j); + if (idx >= 0) { + wp->type = WP_TYPE_TOMATO; + wp->present = 1; + continue; + } + } + } +} + +/* dump infos about area and objects */ +void strat_db_dump(const char *caller) +{ + uint8_t i; + struct waypoint_db *wp; + + if (strat_db.dump_enabled == 0) + return; + + printf_P(PSTR("DB dump from <%s>\r\n"), caller); + for (i=0; ipresent, wp->opp_visited); + if (wp->corn.color == I2C_COB_UNKNOWN) + printf_P(PSTR("unknown")); + else if (wp->corn.color == I2C_COB_BLACK) + printf_P(PSTR("black")); + else if (wp->corn.color == I2C_COB_WHITE) + printf_P(PSTR("white")); + printf_P(PSTR("\r\n")); + } + + for (i=0; ipresent, wp->opp_visited); + } +} diff --git a/projects/microb2010/mainboard/strat_db.h b/projects/microb2010/mainboard/strat_db.h new file mode 100644 index 0000000..1b5314f --- /dev/null +++ b/projects/microb2010/mainboard/strat_db.h @@ -0,0 +1,139 @@ +/* + * Copyright Droids, Microb Technology (2010) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +struct corn_db { + /* I2C_COB_UNKNOWN, I2C_COB_WHITE, I2C_COB_BLACK */ + uint8_t color:2; + uint8_t reserved:6; + + /* index in corn table */ + uint8_t idx; +}; + +struct tomato_db { + /* nothing for now */ +}; + +/* structure describing the status of a waypoint */ +struct waypoint_db { + /* type of the waypoint */ +#define WP_TYPE_WAYPOINT 0 /* no object on it */ +#define WP_TYPE_OBSTACLE 1 /* cannot reach this point */ +#define WP_TYPE_TOMATO 2 /* place for a tomato */ +#define WP_TYPE_CORN 3 /* place for a corn */ + uint8_t type:2; + + /* true if point is near the border */ + uint8_t dangerous:1; + + /* true if element is present */ + uint8_t present:1; + + /* visited by opponent */ + uint8_t opp_visited:1; + + uint8_t reserved:3; + + /* absolute position of the waypoint */ +/* int16_t x; */ +/* int16_t y; */ + + union { + struct corn_db corn; + struct tomato_db tomato; + }; +}; + +/* database reflecting the status of objects on area */ +struct strat_db { + uint8_t dump_enabled; + + /* table of waypoints */ + struct waypoint_db wp_table[WAYPOINTS_NBX][WAYPOINTS_NBY]; + + /* corn_table: pointers to waypoints */ + struct waypoint_db *corn_table[CORN_NB]; + + /* tomato_table: pointers to waypoints */ + struct waypoint_db *tomato_table[TOMATO_NB]; + + /* number of oranges remaining */ + uint8_t our_oranges_count; + uint8_t opp_oranges_count; +}; + +/* global structure storing the database */ +extern struct strat_db strat_db; + +/* convert i,j coords to x,y coords */ +int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y); + +/* return the index of a corn given its i,j coords. */ +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j); + +/* return the i,j coords of a corn given its index */ +int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j); + +/* return the index of a corn given its x,y coords. */ +int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y); + +/* return the index of the closest corn at these coordinates. If the + * corn is really too far (~20cm), return NULL. The x and y pointer are + * updated with the real position of the corn */ +struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y); + +/* set color of a corn + * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN + * it will update the symetric corn if != UNKOWN + * it will also deduct color of some other cobs */ +void corn_set_color(struct waypoint_db *wp, uint8_t color); + +/* return the idx of the symetric corn */ +int8_t corn_get_sym_idx(int8_t i); + +/*********** TOMATO */ + +/* return the index of a tomato given its i,j coords. */ +int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j); + +/* return the i,j coords of a tomato given its index */ +int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j); + +/* return the index of a tomato given its x,y coords. */ +int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y); + +/* return the index of the closest tomato at these coordinates. If the + * tomato is really too far (~20cm), return NULL. The x and y pointer are + * updated with the real position of the tomato */ +struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y); + +/* + * Init internal database. The initialization is done with UNKNOWN + * corn with all objects present + */ +void strat_db_init(void); + +/* dump infos about area and objects */ +void strat_db_dump(const char *caller); -- 2.20.1 From 19cd41f7920628a962928580cd1618a1ee5750aa Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 23 Apr 2010 23:26:52 +0200 Subject: [PATCH 14/16] i2c rework --- .../microb2010/cobboard/commands_cobboard.c | 32 +++------ projects/microb2010/cobboard/i2c_protocol.c | 12 ++-- projects/microb2010/cobboard/state.c | 63 +++++++++++----- projects/microb2010/cobboard/state.h | 1 + projects/microb2010/common/i2c_commands.h | 26 +++---- .../microb2010/mainboard/commands_mainboard.c | 24 ++++--- projects/microb2010/mainboard/i2c_protocol.c | 71 +++++-------------- projects/microb2010/mainboard/i2c_protocol.h | 10 ++- projects/microb2010/mainboard/main.h | 2 + projects/microb2010/mainboard/strat.c | 11 +-- 10 files changed, 120 insertions(+), 132 deletions(-) diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index bea27e9..dbbdd79 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -229,38 +229,26 @@ static void cmd_state2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state2_result *res = parsed_result; - uint8_t side, mode = state_get_mode(); + uint8_t side; - if (!strcmp_P(res->arg2, PSTR("left"))) { + if (!strcmp_P(res->arg2, PSTR("left"))) side = I2C_LEFT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - } - else { + else side = I2C_RIGHT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - } if (!strcmp_P(res->arg1, PSTR("pack"))) { - /* nothing to do */ + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, 0); } else if (!strcmp_P(res->arg1, PSTR("deploy"))) { - if (side == I2C_LEFT_SIDE) - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - else - mode |= I2C_COBBOARD_MODE_R_DEPLOY; + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY); } else if (!strcmp_P(res->arg1, PSTR("harvest"))) { - if (side == I2C_LEFT_SIDE) { - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - mode |= I2C_COBBOARD_MODE_L_HARVEST; - } - else { - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - mode |= I2C_COBBOARD_MODE_R_HARVEST; - } + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST); } - - state_set_mode(mode); } prog_char str_state2_arg0[] = "cobboard"; diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 9b2a9e7..b7f13ee 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -130,16 +130,14 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } -#if 0 case I2C_CMD_COBBOARD_SET_MODE: { struct i2c_cmd_cobboard_set_mode *cmd = void_cmd; if (size != sizeof(struct i2c_cmd_cobboard_set_mode)) goto error; - i2c_set_mode(cmd); + state_set_mode(cmd->mode); break; } -#endif case I2C_CMD_GENERIC_SET_COLOR: { @@ -151,7 +149,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) } #ifdef notyet - case I2C_CMD_EXTENSION_TEST: + case I2C_CMD_EXTENSION_TEST: { struct i2c_cmd_extension_test *cmd = void_cmd; if (size != sizeof (*cmd)) @@ -163,7 +161,6 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* Add other commands here ...*/ - case I2C_REQ_COBBOARD_STATUS: { struct i2c_req_cobboard_status *cmd = void_cmd; @@ -171,7 +168,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* mode is in req */ - state_set_mode(cmd->mode); + if (state_get_status() != I2C_COBBOARD_STATUS_OFF) { + state_set_spickle(I2C_LEFT_SIDE, cmd->lspickle); + state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle); + } i2c_send_status(); break; } diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index d310168..7a1f047 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -57,17 +57,12 @@ #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) static struct vt100 local_vt100; -static volatile uint8_t state_mode; +static volatile uint8_t state_mode, lspickle, rspickle; static volatile uint8_t state_status; static uint8_t cob_count; /* short aliases */ -#define L_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_L_DEPLOY)) -#define R_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_R_DEPLOY)) -#define DEPLOY(side, mode) ((side) == I2C_LEFT_SIDE ? L_DEPLOY(mode) : R_DEPLOY(mode)) -#define L_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_L_HARVEST)) -#define R_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST)) -#define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode)) +#define HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_HARVEST)) #define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT)) #define INIT(mode) (!!((mode) & I2C_COBBOARD_MODE_INIT)) @@ -100,30 +95,58 @@ static uint8_t state_no_cob_inside(void) !sensor_get(S_COB_INSIDE_R); } +static uint8_t is_deployed(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return lspickle & I2C_COBBOARD_SPK_DEPLOY; + else + return rspickle & I2C_COBBOARD_SPK_DEPLOY; +} + +static uint8_t is_autoharvest(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST; + else + return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST; +} + /* pack/deploy spickles depending on mode */ static void spickle_prepare(uint8_t side) { if (cob_count >= 5) spickle_pack(side); - else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode)) + else if (is_deployed(side) && !is_autoharvest(side)) spickle_mid(side); - else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode)) + else if (is_deployed(side) && is_autoharvest(side)) spickle_deploy(side); else spickle_pack(side); } +void state_set_spickle(uint8_t side, uint8_t flags) +{ + if (side == I2C_LEFT_SIDE) { + /* preempt current action if not busy */ + if (lspickle != 0 && flags == 0 && + state_status != I2C_COBBOARD_STATUS_LBUSY) + spickle_prepare(I2C_LEFT_SIDE); + lspickle = flags; + } + else { + /* preempt current action if not busy */ + if (rspickle != 0 && flags == 0 && + state_status != I2C_COBBOARD_STATUS_RBUSY) + spickle_prepare(I2C_RIGHT_SIDE); + rspickle = flags; + } +} + /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { state_mode = mode; - /* preempt current action if not busy */ - if (state_status != I2C_COBBOARD_STATUS_LBUSY) - spickle_prepare(I2C_LEFT_SIDE); - if (state_status != I2C_COBBOARD_STATUS_RBUSY) - spickle_prepare(I2C_RIGHT_SIDE); - /* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */ /* "r_deploy=%d r_harvest=%d eject=%d", */ /* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */ @@ -295,9 +318,11 @@ void state_machine(void) /* harvest */ if (cob_count < 5) { - if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) + if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) && + (lspickle & I2C_COBBOARD_SPK_DEPLOY)) state_do_harvest(I2C_LEFT_SIDE); - if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) + if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) && + (rspickle & I2C_COBBOARD_SPK_DEPLOY)) state_do_harvest(I2C_RIGHT_SIDE); } @@ -307,7 +332,7 @@ void state_machine(void) state_do_eject(); } } - state_status = I2C_COBBOARD_STATUS_READY; + state_status = I2C_COBBOARD_STATUS_OFF; } void state_init(void) @@ -320,5 +345,5 @@ void state_init(void) spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; cob_count = 0; - state_status = I2C_COBBOARD_STATUS_READY; + state_status = I2C_COBBOARD_STATUS_OFF; } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index d9f95fc..141b643 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -24,6 +24,7 @@ /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode); +void state_set_spickle(uint8_t side, uint8_t flags); /* get current state */ uint8_t state_get_mode(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index c0ff8e1..7b04977 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -70,17 +70,13 @@ struct i2c_cmd_generic_color { #define I2C_CMD_COBBOARD_SET_MODE 0x02 -/* XXX disabled, use memory sync instead */ struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; -#define I2C_COBBOARD_MODE_L_DEPLOY 0x01 /* deploy the spickle */ -#define I2C_COBBOARD_MODE_L_HARVEST 0x02 /* auto harvest withe cobs */ -#define I2C_COBBOARD_MODE_R_DEPLOY 0x04 /* deploy the spickle */ -#define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ -#define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ -#define I2C_COBBOARD_MODE_INIT 0x20 /* init state machine */ - //uint8_t mode; +#define I2C_COBBOARD_MODE_HARVEST 0x01 /* harvest mode */ +#define I2C_COBBOARD_MODE_EJECT 0x02 /* eject cobs */ +#define I2C_COBBOARD_MODE_INIT 0x03 /* init state machine */ + uint8_t mode; }; #define I2C_CMD_BALLBOARD_SET_MODE 0x10 @@ -108,20 +104,26 @@ struct i2c_cmd_ballboard_set_mode { struct i2c_req_cobboard_status { struct i2c_cmd_hdr hdr; - uint8_t mode; + +#define I2C_COBBOARD_SPK_DEPLOY 0x01 /* deploy the spickle */ +#define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */ + uint8_t lspickle; + uint8_t rspickle; }; #define I2C_ANS_COBBOARD_STATUS 0x81 struct i2c_ans_cobboard_status { struct i2c_cmd_hdr hdr; + /* mode type are defined above: I2C_COBBOARD_MODE_xxx */ uint8_t mode; #define I2C_COBBOARD_STATUS_READY 0x00 -#define I2C_COBBOARD_STATUS_LBUSY 0x01 -#define I2C_COBBOARD_STATUS_RBUSY 0x02 -#define I2C_COBBOARD_STATUS_EJECT 0x03 +#define I2C_COBBOARD_STATUS_OFF 0x01 +#define I2C_COBBOARD_STATUS_LBUSY 0x02 +#define I2C_COBBOARD_STATUS_RBUSY 0x03 +#define I2C_COBBOARD_STATUS_EJECT 0x04 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index e552583..adec77f 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -730,9 +730,9 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_cobboard_mode_init(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_cobboard_mode_eject(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -763,7 +763,7 @@ struct cmd_cobboard_setmode2_result { }; /* function called when cmd_cobboard_setmode2 is parsed successfully */ -static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) +static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) { struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -773,12 +773,18 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - if (!strcmp_P(res->arg1, PSTR("deploy"))) - i2c_cobboard_mode_deploy(side); - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_cobboard_mode_harvest(side); - else if (!strcmp_P(res->arg1, PSTR("pack"))) - i2c_cobboard_mode_pack(side); + if (!strcmp_P(res->arg1, PSTR("deploy"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_deploy(side); + } + else if (!strcmp_P(res->arg1, PSTR("harvest"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_harvest(side); + } + else if (!strcmp_P(res->arg1, PSTR("pack"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_pack(side); + } } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index aed92c7..6997139 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -272,7 +272,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) goto error; /* status */ - //cobboard.mode = ans->mode; + cobboard.mode = ans->mode; cobboard.status = ans->status; cobboard.left_cobroller_speed = ans->left_cobroller_speed; cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed); @@ -354,7 +354,8 @@ static int8_t i2c_req_cobboard_status(void) int8_t err; buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS; - buf.mode = cobboard.mode; + buf.lspickle = cobboard.lspickle; + buf.rspickle = cobboard.rspickle; err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); @@ -391,78 +392,42 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); } -static int8_t i2c_cobboard_set_mode(uint8_t mode) +int8_t i2c_cobboard_set_mode(uint8_t mode) { #ifdef HOST_VERSION return robotsim_i2c_cobboard_set_mode(mode); #else - cobboard.mode = mode; - return 0; -#endif - -#if 0 /* old */ struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; + buf.mode = mode; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); #endif } -int8_t i2c_cobboard_mode_eject(void) +static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags) { - /* XXXXXXXXX bad bad bad */ - uint8_t mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; - i2c_cobboard_set_mode(mode); - time_wait_ms(500); - mode = cobboard.mode & (~I2C_COBBOARD_MODE_EJECT); - i2c_cobboard_set_mode(mode); + if (side == I2C_LEFT_SIDE) + cobboard.lspickle = flags; + else + cobboard.rspickle = flags; return 0; } -int8_t i2c_cobboard_mode_harvest(uint8_t side) -{ - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) { - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - mode |= I2C_COBBOARD_MODE_L_HARVEST; - } - else { - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - mode |= I2C_COBBOARD_MODE_R_HARVEST; - } - return i2c_cobboard_set_mode(mode); -} - -int8_t i2c_cobboard_mode_deploy(uint8_t side) +int8_t i2c_cobboard_pack(uint8_t side) { - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) { - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - } - else { - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - } - return i2c_cobboard_set_mode(mode); + return i2c_cobboard_set_spickle(side, 0); } -int8_t i2c_cobboard_mode_pack(uint8_t side) +int8_t i2c_cobboard_harvest(uint8_t side) { - uint8_t mode = cobboard.mode; - - if (side == I2C_LEFT_SIDE) - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - else - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - return i2c_cobboard_set_mode(mode); + return i2c_cobboard_set_spickle(side, + I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST); } -int8_t i2c_cobboard_mode_init(void) +int8_t i2c_cobboard_deploy(uint8_t side) { - return i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); + return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY); } int8_t i2c_ballboard_set_mode(uint8_t mode) diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h index 7ae7283..20c8ada 100644 --- a/projects/microb2010/mainboard/i2c_protocol.h +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -35,12 +35,10 @@ void i2c_sendevent(int8_t size); int8_t i2c_set_color(uint8_t addr, uint8_t color); int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state); -int8_t i2c_cobboard_mode_init(void); -int8_t i2c_cobboard_mode_eject(void); -int8_t i2c_cobboard_mode_harvest(uint8_t side); -int8_t i2c_cobboard_mode_deploy(uint8_t side); -int8_t i2c_cobboard_mode_pack(uint8_t side); - +int8_t i2c_cobboard_set_mode(uint8_t mode); +int8_t i2c_cobboard_pack(uint8_t side); +int8_t i2c_cobboard_harvest(uint8_t side); +int8_t i2c_cobboard_deploy(uint8_t side); int8_t i2c_ballboard_set_mode(uint8_t mode); #endif diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index ff86dd2..4fa3dd8 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -196,6 +196,8 @@ struct mainboard { struct cobboard { uint8_t mode; uint8_t status; + uint8_t lspickle; + uint8_t rspickle; int16_t left_cobroller_speed; int16_t right_cobroller_speed; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index a5a952c..df80b79 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -110,8 +110,9 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_harvest(I2C_LEFT_SIDE); + i2c_cobboard_harvest(I2C_RIGHT_SIDE); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* used in strat_base for END_TIMER */ @@ -198,15 +199,15 @@ static uint8_t strat_beginning(void) /* half turn */ trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_pack(I2C_RIGHT_SIDE); trajectory_a_rel(&mainboard.traj, COLOR_A(180)); err = wait_traj_end(END_INTR|END_TRAJ); /* cob ejection */ trajectory_d_rel(&mainboard.traj, -100); err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_eject(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); time_wait_ms(2000); trajectory_hardstop(&mainboard.traj); -- 2.20.1 From b28db6087ed5fdab0bda8f23417ef7098d2a9ae5 Mon Sep 17 00:00:00 2001 From: zer0 Date: Fri, 23 Apr 2010 23:52:48 +0200 Subject: [PATCH 15/16] high level strat --- projects/microb2010/mainboard/strat.c | 57 ++++++++++++++++++++- projects/microb2010/mainboard/strat_utils.c | 42 +++++++++------ projects/microb2010/mainboard/strat_utils.h | 3 ++ 3 files changed, 85 insertions(+), 17 deletions(-) diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index df80b79..e08c4af 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -224,12 +224,67 @@ static uint8_t strat_beginning(void) } while (0) +/* return true if we need to grab some more elements */ +static uint8_t need_more_elements(void) +{ + if (time_get_s() <= 75) { + /* we have enough time left */ + if (get_ball_count() >= 4) + return 0; + if (get_cob_count() >= 4) + return 0; + if ((get_ball_count() >= 2) && + (get_cob_count() >= 2)) + return 0; + return 1; + } + else { + /* not much time remaining */ + if ((get_ball_count() >= 1) && + (get_cob_count() >= 1)) + return 0; + return 1; + } +} + +static uint8_t strat_harvest(void) +{ + return 0; +} + +static uint8_t strat_eject(void) +{ + return 0; +} + uint8_t strat_main(void) { uint8_t err; - /* */ + /* harvest the first cobs + balls */ err = strat_beginning(); + while (1) { + /* end of time exit ! */ + if (err == END_TIMER) { + DEBUG(E_USER_STRAT, "End of time"); + strat_exit(); + break; + } + + if (need_more_elements() == 0) { + /* we have enough elements, go to eject */ + err = strat_eject(); + if (!TRAJ_SUCCESS(err)) + continue; + } + else { + /* harvest */ + err = strat_harvest(); + if (!TRAJ_SUCCESS(err)) + continue; + } + } + return err; } diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 708f08a..41def2c 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -75,7 +75,7 @@ int16_t distance_from_robot(int16_t x, int16_t y) position_get_y_s16(&mainboard.pos), x, y); } -/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ +/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ int16_t simple_modulo_360(int16_t a) { if (a < -180) { @@ -93,10 +93,10 @@ int16_t angle_abs_to_rel(int16_t a_abs) return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos)); } -void rel_da_to_abs_xy(double d_rel, double a_rel_rad, +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs) { - double x = position_get_x_double(&mainboard.pos); + double x = position_get_x_double(&mainboard.pos); double y = position_get_y_double(&mainboard.pos); double a = position_get_a_rad_double(&mainboard.pos); @@ -109,7 +109,7 @@ double norm(double x, double y) return sqrt(x*x + y*y); } -void rel_xy_to_abs_xy(double x_rel, double y_rel, +void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs) { double d_rel, a_rel; @@ -119,13 +119,13 @@ void rel_xy_to_abs_xy(double x_rel, double y_rel, } /* return an angle between -pi and pi */ -void abs_xy_to_rel_da(double x_abs, double y_abs, +void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad) { - double x = position_get_x_double(&mainboard.pos); + double x = position_get_x_double(&mainboard.pos); double y = position_get_y_double(&mainboard.pos); double a = position_get_a_rad_double(&mainboard.pos); - + *a_rel_rad = atan2(y_abs - y, x_abs - x) - a; if (*a_rel_rad < -M_PI) { *a_rel_rad += M_2PI; @@ -139,7 +139,7 @@ void abs_xy_to_rel_da(double x_abs, double y_abs, void rotate(double *x, double *y, double rot) { double l, a; - + l = norm(*x, *y); a = atan2(*y, *x); @@ -176,7 +176,7 @@ uint8_t robot_is_in_area(int16_t margin) uint8_t y_is_more_than(int16_t y) { int16_t posy; - + posy = position_get_y_s16(&mainboard.pos); if (mainboard.our_color == I2C_COLOR_YELLOW) { if (posy > y) @@ -197,7 +197,7 @@ uint8_t y_is_more_than(int16_t y) uint8_t x_is_more_than(int16_t x) { int16_t posx; - + posx = position_get_x_s16(&mainboard.pos); if (posx > x) return 1; @@ -228,15 +228,15 @@ int16_t sin_table[] = { int16_t fast_sin(int16_t deg) { deg %= 360; - + if (deg < 0) deg += 360; - if (deg < 90) + if (deg < 90) return sin_table[(deg*16)/90]; - else if (deg < 180) + else if (deg < 180) return sin_table[((180-deg)*16)/90]; - else if (deg < 270) + else if (deg < 270) return -sin_table[((deg-180)*16)/90]; else return -sin_table[((360-deg)*16)/90]; @@ -318,3 +318,13 @@ uint8_t opponent_is_behind(void) /* return 1; */ return 0; } + +uint8_t get_ball_count(void) +{ + return ballboard.ball_count; +} + +uint8_t get_cob_count(void) +{ + return cobboard.cob_count; +} diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index 7502a43..d6bea8f 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -58,3 +58,6 @@ 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 opponent_is_behind(void); + +uint8_t get_ball_count(void); +uint8_t get_cob_count(void); -- 2.20.1 From 99c22f682c6ddb6d0bcf657ec30aae226d3240f5 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 24 Apr 2010 15:15:25 +0200 Subject: [PATCH 16/16] new servo pos --- projects/microb2010/mainboard/actuator.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c index 777d94a..ed86f33 100644 --- a/projects/microb2010/mainboard/actuator.c +++ b/projects/microb2010/mainboard/actuator.c @@ -58,7 +58,7 @@ void pwm_set_and_save(void *pwm, int32_t val) val = 4095; if (val < -4095) val = -4095; - + if (pwm == LEFT_PWM) mainboard.pwm_l = val; else if (pwm == RIGHT_PWM) @@ -73,16 +73,16 @@ void pwm_set_and_save(void *pwm, int32_t val) void support_balls_deploy(void) { #ifndef HOST_VERSION - pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510); - pwm_ng_set(SUPPORT_BALLS_L_SERVO, 205); + pwm_ng_set(SUPPORT_BALLS_R_SERVO, 560); + pwm_ng_set(SUPPORT_BALLS_L_SERVO, 155); #endif } void support_balls_pack(void) { #ifndef HOST_VERSION - pwm_ng_set(SUPPORT_BALLS_R_SERVO, 250); - pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480); + pwm_ng_set(SUPPORT_BALLS_R_SERVO, 290); + pwm_ng_set(SUPPORT_BALLS_L_SERVO, 430); #endif } -- 2.20.1