From ff7505c0a9d6b5332db32ae9b23326bc466f0b01 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 5 May 2011 23:11:38 +0200 Subject: digital/io-hub: add base robospierre io-hub program --- digital/io-hub/src/robospierre/Makefile | 24 +++++ digital/io-hub/src/robospierre/README | 24 +++++ digital/io-hub/src/robospierre/avrconfig.h | 107 ++++++++++++++++++++++ digital/io-hub/src/robospierre/bot.h | 37 ++++++++ digital/io-hub/src/robospierre/main.c | 142 +++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/simu.host.c | 61 +++++++++++++ digital/io-hub/src/robospierre/simu.host.h | 34 +++++++ 7 files changed, 429 insertions(+) create mode 100644 digital/io-hub/src/robospierre/Makefile create mode 100644 digital/io-hub/src/robospierre/README create mode 100644 digital/io-hub/src/robospierre/avrconfig.h create mode 100644 digital/io-hub/src/robospierre/bot.h create mode 100644 digital/io-hub/src/robospierre/main.c create mode 100644 digital/io-hub/src/robospierre/simu.host.c create mode 100644 digital/io-hub/src/robospierre/simu.host.h (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile new file mode 100644 index 00000000..768fe9a4 --- /dev/null +++ b/digital/io-hub/src/robospierre/Makefile @@ -0,0 +1,24 @@ +# Base directory of AVR. +BASE = ../../../avr +# Name of the program to build. +PROGS = io_hub +# Sources to compile. +io_hub_SOURCES = main.c \ + twi_master.c asserv.c mimot.c \ + chrono.c timer.avr.c simu.host.c +# Modules needed for IO. +MODULES = proto uart twi utils \ + math/fixed math/geometry +AI_MODULES = twi_master common utils +# Configuration file. +CONFIGFILE = avrconfig.h +AVR_MCU = at90usb1287 +# Optimize for speed. +OPTIMIZE = -O2 +HOST_LIBS = -lm + +vpath %.c $(AI_MODULES:%=../../../ai/src/%) +vpath %.h $(AI_MODULES:%=../../../ai/src/%) +INCLUDES += -I. $(AI_MODULES:%=-I../../../ai/src/%) + +include $(BASE)/make/Makefile.gen diff --git a/digital/io-hub/src/robospierre/README b/digital/io-hub/src/robospierre/README new file mode 100644 index 00000000..eaa65ad4 --- /dev/null +++ b/digital/io-hub/src/robospierre/README @@ -0,0 +1,24 @@ +robospierre - Eurobot 2011 AI. + +AI & IO for the APBTeam Robot Robospierre. + + +Copyright (C) 2011 Nicolas Schodet + +APBTeam: + Web: http://apbteam.org/ + Email: team AT apbteam DOT org + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h new file mode 100644 index 00000000..11635646 --- /dev/null +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -0,0 +1,107 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 16000000 + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 1 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 38400 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL WAIT +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER STDIO +/** Same thing for secondary port. */ +#define AC_UART1_PORT -1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +/* twi - TWI module. */ +/** Driver to implement TWI: HARD, SOFT, or USI. */ +#define AC_TWI_DRIVER HARD +/** Do not use interrupts. */ +#define AC_TWI_NO_INTERRUPT 0 +/** TWI frequency, should really be 100 kHz. */ +#define AC_TWI_FREQ 100000 +/** Enable slave part. */ +#define AC_TWI_SLAVE_ENABLE 0 +/** Enable master part. */ +#define AC_TWI_MASTER_ENABLE 1 +/** Master transfer completion callback, optionally defined by the user, called + * at end of master transfer. */ +#undef AC_TWI_MASTER_DONE +/** Use internal pull up. */ +#define AC_TWI_PULL_UP 0 + +/* io-hub - io/ai board. */ +/** TWI address of the io board. */ +#define AC_IO_TWI_ADDRESS 10 + +#endif /* avrconfig_h */ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h new file mode 100644 index 00000000..fa65ca07 --- /dev/null +++ b/digital/io-hub/src/robospierre/bot.h @@ -0,0 +1,37 @@ +#ifndef bot_h +#define bot_h +/* bot.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Robot specific defines. */ + +/** Scaling factor, millimeter per step. */ +#ifdef HOST +# define BOT_SCALE 0.0395840674352314 +#else +# define BOT_SCALE 0.0415178942124 +#endif + +#endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c new file mode 100644 index 00000000..5a987240 --- /dev/null +++ b/digital/io-hub/src/robospierre/main.c @@ -0,0 +1,142 @@ +/* main.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/utils/utils.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" + +#include "timer.h" +#include "chrono.h" +#include "simu.host.h" + +#include "asserv.h" +#include "mimot.h" +#include "twi_master.h" + +#include "io.h" + +/** Our color. */ +enum team_color_e team_color; + +/** Asserv stats counters. */ +static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; + +/** Main initialisation. */ +static void +main_init (void) +{ + /* Serial port */ + uart0_init (); + /* Enable interrupts */ + sei (); + /* Main timer */ + timer_init (); + /* TWI communications */ + asserv_init (); + mimot_init (); + twi_master_init (); + /* Initialization done. */ + proto_send0 ('z'); +} + +/** Main events management. */ +void +main_event_to_fsm (void) +{ +} + +/** Main (and infinite) loop. */ +static void +main_loop (void) +{ + while (1) + { + /* Wait until next cycle. */ + timer_wait (); + /* Update chrono. */ + chrono_update (); + /* Is match over? */ + if (chrono_is_match_over ()) + { + /* End it and block here indefinitely. */ + chrono_end_match (42); + return; + } + /* Handle commands from UART. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); + /* Only manage events if slaves are synchronised. */ + if (twi_master_sync ()) + main_event_to_fsm (); + /* Send stats if requested. */ + if (main_stats_asserv_ && !--main_stats_asserv_cpt_) + { + /* Get current position */ + position_t cur_pos; + asserv_get_position (&cur_pos); + /* Send stats */ + proto_send3w ('A', cur_pos.v.x, cur_pos.v.y, cur_pos.a); + /* Reset stats counter */ + main_stats_asserv_cpt_ = main_stats_asserv_; + } + } +} + +/** Handle received commands. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset */ + utils_reset (); + break; + /* Stats commands. + * - b: interval between stats. */ + case c ('A', 1): + /* Position stats. */ + main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; + break; + default: + /* Unknown commands */ + proto_send0 ('?'); + return; + } + /* When no error, acknowledge commands */ + proto_send (cmd, size, args); +#undef c +} + +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + main_init (); + main_loop (); + return 0; +} diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c new file mode 100644 index 00000000..76e48edd --- /dev/null +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -0,0 +1,61 @@ +/* simu.host.c - Host simulation. */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "simu.host.h" + +#include "modules/utils/utils.h" +#include "modules/host/host.h" +#include "modules/host/mex.h" +#include "io.h" + +/** Initialise simulation. */ +void +simu_init (void) +{ + const char *mex_instance; + mex_node_connect (); + mex_instance = host_get_instance ("io-hub0", 0); +} + +/** Make a simulation step. */ +void +simu_step (void) +{ +} + +void +timer_init (void) +{ + simu_init (); +} + +uint8_t +timer_wait (void) +{ + mex_node_wait_date (mex_node_date () + 4); + simu_step (); + return 0; +} + diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h new file mode 100644 index 00000000..76326a31 --- /dev/null +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -0,0 +1,34 @@ +#ifndef simu_host_h +#define simu_host_h +/* simu.host.h - Host simulation. */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#ifdef HOST + +#else /* !defined (HOST) */ + +#endif /* !defined (HOST) */ + +#endif /* simu_host_h */ -- cgit v1.2.3 From b4a96cca3985cba6079d1b7a16f30aa43432a9b3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 6 May 2011 01:55:49 +0200 Subject: digital/io-hub: add PWM --- digital/io-hub/src/common/pwm.avr.c | 133 +++++++++++++++++++++++++++++ digital/io-hub/src/common/pwm.h | 60 +++++++++++++ digital/io-hub/src/common/pwm.host.c | 109 +++++++++++++++++++++++ digital/io-hub/src/robospierre/Makefile | 5 +- digital/io-hub/src/robospierre/avrconfig.h | 8 ++ digital/io-hub/src/robospierre/main.c | 21 +++++ 6 files changed, 335 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/common/pwm.avr.c create mode 100644 digital/io-hub/src/common/pwm.h create mode 100644 digital/io-hub/src/common/pwm.host.c (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/common/pwm.avr.c b/digital/io-hub/src/common/pwm.avr.c new file mode 100644 index 00000000..4b6ad896 --- /dev/null +++ b/digital/io-hub/src/common/pwm.avr.c @@ -0,0 +1,133 @@ +/* pwm.avr.c */ +/* io-hub - Modular Input/Output. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pwm.h" + +#include "modules/utils/utils.h" +#include "preproc.h" + +/** PWM context. */ +struct pwm_t +{ + /** Output compare register. */ + volatile uint16_t *ocr; + /** Direction port. */ + volatile uint8_t *dir_port; + /** Direction bit value. */ + uint8_t dir_bv; + /** Remaining time before timer elapse, 0 for disabled. */ + uint16_t time; + /** Value to be used after timer elapse. */ + int16_t value_rest; +}; + +/** Array of PWM contexts. */ +#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \ +{ &PREPROC_PASTE (OCR, timer, pwm), \ + &IO_PORT_ (dir_io_port, dir_io_n), IO_BV_ (dir_io_port, dir_io_n), \ + 0, 0 }, +struct pwm_t pwm[] = { + AC_IOHUB_PWM +}; +#undef PWM + +void +pwm_init (void) +{ + /* Fast PWM, TOP = 0x3ff, with positive logic. + f_IO without prescaler. + Fpwm = f_IO / (prescaler * (1 + TOP)). */ + uint8_t timer_com_1 = 0, timer_com_3 = 0; +#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \ + do { \ + PREPROC_PASTE (timer_com_, timer) |= \ + _BV (PREPROC_PASTE (COM, timer, pwm, 1)); \ + IO_DDR_ (pwm_io_port, pwm_io_n) |= IO_BV_ (pwm_io_port, pwm_io_n); \ + IO_DDR_ (dir_io_port, dir_io_n) |= IO_BV_ (dir_io_port, dir_io_n); \ + } while (0); + AC_IOHUB_PWM +#undef PWM + if (timer_com_1) + { + TCCR1A = timer_com_1 | + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 0, 0, 0, 0, 1, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 1, 0, 0, 1); + } + if (timer_com_3) + { + TCCR3A = timer_com_3 | + regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30, + 0, 0, 0, 0, 0, 0, 1, 1); + TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30, + 0, 0, 0, 0, 1, 0, 0, 1); + } +} + +void +pwm_update (void) +{ + uint8_t i; + for (i = 0; i < UTILS_COUNT (pwm); i++) + { + if (pwm[i].time) + { + pwm[i].time--; + if (pwm[i].time == 0) + pwm_set (i, pwm[i].value_rest); + } + } +} + +void +pwm_set (uint8_t index, int16_t value) +{ + assert (index < UTILS_COUNT (pwm)); + assert (value <= PWM_MAX && value >= -PWM_MAX); + pwm[index].time = 0; + if (value >= 0) + { + *pwm[index].dir_port &= ~pwm[index].dir_bv; + *pwm[index].ocr = value; + } + else + { + *pwm[index].dir_port |= pwm[index].dir_bv; + *pwm[index].ocr = -value; + } +} + +void +pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest) +{ + assert (index < UTILS_COUNT (pwm)); + assert (value <= PWM_MAX && value >= -PWM_MAX); + assert (value_rest <= PWM_MAX && value_rest >= -PWM_MAX); + pwm_set (index, value); + pwm[index].time = time; + pwm[index].value_rest = value_rest; +} + diff --git a/digital/io-hub/src/common/pwm.h b/digital/io-hub/src/common/pwm.h new file mode 100644 index 00000000..aff84b39 --- /dev/null +++ b/digital/io-hub/src/common/pwm.h @@ -0,0 +1,60 @@ +#ifndef pwm_h +#define pwm_h +/* pwm.h */ +/* io-hub - Modular Input/Output. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Handle PWM output with direction. Also handle timed PWM setting to apply + * a PWM value for a given number of update, then switch to a resting value. + * + * AC_IOHUB_PWM should be set to a list of space separated PWM macro call: + * + * PWM (timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) + * + * For example: + * + * PWM (1, A, B, 1, B, 2) => use output compare A on timer 1, PWM output on + * B1, direction on B2. */ + +/** Maximum positive value. */ +#define PWM_MAX 0x3ff + +/** Initialise PWM module. */ +void +pwm_init (void); + +/** Update PWM module (handle timed PWM). */ +void +pwm_update (void); + +/** Set PWM value for given PWM index. */ +void +pwm_set (uint8_t index, int16_t value); + +/** Set PWM value for given PWM index for a given time. After time elapsed, + * use a rest value. */ +void +pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest); + +#endif /* pwm_h */ diff --git a/digital/io-hub/src/common/pwm.host.c b/digital/io-hub/src/common/pwm.host.c new file mode 100644 index 00000000..f528522a --- /dev/null +++ b/digital/io-hub/src/common/pwm.host.c @@ -0,0 +1,109 @@ +/* pwm.host.c */ +/* io-hub - Modular Input/Output. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pwm.h" + +#include "modules/utils/utils.h" +#include "modules/host/host.h" +#include "modules/host/mex.h" + +/** PWM context. */ +struct pwm_t +{ + /** Current value. */ + int16_t value; + /** Remaining time before timer elapse, 0 for disabled. */ + uint16_t time; + /** Value to be used after timer elapse. */ + int16_t value_rest; +}; + +/** Array of PWM contexts. */ +#define PWM(timer, pwm, pwm_io_port, pwm_io_n, dir_io_port, dir_io_n) \ + { 0, 0, 0 }, +struct pwm_t pwm[] = { + AC_IOHUB_PWM +}; +#undef PWM + +/** Message type. */ +uint8_t pwm_mtype; + +/** Message has to be sent. */ +uint8_t pwm_changed; + +void +pwm_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + pwm_mtype = mex_node_reservef ("%s:pwm", mex_instance); +} + +void +pwm_update (void) +{ + uint8_t i; + /* Update timed PWM. */ + for (i = 0; i < UTILS_COUNT (pwm); i++) + { + if (pwm[i].time) + { + pwm[i].time--; + if (pwm[i].time == 0) + pwm_set (i, pwm[i].value_rest); + } + } + /* Send change if needed. */ + if (pwm_changed) + { + mex_msg_t *m = mex_msg_new (pwm_mtype); + for (i = 0; i < UTILS_COUNT (pwm); i++) + mex_msg_push (m, "h", pwm[i].value); + mex_node_send (m); + pwm_changed = 0; + } +} + +void +pwm_set (uint8_t index, int16_t value) +{ + assert (index < UTILS_COUNT (pwm)); + assert (value <= PWM_MAX && value >= -PWM_MAX); + pwm[index].time = 0; + pwm[index].value = value; + pwm_changed = 1; +} + +void +pwm_set_timed (uint8_t index, int16_t value, uint16_t time, int16_t value_rest) +{ + assert (index < UTILS_COUNT (pwm)); + assert (value <= PWM_MAX && value >= -PWM_MAX); + assert (value_rest <= PWM_MAX && value_rest >= -PWM_MAX); + pwm_set (index, value); + pwm[index].time = time; + pwm[index].value_rest = value_rest; +} + diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 768fe9a4..9d0035fe 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,6 +4,7 @@ BASE = ../../../avr PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ + pwm.avr.c pwm.host.c \ twi_master.c asserv.c mimot.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. @@ -17,8 +18,10 @@ AVR_MCU = at90usb1287 OPTIMIZE = -O2 HOST_LIBS = -lm +vpath %.c ../common +vpath %.h ../common vpath %.c $(AI_MODULES:%=../../../ai/src/%) vpath %.h $(AI_MODULES:%=../../../ai/src/%) -INCLUDES += -I. $(AI_MODULES:%=-I../../../ai/src/%) +INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%) include $(BASE)/make/Makefile.gen diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index 11635646..0fc2f11d 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -103,5 +103,13 @@ /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 +/** PWM setting. */ +#define AC_IOHUB_PWM \ + PWM (1, A, B, 5, D, 4) \ + PWM (1, B, B, 6, D, 5) \ + PWM (1, C, B, 7, D, 6) \ + PWM (3, A, C, 6, C, 7) \ + PWM (3, B, C, 5, C, 3) \ + PWM (3, C, C, 4, C, 2) #endif /* avrconfig_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 5a987240..c0937615 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -36,6 +36,8 @@ #include "mimot.h" #include "twi_master.h" +#include "pwm.h" + #include "io.h" /** Our color. */ @@ -58,6 +60,8 @@ main_init (void) asserv_init (); mimot_init (); twi_master_init (); + /* IO modules. */ + pwm_init (); /* Initialization done. */ proto_send0 ('z'); } @@ -88,6 +92,8 @@ main_loop (void) /* Handle commands from UART. */ while (uart0_poll ()) proto_accept (uart0_getc ()); + /* Update IO modules. */ + pwm_update (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync ()) main_event_to_fsm (); @@ -116,6 +122,21 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Reset */ utils_reset (); break; + case c ('w', 4): + /* Set PWM. + * - 1w: index. + * - 1w: value. */ + pwm_set (v8_to_v16 (args[0], args[1]), v8_to_v16 (args[2], args[3])); + break; + case c ('w', 8): + /* Set timed PWM. + * - 1w: index. + * - 1w: value. + * - 1w: time. + * - 1w: rest value. */ + pwm_set_timed (v8_to_v16 (args[0], args[1]), v8_to_v16 (args[2], args[3]), + v8_to_v16 (args[4], args[5]), v8_to_v16 (args[6], args[7])); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): -- cgit v1.2.3 From 8bae07b463b80a6e3b877b01302507e4d1aa335b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 6 May 2011 22:25:04 +0200 Subject: digital/io-hub: PWM index is a byte --- digital/io-hub/src/robospierre/main.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index c0937615..12a35054 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -122,20 +122,21 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Reset */ utils_reset (); break; - case c ('w', 4): + case c ('w', 3): /* Set PWM. - * - 1w: index. + * - 1b: index. * - 1w: value. */ - pwm_set (v8_to_v16 (args[0], args[1]), v8_to_v16 (args[2], args[3])); + pwm_set (args[0], v8_to_v16 (args[1], args[2])); break; - case c ('w', 8): + case c ('w', 7): /* Set timed PWM. - * - 1w: index. + * - 1b: index. * - 1w: value. * - 1w: time. * - 1w: rest value. */ - pwm_set_timed (v8_to_v16 (args[0], args[1]), v8_to_v16 (args[2], args[3]), - v8_to_v16 (args[4], args[5]), v8_to_v16 (args[6], args[7])); + pwm_set_timed (args[0], v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[3], args[4]), + v8_to_v16 (args[5], args[6])); break; /* Stats commands. * - b: interval between stats. */ -- cgit v1.2.3 From 554c4baf249f68bb8f716af97cbf23daad73ef5c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 6 May 2011 23:44:36 +0200 Subject: digital/io-hub: add contacts --- digital/io-hub/src/common/contact.avr.c | 98 +++++++++++++++++++++++++++ digital/io-hub/src/common/contact.h | 55 +++++++++++++++ digital/io-hub/src/common/contact.host.c | 98 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/contact_defs.h | 37 ++++++++++ digital/io-hub/src/robospierre/main.c | 15 ++++ digital/io-hub/src/robospierre/simu.host.c | 3 + digital/io-hub/src/robospierre/simu.host.h | 2 + 8 files changed, 309 insertions(+) create mode 100644 digital/io-hub/src/common/contact.avr.c create mode 100644 digital/io-hub/src/common/contact.h create mode 100644 digital/io-hub/src/common/contact.host.c create mode 100644 digital/io-hub/src/robospierre/contact_defs.h (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/common/contact.avr.c b/digital/io-hub/src/common/contact.avr.c new file mode 100644 index 00000000..9f6869b4 --- /dev/null +++ b/digital/io-hub/src/common/contact.avr.c @@ -0,0 +1,98 @@ +/* contact.avr.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "contact.h" + +/** Filter depth. */ +#define CONTACT_JACK_FILTER_DEPTH 50 + +/** Contact context. */ +struct contact_t +{ + /** Jack current state. */ + uint8_t jack_state; + /** Jack filter, number of updates before state can change. */ + uint8_t jack_filter; +}; + +/** Global context. */ +struct contact_t contact_global; +#define ctx contact_global + +void +contact_init (void) +{ + /* Pull-up. */ +#define CONTACT(io) IO_SET_ (io); + CONTACT (CONTACT_COLOR); + CONTACT (CONTACT_JACK); + CONTACT_LIST +#undef CONTACT + /* Default to jack out. */ + ctx.jack_state = 1; +} + +void +contact_update (void) +{ + uint8_t jack_now = IO_GET (CONTACT_JACK); + /* If state is unchanged, reset filter. */ + if (jack_now == ctx.jack_state) + ctx.jack_filter = 0; + /* If state changed, should be stable for CONTACT_JACK_FILTER_DEPTH before + * it goes through the filter. */ + else if (ctx.jack_filter++ == CONTACT_JACK_FILTER_DEPTH) + { + ctx.jack_state = jack_now; + ctx.jack_filter = 0; + } +} + +enum team_color_e +contact_get_color (void) +{ + return !IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; +} + +uint8_t +contact_get_jack (void) +{ + return ctx.jack_state; +} + +uint32_t +contact_all (void) +{ + uint32_t contacts = 0, bit = 1; +#define CONTACT(io) do { \ + contacts |= (IO_GET_ (io) ? bit : 0); \ + bit <<= 1; \ +} while (0); + CONTACT (CONTACT_COLOR); + CONTACT (CONTACT_JACK); + CONTACT_LIST +#undef CONTACT + return contacts; +} diff --git a/digital/io-hub/src/common/contact.h b/digital/io-hub/src/common/contact.h new file mode 100644 index 00000000..fce06f84 --- /dev/null +++ b/digital/io-hub/src/common/contact.h @@ -0,0 +1,55 @@ +#ifndef contact_h +#define contact_h +/* contact.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "simu.host.h" +#include "defs.h" +#include "io.h" + +#include "contact_defs.h" + +/** To get a state of a generic contact, use: IO_GET (CONTACT_EXAMPLE). */ + +/** Initialize contact module. */ +void +contact_init (void); + +/** Update contact module. This handle debouncing. */ +void +contact_update (void); + +/** Get color switch state, unfiltered. */ +enum team_color_e +contact_get_color (void); + +/** Get filtered jack state. */ +uint8_t +contact_get_jack (void); + +/** Return state of all contact, unfiltered. */ +uint32_t +contact_all (void); + +#endif /* contact_h */ diff --git a/digital/io-hub/src/common/contact.host.c b/digital/io-hub/src/common/contact.host.c new file mode 100644 index 00000000..5e2309c4 --- /dev/null +++ b/digital/io-hub/src/common/contact.host.c @@ -0,0 +1,98 @@ +/* contact.host.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "contact.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +/** Contact context. */ +struct contact_t +{ + /** All contact state. */ + uint32_t all; + /** Jack current state. */ + uint8_t jack_state; + /** Color current state. */ + enum team_color_e color_state; +}; + +/** Global context. */ +struct contact_t contact_global; +#define ctx contact_global + +static void +contact_handle (void *user, mex_msg_t *msg) +{ + uint32_t contacts; + mex_msg_pop (msg, "L", &contacts); + ctx.all = contacts; + ctx.color_state = !(contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; + ctx.jack_state = (contacts & 2) ? 1 : 0; + contacts >>= 2; +#define CONTACT(io) do { \ + if (contacts & 1) \ + IO_PIN_ (io) |= IO_BV_ (io); \ + else \ + IO_PIN_ (io) &= ~IO_BV_ (io); \ + contacts >>= 1; \ +} while (0); + CONTACT_LIST +#undef CONTACT +} + +void +contact_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:contact", mex_instance); + mex_node_register (mtype, contact_handle, 0); + /* Default to jack out. */ + ctx.jack_state = 1; +} + +void +contact_update (void) +{ + /* Nothing to do. */ +} + +enum team_color_e +contact_get_color (void) +{ + return ctx.color_state; +} + +uint8_t +contact_get_jack (void) +{ + return ctx.jack_state; +} + +uint32_t +contact_all (void) +{ + return ctx.all; +} diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 9d0035fe..a95d7586 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,6 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ pwm.avr.c pwm.host.c \ + contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h new file mode 100644 index 00000000..55b34354 --- /dev/null +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -0,0 +1,37 @@ +#ifndef contact_defs_h +#define contact_defs_h +/* contact_defs.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#define CONTACT_COLOR A, 7 +#define CONTACT_JACK F, 7 +#define CONTACT_EX1 E, 0 +#define CONTACT_EX2 E, 1 + +#define CONTACT_LIST \ + CONTACT (CONTACT_EX1) \ + CONTACT (CONTACT_EX2) + +#endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 12a35054..bbc9bdb9 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -37,6 +37,7 @@ #include "twi_master.h" #include "pwm.h" +#include "contact.h" #include "io.h" @@ -46,6 +47,9 @@ enum team_color_e team_color; /** Asserv stats counters. */ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; +/** Contact stats counters. */ +static uint8_t main_stats_contact_, main_stats_contact_cpt_; + /** Main initialisation. */ static void main_init (void) @@ -62,6 +66,7 @@ main_init (void) twi_master_init (); /* IO modules. */ pwm_init (); + contact_init (); /* Initialization done. */ proto_send0 ('z'); } @@ -94,6 +99,7 @@ main_loop (void) proto_accept (uart0_getc ()); /* Update IO modules. */ pwm_update (); + contact_update (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync ()) main_event_to_fsm (); @@ -108,6 +114,11 @@ main_loop (void) /* Reset stats counter */ main_stats_asserv_cpt_ = main_stats_asserv_; } + if (main_stats_contact_ && !--main_stats_contact_cpt_) + { + proto_send1d ('P', contact_all ()); + main_stats_contact_cpt_ = main_stats_contact_; + } } } @@ -144,6 +155,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Position stats. */ main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; break; + case c ('P', 1): + /* Contact stats. */ + main_stats_contact_ = main_stats_contact_cpt_ = args[0]; + break; default: /* Unknown commands */ proto_send0 ('?'); diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 76e48edd..8f4019aa 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -30,6 +30,9 @@ #include "modules/host/mex.h" #include "io.h" +/** AVR registers. */ +uint8_t PINE; + /** Initialise simulation. */ void simu_init (void) diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 76326a31..4b461e86 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,6 +27,8 @@ #ifdef HOST +extern uint8_t PINE; + #else /* !defined (HOST) */ #endif /* !defined (HOST) */ -- cgit v1.2.3 From 8ed9fb23f3ab93374d9bcbe6560e84c6c1de1b2a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 7 May 2011 00:30:10 +0200 Subject: digital/{ai,io-hub}, host/simu: use io-hub for simu --- digital/ai/tools/robospierre.py | 10 +++++----- digital/ai/tools/test_simu_control_robospierre.py | 4 ++-- digital/io-hub/src/robospierre/main.c | 1 + host/simu/robots/robospierre/link/bag.py | 4 ++-- host/simu/robots/robospierre/model/bag.py | 23 +++++++++++------------ host/simu/robots/robospierre/view/bag.py | 2 -- 6 files changed, 21 insertions(+), 23 deletions(-) (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 23fb19a3..59023184 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -9,8 +9,8 @@ import asserv import asserv.init import mimot import mimot.init -import io -import io.init +import io_hub +import io_hub.init from proto.popen_io import PopenIO import math @@ -25,13 +25,13 @@ class Robot: self.robot_view = simu.robots.robospierre.view.bag asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel') mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') - io_cmd = ('../../io/src/io.host') + io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, **asserv.init.host) self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time, **mimot.init.host) - self.io = io.Proto (PopenIO (io_cmd), proto_time, - **io.init.host) + self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, + **io_hub.init.host) self.robot_start_pos = { False: (700, 2100 - 250, math.radians (-270)), True: (3000 - 700, 2100 - 250, math.radians (-270)) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 9da57196..5300918a 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -78,9 +78,9 @@ class TestSimuControl (TestSimu): def clamp_command (self): if self.clamp_var.get (): - self.io.pwm_set (-0x7fff, 255) + self.io.pwm_set_timed (0, -0x3ff, 255, 0) else: - self.io.pwm_set (0x7fff, 255) + self.io.pwm_set_timed (0, 0x3ff, 255, 0) def elevation_up_command (self): self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index bbc9bdb9..980ab138 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -60,6 +60,7 @@ main_init (void) sei (); /* Main timer */ timer_init (); + timer_wait (); /* TWI communications */ asserv_init (); mimot_init (); diff --git a/host/simu/robots/robospierre/link/bag.py b/host/simu/robots/robospierre/link/bag.py index 2760aec0..ac68889a 100644 --- a/host/simu/robots/robospierre/link/bag.py +++ b/host/simu/robots/robospierre/link/bag.py @@ -22,7 +22,7 @@ # # }}} """Robospierre bag of links.""" -import io.mex +import io_hub.mex import asserv.mex import mimot.mex @@ -30,6 +30,6 @@ class Bag: def __init__ (self, node): self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) + self.io_hub = io_hub.mex.Mex (node) self.mimot = mimot.mex.Mex (node) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index e6810e18..ce55f997 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -32,26 +32,25 @@ from math import pi class Bag: def __init__ (self, scheduler, table, link_bag): - self.jack = Switch (link_bag.io.jack) - self.color_switch = Switch (link_bag.io.color_switch) - self.contact = [ Switch (contact) for contact in link_bag.io.contact ] + self.color_switch = Switch (link_bag.io_hub.contact[0]) + self.jack = Switch (link_bag.io_hub.contact[1]) + self.contact = [ Switch (contact) + for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) - self.clamping_motor = MotorBasic (link_bag.io.pwm[0], scheduler, + self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[0], scheduler, 2 * pi, 0, pi) self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], link_bag.mimot.aux[1], self.clamping_motor) self.distance_sensor = [ - DistanceSensorSensopart (link_bag.io.adc[0], scheduler, table, + DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, -20), -pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io.adc[1], scheduler, table, + DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io.adc[2], scheduler, table, + DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io.adc[3], scheduler, table, + DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, -20), pi + pi * 10 / 180, (self.position, ), 2), ] - link_bag.io.adc[4].value = 0 - link_bag.io.adc[5].value = 0 - self.path = link_bag.io.path - self.pos_report = link_bag.io.pos_report + for adc in link_bag.io_hub.adc[4:]: + adc.value = 0 diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 361c79d3..0b5a85ad 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -43,6 +43,4 @@ class Bag: ClampSide.height), model_bag.clamp)) self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] - self.path = Path (table, model_bag.path) - self.pos_report = PosReport (table, model_bag.pos_report) -- cgit v1.2.3 From d385a9eacb2e8580239f101f96356f7ed216dd97 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 8 May 2011 13:02:53 +0200 Subject: digital/io-hub: add clamp moving --- digital/ai/tools/test_simu_control_robospierre.py | 10 ++ digital/io-hub/src/robospierre/Makefile | 13 +- digital/io-hub/src/robospierre/bot.h | 39 +++++ digital/io-hub/src/robospierre/clamp.c | 182 ++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 57 +++++++ digital/io-hub/src/robospierre/main.c | 45 ++++++ digital/io-hub/tools/io_hub/io_hub.py | 3 + 7 files changed, 348 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/robospierre/clamp.c create mode 100644 digital/io-hub/src/robospierre/clamp.h (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 5300918a..b1cccf4b 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -62,6 +62,13 @@ class TestSimuControl (TestSimu): text = 'Rotation ccw', padx = 0, pady = 0, command = self.rotation_ccw_command) self.rotation_ccw_button.pack () + self.clamp_pos_scale = Scale (self.control_frame, orient = HORIZONTAL, + from_ = 0, to = 6) + self.clamp_pos_scale.pack () + self.clamp_move_button = Button (self.control_frame, + text = 'Move clamp', padx = 0, pady = 0, + command = self.clamp_move_command) + self.clamp_move_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) @@ -94,6 +101,9 @@ class TestSimuControl (TestSimu): def rotation_ccw_command (self): self.mimot.speed_pos ('a1', -self.ROTATION_STROKE / 2) + def clamp_move_command (self): + self.io.clamp_move (self.clamp_pos_scale.get ()) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index a95d7586..21411631 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,6 +4,8 @@ BASE = ../../../avr PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ + clamp.c \ + fsm.host.c fsm_AI_gen.avr.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ @@ -11,7 +13,7 @@ io_hub_SOURCES = main.c \ # Modules needed for IO. MODULES = proto uart twi utils \ math/fixed math/geometry -AI_MODULES = twi_master common utils +AI_MODULES = twi_master common utils fsm # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 @@ -25,4 +27,13 @@ vpath %.c $(AI_MODULES:%=../../../ai/src/%) vpath %.h $(AI_MODULES:%=../../../ai/src/%) INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%) +EXTRA_CLEAN_FILES = fsm_AI_gen.h fsm_AI_gen.avr.c + include $(BASE)/make/Makefile.gen + +# FSM generation. +obj/main.avr.o: fsm_AI_gen.h +fsm_AI_gen.avr.c: fsm_AI_gen.h +fsm_AI_gen.h: io_hub.host + ./$< --gen + mv fsm_AI_gen.c fsm_AI_gen.avr.c diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index fa65ca07..9043a62c 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,4 +34,43 @@ # define BOT_SCALE 0.0415178942124 #endif +#ifdef HOST + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2) + +#else /* !HOST */ + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820 + +#endif /* !HOST */ + +#define BOT_CLAMP_ELEVATION_SPEED 0x60 +#define BOT_CLAMP_ROTATION_SPEED 0x30 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c new file mode 100644 index 00000000..2f1b2471 --- /dev/null +++ b/digital/io-hub/src/robospierre/clamp.c @@ -0,0 +1,182 @@ +/* clamp.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "clamp.h" + +#define FSM_NAME AI +#include "fsm.h" + +#include "mimot.h" +#include "bot.h" + +FSM_INIT + +FSM_STATES ( + /* Wait order. */ + CLAMP_IDLE, + /* Moving to a final or intermediary position. */ + CLAMP_ROUTING) + +FSM_EVENTS ( + /* Order to move the clamp. */ + clamp_move, + /* Elevation and elevation motor success. */ + clamp_elevation_rotation_success, + /* Elevation motor failure. */ + clamp_elevation_failure, + /* Rotation motor failure. */ + clamp_rotation_failure) + +FSM_START_WITH (CLAMP_IDLE) + +/** Clamp context. */ +struct clamp_t +{ + /* Current position. */ + uint8_t pos_current; + /* Requested position. */ + uint8_t pos_request; +}; + +/** Global context. */ +struct clamp_t clamp_global; +#define ctx clamp_global + +/** Clamp positions. */ +static const uint16_t clamp_pos[][2] = { + { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, + { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, +}; + +void +clamp_move (uint8_t pos) +{ + if (pos != ctx.pos_current) + { + ctx.pos_request = pos; + FSM_HANDLE (AI, clamp_move); + } +} + +/** Find next position and start motors. */ +static void +clamp_route (void) +{ + uint8_t pos_new; + uint8_t pos_current = ctx.pos_current; + uint8_t pos_request = ctx.pos_request; + /* Compute new position. */ + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) + { + if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = CLAMP_BAY_FRONT_LEAVE; + else + pos_new = pos_request; + } + else if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_current)) + { + if (!CLAMP_IS_SLOT_IN_BACK_BAY (pos_request)) + pos_new = CLAMP_BAY_BACK_LEAVE; + else + pos_new = pos_request; + } + else if (pos_current == CLAMP_SLOT_SIDE) + { + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + } + else if (pos_current == CLAMP_BAY_FRONT_LEAVE) + { + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + else + pos_new = CLAMP_SLOT_BACK_MIDDLE; + } + else if (pos_current == CLAMP_BAY_BACK_LEAVE) + { + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + else + pos_new = CLAMP_SLOT_FRONT_MIDDLE; + } + else + { + assert (pos_current == CLAMP_BAY_SIDE_ENTER_LEAVE); + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = pos_request; + else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = CLAMP_SLOT_FRONT_MIDDLE; + else + pos_new = CLAMP_SLOT_BACK_MIDDLE; + } + /* Run motors. */ + mimot_move_motor0_absolute (clamp_pos[pos_new][0], + BOT_CLAMP_ELEVATION_SPEED); + mimot_move_motor1_absolute (clamp_pos[pos_new][1], + BOT_CLAMP_ROTATION_SPEED); + /* Remember new position. */ + ctx.pos_current = pos_new; +} + +FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING) +{ + clamp_route (); + return FSM_NEXT (CLAMP_IDLE, clamp_move); +} + +FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_IDLE, + next, CLAMP_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + done); + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + next); + } +} + diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h new file mode 100644 index 00000000..104e3cb8 --- /dev/null +++ b/digital/io-hub/src/robospierre/clamp.h @@ -0,0 +1,57 @@ +#ifndef clamp_h +#define clamp_h +/* clamp.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +enum { + /** Slot positions. */ + CLAMP_SLOT_FRONT_BOTTOM, + CLAMP_SLOT_FRONT_MIDDLE, + CLAMP_SLOT_FRONT_TOP, + CLAMP_SLOT_BACK_BOTTOM, + CLAMP_SLOT_BACK_MIDDLE, + CLAMP_SLOT_BACK_TOP, + CLAMP_SLOT_SIDE, + /** Leave the front bay, ready to enter side tunnel. */ + CLAMP_BAY_FRONT_LEAVE, + /** Leave the back bay, ready to enter side tunnel. */ + CLAMP_BAY_BACK_LEAVE, + /* Enter the side bay. Position on the side, above wheels. */ + CLAMP_BAY_SIDE_ENTER_LEAVE, +}; + +/** Is slot in front bay? */ +#define CLAMP_IS_SLOT_IN_FRONT_BAY(slot) \ + ((slot) <= CLAMP_SLOT_FRONT_TOP) + +/** Is slot in back bay? */ +#define CLAMP_IS_SLOT_IN_BACK_BAY(slot) \ + ((slot) >= CLAMP_SLOT_BACK_BOTTOM && (slot) <= CLAMP_SLOT_BACK_TOP) + +/** Move clamp to given position. */ +void +clamp_move (uint8_t pos); + +#endif /* clamp_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 980ab138..3a32a1ae 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -39,6 +39,14 @@ #include "pwm.h" #include "contact.h" +#define FSM_NAME AI +#include "fsm.h" +#ifdef HOST +# include +#endif + +#include "clamp.h" + #include "io.h" /** Our color. */ @@ -76,6 +84,27 @@ main_init (void) void main_event_to_fsm (void) { + /* If an event is handled, stop generating any other event, because a + * transition may have invalidated the current robot state. */ +#define FSM_HANDLE_E(fsm, event) \ + do { if (FSM_HANDLE (fsm, event)) return; } while (0) +#define FSM_HANDLE_VAR_E(fsm, event) \ + do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0) +#define FSM_HANDLE_TIMEOUT_E(fsm) \ + do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) + /* Update FSM timeouts. */ + //FSM_HANDLE_TIMEOUT_E (AI); + /* Motor status. */ + asserv_status_e mimot_motor0_status, mimot_motor1_status; + mimot_motor0_status = mimot_motor0_cmd_status (); + mimot_motor1_status = mimot_motor1_cmd_status (); + if (mimot_motor0_status == success + && mimot_motor1_status == success) + FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + else if (mimot_motor0_status == failure) + FSM_HANDLE_E (AI, clamp_elevation_failure); + else if (mimot_motor1_status == failure) + FSM_HANDLE_E (AI, clamp_rotation_failure); } /** Main (and infinite) loop. */ @@ -150,6 +179,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v16 (args[3], args[4]), v8_to_v16 (args[5], args[6])); break; + case c ('c', 1): + /* Move clamp. + * - 1b: position. */ + clamp_move (args[0]); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): @@ -173,6 +207,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) int main (int argc, char **argv) { +#ifdef HOST + /* Produce AVR's FSM headers. */ + int i; + if (argc > 1) + for (i = 1; i < argc; i++) + if (strcmp (argv[i], "--gen") == 0) + { + FSM_GENERATE (AVR, 0); + return 0; + } +#endif avr_init (argc, argv); main_init (); main_loop (); diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index 36e5e4df..a363e5f6 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -44,6 +44,9 @@ class Proto: def pwm_set_timed (self, index, value, time, rest_value): self.proto.send ('w', 'BhHh', index, value, time, rest_value) + def clamp_move (self, pos): + self.proto.send ('c', 'B', pos) + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3