From 624f2f2aade9f87ff22f56e0b5a9999cdd1db458 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 19 Mar 2008 11:33:37 +0100 Subject: * digital/io/src - add servo module (not integrated yet in the io program). --- digital/io/src/Makefile | 2 +- digital/io/src/servo.c | 199 ++++++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/servo.h | 85 +++++++++++++++++++++ 3 files changed, 285 insertions(+), 1 deletion(-) create mode 100644 digital/io/src/servo.c create mode 100644 digital/io/src/servo.h (limited to 'digital') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 70dc62c7..60d037f3 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -1,6 +1,6 @@ BASE = ../../avr AVR_PROGS = io -io_SOURCES = main.c asserv.c +io_SOURCES = main.c asserv.c servo.c MODULES = proto uart twi CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... diff --git a/digital/io/src/servo.c b/digital/io/src/servo.c new file mode 100644 index 00000000..66fb9681 --- /dev/null +++ b/digital/io/src/servo.c @@ -0,0 +1,199 @@ +/* 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 "servo.h" + +#include "io.h" /* General defines of registers */ +#include "modules/utils/utils.h" /* regv */ +#include "modules/utils/byte.h" /* v16_to_v8 */ + +/** + * @defgroup ServoConfig Servo module configuration variables and defines. + * @{ + */ + +/** + * All servos are connected to the PORTA. + */ +#define SERVO_PORT PORTA + +/** + * 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. + */ +#define SERVO_TIC_CYCLE 1152 + +/** @} */ + +/** + * @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) +{ +#define set_bit(port, bit) (port |= _BV(bit)) + /* Set-up all the pins of the servo to out direction */ + set_bit (SERVO_PORT, 0); + set_bit (SERVO_PORT, 1); + set_bit (SERVO_PORT, 2); + set_bit (SERVO_PORT, 3); + set_bit (SERVO_PORT, 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 */ + TIMSK |= _BV (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_CYCLE; + + /* 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 */ + SERVO_PORT |= _BV (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_CYCLE; + } + } + break; + } +} diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h new file mode 100644 index 00000000..b52b02bc --- /dev/null +++ b/digital/io/src/servo.h @@ -0,0 +1,85 @@ +#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 + * 0.5ms 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. + */ + +/** + * Number of servos motor managed by this module. + */ +#define SERVO_NUMBER 5 + +/** + * 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. + * A important remark: you need to set the default values of each servos motor + * before calling this functions if you do not want to have some strange + * behavior. + */ +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. + */ +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 */ -- cgit v1.2.3