summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorJérémy Dufour2008-03-19 11:33:37 +0100
committerJérémy Dufour2008-03-19 11:33:37 +0100
commit624f2f2aade9f87ff22f56e0b5a9999cdd1db458 (patch)
tree4b17cc2b018b577550b938a89904be2054548261 /digital
parentfc4e130d5874a275db94719070d039b1913d8c57 (diff)
* digital/io/src
- add servo module (not integrated yet in the io program).
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/servo.c199
-rw-r--r--digital/io/src/servo.h85
3 files changed, 285 insertions, 1 deletions
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 */