From 2700442e8ecb331844687e349f5b20162a008b27 Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Sun, 29 May 2011 14:20:56 +0200 Subject: digital/avr/module: add module servo from digital/io/src --- digital/avr/modules/devices/servo/Makefile.module | 1 + digital/avr/modules/devices/servo/README | 23 +++ digital/avr/modules/devices/servo/avrconfig.h | 33 ++++ digital/avr/modules/devices/servo/servo.avr.c | 193 ++++++++++++++++++++ digital/avr/modules/devices/servo/servo.h | 96 ++++++++++ digital/avr/modules/devices/servo/servo_pos.c | 60 +++++++ digital/avr/modules/devices/servo/servo_pos.h | 73 ++++++++ digital/avr/modules/devices/servo/test/Makefile | 12 ++ digital/avr/modules/devices/servo/test/avrconfig.h | 91 ++++++++++ .../avr/modules/devices/servo/test/test_servo.c | 86 +++++++++ digital/io/src/Makefile | 6 +- digital/io/src/avrconfig.h | 5 + digital/io/src/eeprom.avr.c | 4 +- digital/io/src/main.c | 4 +- digital/io/src/servo.avr.c | 200 --------------------- digital/io/src/servo.h | 96 ---------- digital/io/src/servo_pos.c | 60 ------- digital/io/src/servo_pos.h | 73 -------- digital/io/src/simu.host.c | 26 +-- 19 files changed, 693 insertions(+), 449 deletions(-) create mode 100644 digital/avr/modules/devices/servo/Makefile.module create mode 100644 digital/avr/modules/devices/servo/README create mode 100644 digital/avr/modules/devices/servo/avrconfig.h create mode 100644 digital/avr/modules/devices/servo/servo.avr.c create mode 100644 digital/avr/modules/devices/servo/servo.h create mode 100644 digital/avr/modules/devices/servo/servo_pos.c create mode 100644 digital/avr/modules/devices/servo/servo_pos.h create mode 100644 digital/avr/modules/devices/servo/test/Makefile create mode 100644 digital/avr/modules/devices/servo/test/avrconfig.h create mode 100644 digital/avr/modules/devices/servo/test/test_servo.c delete mode 100644 digital/io/src/servo.avr.c delete mode 100644 digital/io/src/servo.h delete mode 100644 digital/io/src/servo_pos.c delete mode 100644 digital/io/src/servo_pos.h (limited to 'digital') diff --git a/digital/avr/modules/devices/servo/Makefile.module b/digital/avr/modules/devices/servo/Makefile.module new file mode 100644 index 00000000..bf82ffac --- /dev/null +++ b/digital/avr/modules/devices/servo/Makefile.module @@ -0,0 +1 @@ +devices_servo_SOURCES = servo_pos.c servo.avr.c diff --git a/digital/avr/modules/devices/servo/README b/digital/avr/modules/devices/servo/README new file mode 100644 index 00000000..4981de17 --- /dev/null +++ b/digital/avr/modules/devices/servo/README @@ -0,0 +1,23 @@ +avr.devices.servo - Servo AVR module. + +Servo module for AVR. See modules README for more details about AVR modules. + +Copyright (C) 2011 Maxime Hadjinlian + +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/avr/modules/devices/servo/avrconfig.h b/digital/avr/modules/devices/servo/avrconfig.h new file mode 100644 index 00000000..d192598a --- /dev/null +++ b/digital/avr/modules/devices/servo/avrconfig.h @@ -0,0 +1,33 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * 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. + * + * }}} */ + +/* servo - Servo module. */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/devices/servo/servo.avr.c b/digital/avr/modules/devices/servo/servo.avr.c new file mode 100644 index 00000000..fc5adc78 --- /dev/null +++ b/digital/avr/modules/devices/servo/servo.avr.c @@ -0,0 +1,193 @@ +/* servo.avr.c */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * 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. + * @{ + */ + +/** + * 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_cycle_ = AC_FREQ / 256 * 20 / 1000; + +/** @} */ + +/** + * @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_position_[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 */ + AC_SERVO_DDR = 0xff; + /* 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); + + /* By default, servo init disable all servo. */ + uint8_t i; + for (i = 0; i < SERVO_NUMBER; i++) + servo_set_position (i, 0); +} + +/* Set the duration of the input signal at the high state of a servo. */ +void +servo_set_position (uint8_t servo, uint8_t position) +{ + uint8_t filtered = position; + if (filtered != 0) + UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX); + /* Sanity check */ + if (servo < SERVO_NUMBER) + /* Set new desired position (high value time) */ + servo_position_[servo] = filtered; +} + +/* Get the duration of the servo's input signal at high state. */ +uint8_t +servo_get_position (uint8_t servo) +{ + /* Sanity check */ + if (servo < SERVO_NUMBER) + return servo_position_[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_position_cycle = servo_tic_cycle_; + + /* State machine actions */ + if (servo_updating_id_ >= 0) + { + /* 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) + AC_SERVO_PORT &= ~_BV (servo_updating_id_ - 1); + /* Set to high state the current servo motor pin, unless is zero */ + if (servo_position_[servo_updating_id_]) + set_bit (AC_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_position_[servo_updating_id_]; + /* Update the time spent at high state by all servo motors for this + * cycle */ + servo_position_cycle += servo_position_[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; + } + else + { + /* 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 */ + AC_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_position_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_position_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_position_cycle = servo_tic_cycle_; + } + } + } +} diff --git a/digital/avr/modules/devices/servo/servo.h b/digital/avr/modules/devices/servo/servo.h new file mode 100644 index 00000000..59d35907 --- /dev/null +++ b/digital/avr/modules/devices/servo/servo.h @@ -0,0 +1,96 @@ +#ifndef servo_h +#define servo_h +/* servo.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * 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" // uint8_t + +/** + * @file Module to control servo motors. + * This module contains low-level functions to control the servo motors. If + * you want to control the traps to store the balls at a specific place, have + * a look at the trap module. + * It uses the timer/counter 2 (8-bit). + * + * Servo motors can be controlled by the time the input signal spend at its + * high state. For example, if the signal sent to the servo motor only spend + * 1ms at the high state, it will have a 0° angle position. If the signal + * stays for 1.5ms at high state, it will have a 90° angle position. + * To manage all servo motors in a "one time shot", we need to use the + * timer/counter 2 of the ATmega128 and its overflow. + * We setup the timer/counter to the value of its overflow minus the time the + * input signal of the servo need to spend at high state ; we put the input + * signal of this servo motor to the high value. When the timer overflows, we + * put it back to the low state. We go to the next servo motors and do the + * same algorithm. When the all servos motor have been taken care of, we + * set-up the timer to overflow a certain number of times to wait before + * restarting the whole cycle. + * + * All servos are connected to AC_SERVO_PORT defined in avrconfig.h + */ + +/** + * Number of servos motor managed by this module. + * If you change it, you _must_ update the key of the eeprom module! + */ +#define SERVO_NUMBER 8 + +/** + * Minimum high time for servos. + */ +#define SERVO_HIGH_TIME_MIN 0x24 + +/** + * Maximum high time for servos. + */ +#define SERVO_HIGH_TIME_MAX 0x88 + +/** + * Initialize servo module. + * This functions put the pins of the servos motor in the right direction, + * initialize the timer/counter 2 and some internals stuff. + */ +void +servo_init (void); + +/** + * Set the duration of the input signal of a servo at high state + * and thus its position. + * @param servo the servo to change the position. + * @param position the duration while the input signal will be at the + * high state. The servo will then move to a position. + * A zero will let the servo floating. + */ +void +servo_set_position (uint8_t servo, uint8_t position); + +/** + * Get the duration of the servo's input signal at high state. + * @param servo the servo to get the position of. + * @return the current position of the servo. + */ +uint8_t +servo_get_position (uint8_t servo); + +#endif /* servo_h */ diff --git a/digital/avr/modules/devices/servo/servo_pos.c b/digital/avr/modules/devices/servo/servo_pos.c new file mode 100644 index 00000000..6308ceaf --- /dev/null +++ b/digital/avr/modules/devices/servo/servo_pos.c @@ -0,0 +1,60 @@ +/* servo_pos.c */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2009 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 "servo_pos.h" + +uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +void +servo_pos_init (void) +{ + /* Initialize under layer, servo module. */ + servo_init (); +} + +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]) +{ + uint8_t i; + /* If servo exists. */ + if (servo_id < SERVO_NUMBER) + { + /* For each position. */ + for (i = 0; i < SERVO_POS_NUMBER; i++) + servo_pos_high_time[servo_id][i] = high_times[i]; + } +} + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position) +{ + servo_set_position (servo_id, servo_pos_high_time[servo_id][position]); +} diff --git a/digital/avr/modules/devices/servo/servo_pos.h b/digital/avr/modules/devices/servo/servo_pos.h new file mode 100644 index 00000000..e173ab6d --- /dev/null +++ b/digital/avr/modules/devices/servo/servo_pos.h @@ -0,0 +1,73 @@ +#ifndef servo_pos_h +#define servo_pos_h +/* servo.pos.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2009 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. + * + * }}} */ + +/** + * Module to control two positions of the servo motor. + * It's an higher interface that uses the servo module but remember the + * positions (for example, two positions, open and close) that they can take. + */ + +#include "servo.h" + +/** + * Number of positions the servo can take. + * @warning if you change this define, you must update the key value in the + * EEPROM module. Otherwise, everything will fail! + */ +#define SERVO_POS_NUMBER 2 + +/** + * Table to store the correspondence between the positions and the high time. + */ +extern uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +/** + * Initialize the servo with positions module. + * @note it also initialize the servo module for you. + */ +void +servo_pos_init (void); + +/** + * Set the high times of a servo for positions. + * @param servo_id the ID of the servo. + * @param high_times the different high times for the positions of the + * servo. + */ +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]); + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position); + +#endif /* servo_pos_h */ diff --git a/digital/avr/modules/devices/servo/test/Makefile b/digital/avr/modules/devices/servo/test/Makefile new file mode 100644 index 00000000..a0ad639f --- /dev/null +++ b/digital/avr/modules/devices/servo/test/Makefile @@ -0,0 +1,12 @@ +BASE = ../../../.. +AVR_PROGS = test_servo +test_servo_SOURCES = test_servo.c +MODULES = proto devices/servo uart utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/devices/servo/test/avrconfig.h b/digital/avr/modules/devices/servo/test/avrconfig.h new file mode 100644 index 00000000..6ee81392 --- /dev/null +++ b/digital/avr/modules/devices/servo/test/avrconfig.h @@ -0,0 +1,91 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * 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 14745600 + +/* servo - Servo module. */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + +/* 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 DROP +/** 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 8 +/** 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 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/devices/servo/test/test_servo.c b/digital/avr/modules/devices/servo/test/test_servo.c new file mode 100644 index 00000000..924fae1c --- /dev/null +++ b/digital/avr/modules/devices/servo/test/test_servo.c @@ -0,0 +1,86 @@ +/* test_servo.c. */ +/* avr.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * 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/uart/uart.h" +#include "modules/proto/proto.h" +#include "modules/devices/servo/servo.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "io.h" + +/* proto_callback is called by proto + * when a command is complete and valid. + * proto act as an intelligent buffer for us. +*/ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ + /* This macro combine command and size in one integer. */ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* This should be generaly implemented. */ + utils_reset (); + break; + case c ('s', 3): + /* We are looking to receive a command in the form !sxxyyyy + * where xx is the servo id and yyyy is the command itself. + * For the size, please read digital/avr/modules/proto/proto.txt + */ + servo_set_position (args[0], v8_to_v16 (args[1], args[2])); + break; + default: + /* This is to handle default commands, return an error. */ + proto_send0 ('?'); + return; + } + /* When no error acknoledge. */ + proto_send (cmd, size, args); +#undef c +} + +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + sei (); + uart0_init (); + servo_init (); + + /* This command should be generaly sent on reset. */ + proto_send0 ('z'); + /* This is to accept commands. */ + while (1) + { + uint8_t c = uart0_getc (); + proto_accept (c); + /* once the command is complete according to + * proto's state machine, proto_callback will + * be called with the full command. + */ + } +} diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 463e148e..4ebc5a66 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -3,15 +3,15 @@ BASE = ../../avr # Name of the program to build. PROGS = io # Sources to compile. -io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ - switch.avr.c chrono.c timer.avr.c servo_pos.c \ +io_SOURCES = main.c fsm_queue.c eeprom.avr.c pwm.c \ + switch.avr.c chrono.c timer.avr.c \ twi_master.c asserv.c mimot.c \ simu.host.c contact.c radar.c radar_defs.c \ path.c food.c events.host.c \ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ - devices/usdist \ + devices/usdist devices/servo \ trace flash spi AI_MODULES = twi_master common utils fsm move # Configuration file. diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index a46b4abe..cb9061fd 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -129,6 +129,11 @@ USDIST_SENSOR (2, C, 7) \ USDIST_SENSOR (3, D, 4) +/* servo - Servo module */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + /* io - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 2 diff --git a/digital/io/src/eeprom.avr.c b/digital/io/src/eeprom.avr.c index a32b9606..febba55c 100644 --- a/digital/io/src/eeprom.avr.c +++ b/digital/io/src/eeprom.avr.c @@ -26,8 +26,8 @@ #include "common.h" #include "eeprom.h" -#include "servo.h" /* SERVO_NUMBER */ -#include "servo_pos.h" +#include "modules/devices/servo/servo.h" +#include "modules/devices/servo/servo_pos.h" #include /* eeprom_{read,write}_byte */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 016aad36..5aa35fb7 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -29,6 +29,7 @@ #include "modules/utils/utils.h" #include "modules/path/path.h" #include "modules/devices/usdist/usdist.h" +#include "modules/devices/servo/servo_pos.h" #include "modules/flash/flash.h" #include "modules/trace/trace.h" #include "events.h" @@ -51,7 +52,6 @@ #include "fsm.h" #include "fsm_queue.h" #include "bot.h" -#include "servo_pos.h" #include "radar.h" #include "chrono.h" /* chrono_end_match */ #include "pwm.h" @@ -408,7 +408,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: servo id number; * - 1b: pwm high time value (position). */ - servo_set_high_time (args[0], args[1]); + servo_set_position (args[0], args[1]); break; case c ('S', 0): diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c deleted file mode 100644 index de50c819..00000000 --- a/digital/io/src/servo.avr.c +++ /dev/null @@ -1,200 +0,0 @@ -/* 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_cycle_ = AC_FREQ / 256 * 20 / 1000; - -/** @} */ - -/** - * @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 */ - SERVO_DDR = 0xff; - /* 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); - - /* By default, servo init disable all servo. */ - uint8_t i; - for (i = 0; i < SERVO_NUMBER; i++) - servo_set_high_time (i, 0); -} - -/* 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) -{ - uint8_t filtered = high_time; - if (filtered != 0) - UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX); - /* Sanity check */ - if (servo < SERVO_NUMBER) - /* Set new desired position (high value time) */ - servo_high_time_[servo] = filtered; -} - -/* 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_cycle_; - - /* State machine actions */ - if (servo_updating_id_ >= 0) - { - /* 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, unless is zero */ - if (servo_high_time_[servo_updating_id_]) - 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; - } - else - { - /* 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_cycle_; - } - } - } -} diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h deleted file mode 100644 index b9f7e3ea..00000000 --- a/digital/io/src/servo.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef servo_h -#define servo_h -/* servo.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. - * - * }}} */ - -#include "common.h" // uint8_t - -/** - * @file Module to control servo motors. - * This module contains low-level functions to control the servo motors. If - * you want to control the traps to store the balls at a specific place, have - * a look at the trap module. - * It uses the timer/counter 2 (8-bit). - * - * Servo motors can be controlled by the time the input signal spend at its - * high state. For example, if the signal sent to the servo motor only spend - * 1ms at the high state, it will have a 0° angle position. If the signal - * stays for 1.5ms at high state, it will have a 90° angle position. - * To manage all servo motors in a "one time shot", we need to use the - * timer/counter 2 of the ATmega128 and its overflow. - * We setup the timer/counter to the value of its overflow minus the time the - * input signal of the servo need to spend at high state ; we put the input - * signal of this servo motor to the high value. When the timer overflows, we - * put it back to the low state. We go to the next servo motors and do the - * same algorithm. When the all servos motor have been taken care of, we - * set-up the timer to overflow a certain number of times to wait before - * restarting the whole cycle. - * - * All servos are connected to the PORTA of the ATmega. - */ - -/** - * Number of servos motor managed by this module. - * If you change it, you _must_ update the key of the eeprom module! - */ -#define SERVO_NUMBER 8 - -/** - * Minimum high time for servos. - */ -#define SERVO_HIGH_TIME_MIN 0x24 - -/** - * Maximum high time for servos. - */ -#define SERVO_HIGH_TIME_MAX 0x88 - -/** - * Initialize servo module. - * This functions put the pins of the servos motor in the right direction, - * initialize the timer/counter 2 and some internals stuff. - */ -void -servo_init (void); - -/** - * Set the high time of the input signal of a servo (and its position). - * @param servo the servo to change the position. - * @param high_time the high time we want the input signal to spend at the - * high state to set the servo motor to a position. A zero will let the servo - * floating. - */ -void -servo_set_high_time (uint8_t servo, uint8_t high_time); - -/** - * Get the high time of the servo. - * @param servo the servo to get the position of. - * @return the current position of the servo. - */ -uint8_t -servo_get_high_time (uint8_t servo); - -#endif /* servo_h */ diff --git a/digital/io/src/servo_pos.c b/digital/io/src/servo_pos.c deleted file mode 100644 index d04077fd..00000000 --- a/digital/io/src/servo_pos.c +++ /dev/null @@ -1,60 +0,0 @@ -/* servo_pos.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 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 "servo_pos.h" - -uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; - -void -servo_pos_init (void) -{ - /* Initialize under layer, servo module. */ - servo_init (); -} - -void -servo_pos_set_high_time (uint8_t servo_id, - uint8_t high_times[SERVO_POS_NUMBER]) -{ - uint8_t i; - /* If servo exists. */ - if (servo_id < SERVO_NUMBER) - { - /* For each position. */ - for (i = 0; i < SERVO_POS_NUMBER; i++) - servo_pos_high_time[servo_id][i] = high_times[i]; - } -} - -/** - * Move a servo to a specific position. - * @param servo_id the id of the servo to move. - * @param position the position identifier where to move the servo. - */ -void -servo_pos_move_to (uint8_t servo_id, uint8_t position) -{ - servo_set_high_time (servo_id, servo_pos_high_time[servo_id][position]); -} diff --git a/digital/io/src/servo_pos.h b/digital/io/src/servo_pos.h deleted file mode 100644 index c8e56d2b..00000000 --- a/digital/io/src/servo_pos.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef servo_pos_h -#define servo_pos_h -/* servo_pos.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 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. - * - * }}} */ - -/** - * Module to control two positions of the servo motor. - * It's an higher interface that uses the servo module but remember the - * positions (for example, two positions, open and close) that they can take. - */ - -#include "servo.h" - -/** - * Number of positions the servo can take. - * @warning if you change this define, you must update the key value in the - * EEPROM module. Otherwise, everything will fail! - */ -#define SERVO_POS_NUMBER 2 - -/** - * Table to store the correspondence between the positions and the high time. - */ -extern uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; - -/** - * Initialize the servo with positions module. - * @note it also initialize the servo module for you. - */ -void -servo_pos_init (void); - -/** - * Set the high times of a servo for positions. - * @param servo_id the ID of the servo. - * @param high_times the different high times for the positions of the - * servo. - */ -void -servo_pos_set_high_time (uint8_t servo_id, - uint8_t high_times[SERVO_POS_NUMBER]); - -/** - * Move a servo to a specific position. - * @param servo_id the id of the servo to move. - * @param position the position identifier where to move the servo. - */ -void -servo_pos_move_to (uint8_t servo_id, uint8_t position); - -#endif /* servo_pos_h */ diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index f1b7e0d2..a35b9d12 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -25,7 +25,7 @@ #include "common.h" #include "simu.host.h" -#include "servo.h" +#include "modules/devices/servo/servo.h" #include "pwm.h" #include "modules/utils/utils.h" @@ -36,10 +36,10 @@ #include "io.h" /** Requested servo position. */ -uint8_t servo_high_time_[SERVO_NUMBER]; +uint8_t servo_position_[SERVO_NUMBER]; /** Current servo position. */ -uint8_t servo_high_time_current_[SERVO_NUMBER]; +uint8_t servo_position_current_[SERVO_NUMBER]; /** Servo speed is about 120 ms for 60 degrees. This means about 360 ms for * the full swing. */ @@ -112,13 +112,13 @@ simu_step (void) /* Update servos. */ for (i = 0; i < SERVO_NUMBER; i++) { - if (UTILS_ABS (servo_high_time_current_[i] - servo_high_time_[i]) < + if (UTILS_ABS (servo_position_current_[i] - servo_position_[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; + servo_position_current_[i] = servo_position_[i]; + else if (servo_position_current_[i] < servo_position_[i]) + servo_position_current_[i] += SERVO_SPEED; else - servo_high_time_current_[i] -= SERVO_SPEED; + servo_position_current_[i] -= SERVO_SPEED; } /* Send servos. */ if (simu_servo_update && !--simu_servo_update_cpt) @@ -126,7 +126,7 @@ simu_step (void) simu_servo_update_cpt = simu_servo_update; m = mex_msg_new (simu_mex_servo); for (i = 0; i < SERVO_NUMBER; i++) - mex_msg_push (m, "B", servo_high_time_current_[i]); + mex_msg_push (m, "B", servo_position_current_[i]); mex_node_send (m); } /* Update switches. */ @@ -180,15 +180,15 @@ servo_init (void) } void -servo_set_high_time (uint8_t servo, uint8_t high_time) +servo_set_position (uint8_t servo, uint8_t position) { - servo_high_time_[servo] = high_time; + servo_position_[servo] = position; } uint8_t -servo_get_high_time (uint8_t servo) +servo_get_position (uint8_t servo) { - return servo_high_time_[servo]; + return servo_position_[servo]; } void -- cgit v1.2.3