From ebfaaedd491e61696cc93b353471be15408d23e4 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 16:30:39 +0200 Subject: [PATCH] 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.39.5