From 17cd185559320a20f09ccb3cd7743b96b79dcc4c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 4 Apr 2008 20:29:50 +0200 Subject: * digital/io/src, host/inter: - added host support to io. --- digital/io/src/Makefile | 5 +- digital/io/src/avrconfig.h | 2 +- digital/io/src/eeprom.avr.c | 121 +++++++++++++++++++++++ digital/io/src/eeprom.c | 121 ----------------------- digital/io/src/gutter_cb.c | 2 +- digital/io/src/main.c | 14 ++- digital/io/src/servo.avr.c | 201 ++++++++++++++++++++++++++++++++++++++ digital/io/src/servo.c | 201 -------------------------------------- digital/io/src/simu.host.c | 153 +++++++++++++++++++++++++++++ digital/io/src/simu.host.h | 52 ++++++++++ digital/io/src/test/gutter/main.c | 2 +- 11 files changed, 546 insertions(+), 328 deletions(-) create mode 100644 digital/io/src/eeprom.avr.c delete mode 100644 digital/io/src/eeprom.c create mode 100644 digital/io/src/servo.avr.c delete mode 100644 digital/io/src/servo.c create mode 100644 digital/io/src/simu.host.c create mode 100644 digital/io/src/simu.host.h (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index e263479b..7a82e78d 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -1,6 +1,7 @@ BASE = ../../avr -AVR_PROGS = io -io_SOURCES = main.c asserv.c servo.c eeprom.c trap.c sharp.c \ +PROGS = io +io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \ + simu.host.c \ fsm.c getsamples.c getsamples_fsm.c getsamples_cb.c \ gutter_fsm.c gutter_cb.c MODULES = proto uart twi utils adc diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index e7555acb..27a550c0 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -59,7 +59,7 @@ /** In HOST compilation: * - STDIO: use stdin/out. * - PTS: use pseudo terminal. */ -#define AC_UART0_HOST_DRIVER PTS +#define AC_UART0_HOST_DRIVER STDIO /** Same thing for secondary port. */ #define AC_UART1_PORT -1 #define AC_UART1_BAUDRATE 115200 diff --git a/digital/io/src/eeprom.avr.c b/digital/io/src/eeprom.avr.c new file mode 100644 index 00000000..b16130c6 --- /dev/null +++ b/digital/io/src/eeprom.avr.c @@ -0,0 +1,121 @@ +/* eeprom.avr.c */ +/* 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. + * + * }}} */ + +#include "common.h" +#include "eeprom.h" + +#include "servo.h" /* SERVO_NUMBER */ +#include "trap.h" /* trap_high_time_pos */ +#include "sharp.h" /* sharp_threshold */ + +#include /* eeprom_{read,write}_byte */ + +/** + * @defgroup EepromPrivate EEPROM module private variables and functions + * declarations + * @{ + */ + +/** + * What is the address, in the EEPROM, where are stored the parameters values? + */ +#define EEPROM_PARAM_START 0 + +/** + * Current version of the parameters organization. + */ +#define EEPROM_PARAM_KEY 0x03 + +/** @} */ + +/* Load parameters from the EEPROM. */ +void +eeprom_load_param () +{ + uint8_t compt; + uint16_t *ptr16; + + /* The parameters start at the given address */ + uint8_t *ptr8 = (uint8_t *) EEPROM_PARAM_START; + /* Check if the key/version is correct */ + if (eeprom_read_byte (ptr8++) != EEPROM_PARAM_KEY) + /* Error, stop here */ + return; + + /* Load trap module data */ + /* Extreme position of the servos motor (high time value of the PWM) */ + for (compt = 0; compt < SERVO_NUMBER; compt++) + { + trap_high_time_pos[0][compt] = eeprom_read_byte(ptr8++); + trap_high_time_pos[1][compt] = eeprom_read_byte(ptr8++); + } + + /* Load sharp module data */ + ptr16 = (uint16_t *) ptr8; + /* Threshold values, high and low */ + for (compt = 0; compt < SHARP_NUMBER; compt++) + { + sharp_threshold[compt][0] = eeprom_read_word (ptr16++); + sharp_threshold[compt][1] = eeprom_read_word (ptr16++); + } +} + +/* Store parameters in the EEPROM. */ +void +eeprom_save_param () +{ + uint8_t compt; + uint16_t *ptr16; + + /* The parameters start at the given address */ + uint8_t *ptr8 = (uint8_t *) EEPROM_PARAM_START; + /* Store the key */ + eeprom_write_byte (ptr8++, EEPROM_PARAM_KEY); + + /* Store trap module data */ + /* Extreme position of the servos motor (high time value of the PWM) */ + for (compt = 0; compt < SERVO_NUMBER; compt++) + { + eeprom_write_byte (ptr8++, trap_high_time_pos[0][compt]); + eeprom_write_byte (ptr8++, trap_high_time_pos[1][compt]); + } + + /* Store sharp module data */ + ptr16 = (uint16_t *) ptr8; + /* Threshold values, high and low */ + for (compt = 0; compt < SHARP_NUMBER; compt++) + { + eeprom_write_word (ptr16++, sharp_threshold[compt][0]); + eeprom_write_word (ptr16++, sharp_threshold[compt][1]); + } +} + +/* Clear parameters in the EEPROM by invalidating the key. */ +void +eeprom_clear_param () +{ + /* Write an invalid key */ + eeprom_write_byte ((uint8_t *) EEPROM_PARAM_START, 0xFF); +} diff --git a/digital/io/src/eeprom.c b/digital/io/src/eeprom.c deleted file mode 100644 index c1685a09..00000000 --- a/digital/io/src/eeprom.c +++ /dev/null @@ -1,121 +0,0 @@ -/* eeprom.c */ -/* 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. - * - * }}} */ - -#include "common.h" -#include "eeprom.h" - -#include "servo.h" /* SERVO_NUMBER */ -#include "trap.h" /* trap_high_time_pos */ -#include "sharp.h" /* sharp_threshold */ - -#include /* eeprom_{read,write}_byte */ - -/** - * @defgroup EepromPrivate EEPROM module private variables and functions - * declarations - * @{ - */ - -/** - * What is the address, in the EEPROM, where are stored the parameters values? - */ -#define EEPROM_PARAM_START 0 - -/** - * Current version of the parameters organization. - */ -#define EEPROM_PARAM_KEY 0x03 - -/** @} */ - -/* Load parameters from the EEPROM. */ -void -eeprom_load_param () -{ - uint8_t compt; - uint16_t *ptr16; - - /* The parameters start at the given address */ - uint8_t *ptr8 = (uint8_t *) EEPROM_PARAM_START; - /* Check if the key/version is correct */ - if (eeprom_read_byte (ptr8++) != EEPROM_PARAM_KEY) - /* Error, stop here */ - return; - - /* Load trap module data */ - /* Extreme position of the servos motor (high time value of the PWM) */ - for (compt = 0; compt < SERVO_NUMBER; compt++) - { - trap_high_time_pos[0][compt] = eeprom_read_byte(ptr8++); - trap_high_time_pos[1][compt] = eeprom_read_byte(ptr8++); - } - - /* Load sharp module data */ - ptr16 = (uint16_t *) ptr8; - /* Threshold values, high and low */ - for (compt = 0; compt < SHARP_NUMBER; compt++) - { - sharp_threshold[compt][0] = eeprom_read_word (ptr16++); - sharp_threshold[compt][1] = eeprom_read_word (ptr16++); - } -} - -/* Store parameters in the EEPROM. */ -void -eeprom_save_param () -{ - uint8_t compt; - uint16_t *ptr16; - - /* The parameters start at the given address */ - uint8_t *ptr8 = (uint8_t *) EEPROM_PARAM_START; - /* Store the key */ - eeprom_write_byte (ptr8++, EEPROM_PARAM_KEY); - - /* Store trap module data */ - /* Extreme position of the servos motor (high time value of the PWM) */ - for (compt = 0; compt < SERVO_NUMBER; compt++) - { - eeprom_write_byte (ptr8++, trap_high_time_pos[0][compt]); - eeprom_write_byte (ptr8++, trap_high_time_pos[1][compt]); - } - - /* Store sharp module data */ - ptr16 = (uint16_t *) ptr8; - /* Threshold values, high and low */ - for (compt = 0; compt < SHARP_NUMBER; compt++) - { - eeprom_write_word (ptr16++, sharp_threshold[compt][0]); - eeprom_write_word (ptr16++, sharp_threshold[compt][1]); - } -} - -/* Clear parameters in the EEPROM by invalidating the key. */ -void -eeprom_clear_param () -{ - /* Write an invalid key */ - eeprom_write_byte ((uint8_t *) EEPROM_PARAM_START, 0xFF); -} diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 30166929..336ae404 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -37,7 +37,7 @@ fsm_branch_t gutter__START__ok (void) { - asserv_go_to_gutter(); + asserv_go_to_the_wall(); return gutter_next (START, ok); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 7f0fc6eb..bab667e4 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -31,13 +31,17 @@ /* AVR include, non HOST */ #ifndef HOST # include "main_timer.avr.h" +# include "switch.h" /* Manage switches (jack, color selector) */ #endif /* HOST */ +#include "simu.host.h" + #include "asserv.h" /* Functions to control the asserv board */ -#include "switch.h" /* Manage switches (jack, color selector) */ #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ #include "trap.h" /* Trap module (trap_* functions) */ +#include "io.h" + /** * Initialize the main and all its subsystems. */ @@ -135,6 +139,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: pwm high time value (position). */ servo_set_high_time (args[0], args[1]); + break; + + case c ('S', 0): + /* Report switch states. */ + proto_send1b ('S', switch_get_color () << 1 | switch_get_jack ()); + break; /* EEPROM command */ case c ('e', 1): @@ -165,6 +175,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) int main (int argc, char **argv) { + avr_init (argc, argv); + /* Initialize the main and its subsystems */ main_init (); diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c new file mode 100644 index 00000000..c2603c6e --- /dev/null +++ b/digital/io/src/servo.avr.c @@ -0,0 +1,201 @@ +/* servo.avr.c */ +/* 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. + * + * }}} */ + +#include "common.h" +#include "servo.h" + +#include "modules/utils/utils.h" /* regv, set_bit */ +#include "modules/utils/byte.h" /* v16_to_v8 */ +#include "io.h" /* General defines of registers */ + +/** + * @defgroup ServoConfig Servo module configuration variables and defines. + * @{ + */ + +/** + * All servos are connected to the PORTA. + */ +#define SERVO_PORT PORTA +#define SERVO_DDR DDRA + +/** + * TOP of the timer/counter. + */ +#define SERVO_TCNT_TOP 0xFF + +/** + * Number of TIC the timer/counter need to do to make a whole cycle of servo + * update. + * It does not depend on the servo motors we manage but on the time we want a + * whole cycle to last. + * The formula used is: + * time_of_a_cycle * AVR_frequency / timer_counter_prescaler + * We want a time of 20ms (20/1000). + * See @a servo_init to know the prescaler value of the timer/counter. + */ +static const uint16_t servo_tic_cyle_ = ((20/1000) * AC_FREQ / 256); + +/** @} */ + +/** + * @defgroup ServoPrivate Servo module private variables and functions + * declarations + * @{ + */ + +/** + * Identifier of the servo we are currently updating. + * Note: -1 is a special value used by the servo module system to update the + * low state of all the servos. + */ +volatile int8_t servo_updating_id_; + +/** + * A table for the time spent by each servo in high state. + */ +volatile uint8_t servo_high_time_[SERVO_NUMBER]; + +/** + * Overflow of timer/counter 2 handler. + */ +SIGNAL (SIG_OVERFLOW2); + +/** @} */ + +/* Initialize servo module. */ +void +servo_init (void) +{ + /* Set-up all the pins of the servo to out direction */ + set_bit (SERVO_DDR, 0); + set_bit (SERVO_DDR, 1); + set_bit (SERVO_DDR, 2); + set_bit (SERVO_DDR, 3); + set_bit (SERVO_DDR, 4); + /* All pins are at low state by default */ + + /* Set-up the timer/counter 2: + - prescaler 256 => 4.44 ms TOP */ + TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, + 0, 0, 0, 0, 0, 1, 0, 0); + + /* The state machine start with the first servo */ + servo_updating_id_ = 0; + + /* Enable overflow interrupt */ + set_bit (TIMSK, TOIE2); +} + +/* Set the high time of the input signal of a servo (and its position). */ +void +servo_set_high_time (uint8_t servo, uint8_t high_time) +{ + /* Sanity check */ + if (servo < SERVO_NUMBER) + /* Set new desired position (high value time) */ + servo_high_time_[servo] = high_time; +} + +/* Get the high time of the servo. */ +uint8_t +servo_get_high_time (uint8_t servo) +{ + /* Sanity check */ + if (servo < SERVO_NUMBER) + return servo_high_time_[servo]; + return 0; +} + +/* Overflow of timer/counter 2 handler. */ +SIGNAL (SIG_OVERFLOW2) +{ + /* Overflow count (used when we wait in the lower state). + -1 is used for the first count where we wait less than a complete + overflow */ + static int8_t servo_overflow_count = -1; + /* Time spent by each servo motor at high state during a whole cycle */ + static uint16_t servo_high_time_cycle = servo_tic_cyle_; + + /* State machine actions */ + switch (servo_updating_id_) + { + case 0: + case 1: + case 2: + case 3: + case 4: + /* Servos motor high state mode */ + + /* Set to low state the previous servo motor pin if needed (not for + * the first one) */ + if (servo_updating_id_ != 0) + SERVO_PORT &= ~_BV (servo_updating_id_ - 1); + /* Set to high state the current servo motor pin */ + set_bit (SERVO_PORT, servo_updating_id_); + /* Plan next timer overflow to the TOP minus the current configuration + * of the servo motor */ + TCNT2 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; + /* Update the time spent at high state by all servo motors for this + * cycle */ + servo_high_time_cycle += servo_high_time_[servo_updating_id_]; + /* Update the identifier of the current servo motor (and manage when + * we are at the last one) */ + if (++servo_updating_id_ == SERVO_NUMBER) + servo_updating_id_ = -1; + break; + + case -1: + /* Sleeping time mode */ + + /* Is it the first we are in this mode? */ + if (servo_overflow_count == -1) + { + /* Set to low state the previous servo motor pin */ + SERVO_PORT &= ~_BV (SERVO_NUMBER - 1); + /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to + * wait (division by SERVO_TCNT_TOP or >> 8) */ + servo_overflow_count = servo_high_time_cycle >> 8; + /* Restart the counter from remaining TIC that are left and can + * not be used to make a full overflow */ + TCNT2 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0); + } + else + { + /* We just have an overflow, are we at the last one needed? The -1 + * is normal: we do not count the first overflow of the sleeping + * mode because it is not a full one */ + if (--servo_overflow_count == -1) + { + /* Restart with first servo motor */ + servo_updating_id_ = 0; + /* Re-initialize the counter of time spent by each servo motor + * at high state */ + servo_high_time_cycle = servo_tic_cyle_; + } + } + break; + } +} diff --git a/digital/io/src/servo.c b/digital/io/src/servo.c deleted file mode 100644 index 0d2b2580..00000000 --- a/digital/io/src/servo.c +++ /dev/null @@ -1,201 +0,0 @@ -/* servo.c */ -/* 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. - * - * }}} */ - -#include "common.h" -#include "servo.h" - -#include "modules/utils/utils.h" /* regv, set_bit */ -#include "modules/utils/byte.h" /* v16_to_v8 */ -#include "io.h" /* General defines of registers */ - -/** - * @defgroup ServoConfig Servo module configuration variables and defines. - * @{ - */ - -/** - * All servos are connected to the PORTA. - */ -#define SERVO_PORT PORTA -#define SERVO_DDR DDRA - -/** - * TOP of the timer/counter. - */ -#define SERVO_TCNT_TOP 0xFF - -/** - * Number of TIC the timer/counter need to do to make a whole cycle of servo - * update. - * It does not depend on the servo motors we manage but on the time we want a - * whole cycle to last. - * The formula used is: - * time_of_a_cycle * AVR_frequency / timer_counter_prescaler - * We want a time of 20ms (20/1000). - * See @a servo_init to know the prescaler value of the timer/counter. - */ -static const uint16_t servo_tic_cyle_ = ((20/1000) * AC_FREQ / 256); - -/** @} */ - -/** - * @defgroup ServoPrivate Servo module private variables and functions - * declarations - * @{ - */ - -/** - * Identifier of the servo we are currently updating. - * Note: -1 is a special value used by the servo module system to update the - * low state of all the servos. - */ -volatile int8_t servo_updating_id_; - -/** - * A table for the time spent by each servo in high state. - */ -volatile uint8_t servo_high_time_[SERVO_NUMBER]; - -/** - * Overflow of timer/counter 2 handler. - */ -SIGNAL (SIG_OVERFLOW2); - -/** @} */ - -/* Initialize servo module. */ -void -servo_init (void) -{ - /* Set-up all the pins of the servo to out direction */ - set_bit (SERVO_DDR, 0); - set_bit (SERVO_DDR, 1); - set_bit (SERVO_DDR, 2); - set_bit (SERVO_DDR, 3); - set_bit (SERVO_DDR, 4); - /* All pins are at low state by default */ - - /* Set-up the timer/counter 2: - - prescaler 256 => 4.44 ms TOP */ - TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, - 0, 0, 0, 0, 0, 1, 0, 0); - - /* The state machine start with the first servo */ - servo_updating_id_ = 0; - - /* Enable overflow interrupt */ - set_bit (TIMSK, TOIE2); -} - -/* Set the high time of the input signal of a servo (and its position). */ -void -servo_set_high_time (uint8_t servo, uint8_t high_time) -{ - /* Sanity check */ - if (servo < SERVO_NUMBER) - /* Set new desired position (high value time) */ - servo_high_time_[servo] = high_time; -} - -/* Get the high time of the servo. */ -uint8_t -servo_get_high_time (uint8_t servo) -{ - /* Sanity check */ - if (servo < SERVO_NUMBER) - return servo_high_time_[servo]; - return 0; -} - -/* Overflow of timer/counter 2 handler. */ -SIGNAL (SIG_OVERFLOW2) -{ - /* Overflow count (used when we wait in the lower state). - -1 is used for the first count where we wait less than a complete - overflow */ - static int8_t servo_overflow_count = -1; - /* Time spent by each servo motor at high state during a whole cycle */ - static uint16_t servo_high_time_cycle = servo_tic_cyle_; - - /* State machine actions */ - switch (servo_updating_id_) - { - case 0: - case 1: - case 2: - case 3: - case 4: - /* Servos motor high state mode */ - - /* Set to low state the previous servo motor pin if needed (not for - * the first one) */ - if (servo_updating_id_ != 0) - SERVO_PORT &= ~_BV (servo_updating_id_ - 1); - /* Set to high state the current servo motor pin */ - set_bit (SERVO_PORT, servo_updating_id_); - /* Plan next timer overflow to the TOP minus the current configuration - * of the servo motor */ - TCNT2 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; - /* Update the time spent at high state by all servo motors for this - * cycle */ - servo_high_time_cycle += servo_high_time_[servo_updating_id_]; - /* Update the identifier of the current servo motor (and manage when - * we are at the last one) */ - if (++servo_updating_id_ == SERVO_NUMBER) - servo_updating_id_ = -1; - break; - - case -1: - /* Sleeping time mode */ - - /* Is it the first we are in this mode? */ - if (servo_overflow_count == -1) - { - /* Set to low state the previous servo motor pin */ - SERVO_PORT &= ~_BV (SERVO_NUMBER - 1); - /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to - * wait (division by SERVO_TCNT_TOP or >> 8) */ - servo_overflow_count = servo_high_time_cycle >> 8; - /* Restart the counter from remaining TIC that are left and can - * not be used to make a full overflow */ - TCNT2 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0); - } - else - { - /* We just have an overflow, are we at the last one needed? The -1 - * is normal: we do not count the first overflow of the sleeping - * mode because it is not a full one */ - if (--servo_overflow_count == -1) - { - /* Restart with first servo motor */ - servo_updating_id_ = 0; - /* Re-initialize the counter of time spent by each servo motor - * at high state */ - servo_high_time_cycle = servo_tic_cyle_; - } - } - break; - } -} diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c new file mode 100644 index 00000000..cc9155bd --- /dev/null +++ b/digital/io/src/simu.host.c @@ -0,0 +1,153 @@ +/* simu.host.c - Host simulation. */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 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 "servo.h" + +#include "modules/utils/utils.h" +#include "modules/host/host.h" +#include "modules/host/mex.h" + +enum +{ + MSG_SIMU_IO_JACK = 0xb0, + MSG_SIMU_IO_COLOR = 0xb1, + MSG_SIMU_IO_SERVO = 0xb2, +}; + +/** Requested servo position. */ +uint8_t servo_high_time_[SERVO_NUMBER]; + +/** Current servo position. */ +uint8_t servo_high_time_current_[SERVO_NUMBER]; + +/** Servo speed is about 120 ms for 60 degrees. This means about 360 ms for + * the full swing. */ +#define SERVO_SPEED (int) ((256 / (360.0 / 4.4444)) + 0.5) + +/** Initialise simulation. */ +void +simu_init (void) +{ + mex_node_connect (); +} + +/** Make a simulation step. */ +void +simu_step (void) +{ + int i; + mex_msg_t *m; + /* Update servos. */ + for (i = 0; i < SERVO_NUMBER; i++) + { + if (UTILS_ABS (servo_high_time_current_[i] - servo_high_time_[i]) < + SERVO_SPEED) + servo_high_time_current_[i] = servo_high_time_[i]; + else if (servo_high_time_current_[i] < servo_high_time_[i]) + servo_high_time_current_[i] += SERVO_SPEED; + else + servo_high_time_current_[i] -= SERVO_SPEED; + } + /* Send servos. */ + m = mex_msg_new (MSG_SIMU_IO_SERVO); + for (i = 0; i < SERVO_NUMBER; i++) + mex_msg_push (m, "B", servo_high_time_current_[i]); + mex_node_send (m); +} + +void +servo_init (void) +{ +} + +void +servo_set_high_time (uint8_t servo, uint8_t high_time) +{ + servo_high_time_[servo] = high_time; +} + +uint8_t +servo_get_high_time (uint8_t servo) +{ + return servo_high_time_[servo]; +} + +void +switch_init (void) +{ +} + +uint8_t +switch_get_color (void) +{ + mex_msg_t *m = mex_msg_new (MSG_SIMU_IO_COLOR); + m = mex_node_request (m); + uint8_t r; + mex_msg_pop (m, "B", &r); + mex_msg_delete (m); + return r; +} + +uint8_t +switch_get_jack (void) +{ + mex_msg_t *m = mex_msg_new (MSG_SIMU_IO_JACK); + m = mex_node_request (m); + uint8_t r; + mex_msg_pop (m, "B", &r); + mex_msg_delete (m); + return r; +} + +void +main_timer_init (void) +{ + simu_init (); +} + +void +main_timer_wait (void) +{ + mex_node_wait_date (mex_node_date () + 4); + simu_step (); +} + +void +eeprom_load_param (void) +{ +} + +void +eeprom_save_param (void) +{ +} + +void +eeprom_clear_param (void) +{ +} + diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h new file mode 100644 index 00000000..7d2be4fa --- /dev/null +++ b/digital/io/src/simu.host.h @@ -0,0 +1,52 @@ +#ifndef simu_host_h +#define simu_host_h +/* simu.host.h - Host simulation. */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 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 + +/** Hooked, initialise the host simulation. */ +void +main_timer_init (void); + +/** Hooked, as before, wait for the next iteration. */ +void +main_timer_wait (void); + +/** Hooked, do nothing. */ +void +switch_init (void); + +/** Hooked, request via mex. */ +uint8_t +switch_get_color (void); + +/** Hooked, request via mex. */ +uint8_t +switch_get_jack (void); + +#endif /* defined (HOST) */ + +#endif /* simu_host_h */ diff --git a/digital/io/src/test/gutter/main.c b/digital/io/src/test/gutter/main.c index 38e4c3ea..37eefbda 100644 --- a/digital/io/src/test/gutter/main.c +++ b/digital/io/src/test/gutter/main.c @@ -111,7 +111,7 @@ trap_close_rear_panel(void) } void -asserv_go_to_gutter (void) +asserv_go_to_the_wall (void) { printf ("Go to gutter\n"); } -- cgit v1.2.3