summaryrefslogtreecommitdiff
path: root/n/es-2007/src/servo_motor.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2007/src/servo_motor.c')
-rw-r--r--n/es-2007/src/servo_motor.c185
1 files changed, 185 insertions, 0 deletions
diff --git a/n/es-2007/src/servo_motor.c b/n/es-2007/src/servo_motor.c
new file mode 100644
index 0000000..84e4d80
--- /dev/null
+++ b/n/es-2007/src/servo_motor.c
@@ -0,0 +1,185 @@
+/* servo_motor.c */
+/* es - Input/Output general purpose board. {{{
+ *
+ * Copyright (C) 2006 Dufour Jérémy
+ *
+ * Robot APB Team/Efrei 2004.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * 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_motor.h"
+
+#include "io.h"
+#include "common.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/byte.h"
+
+/** TOP of the timer/counter 2. */
+#define TC2_TOP 0xFF
+/** Prescaler of the timer/counter2. */
+#define TC2_PRESCALER 256
+/** Delay a full cycle in TIC before restarting should last (in 100µs). */
+/* time * AC_FREQ / TC2_PRESCALER */
+#define SRVM_CYCLE_DELAY 1152
+/** The servo motor identifier. */
+#define SRVM_TRASH_PIN 0 /* the trash system */
+#define SRVM_TOTEML_PIN 3 /* first totem */
+#define SRVM_TOTEMR_PIN 2 /* second totem */
+/** Table for the time spend by each servo in high position. */
+volatile uint8_t servo_time[3] = { SRVM_TRASH_POS_CLOSE, SRVM_RIGHT_POS_IN, SRVM_LEFT_POS_IN };
+/** Different states of this module. */
+
+#define SRVM_STATE_SLEEP 0
+#define SRVM_STATE_TRASH 1
+#define SRVM_STATE_TOTEM1 2
+#define SRVM_STATE_TOTEM2 3
+
+/** State variable. */
+volatile uint8_t servo_motor_state;
+/** Time the TC2 have to sleep after manage all the servo motor. */
+volatile uint16_t srvm_need_to_sleep;
+
+/** Init the servo motor system. */
+void
+servo_motor_init (void)
+{
+ /* All the pins are in out mode */
+ DDRB |= _BV (SRVM_TRASH_PIN) | _BV (SRVM_TOTEML_PIN) |
+ _BV (SRVM_TOTEMR_PIN);
+ /* We are sleeping */
+ servo_motor_state = SRVM_STATE_SLEEP;
+ /* Prescaler 256 => 4.44 ms TOP */
+ TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
+ 0, 0, 0, 0, 0, 1, 0, 0);
+ /* Enable overflow interrupt */
+ TIMSK |= _BV (TOIE2);
+ /* Iniate to delay */
+ srvm_need_to_sleep = SRVM_CYCLE_DELAY;
+}
+
+/** Ask the servo motor to go to a position. */
+void
+servo_motor_set_pos (uint8_t servo_mask, uint8_t servo_pos)
+{
+ uint8_t num;
+ /* for each servo motor */
+ for (num = 0; num < 3; num++)
+ if (servo_mask & _BV (num))
+ {
+ /* Set the value and bound it */
+ servo_time[num] = servo_pos;
+ /* Trash */
+ if (num == 0)
+ UTILS_BOUND (servo_time[num], SRVM_TRASH_POS_OPEN,
+ SRVM_TRASH_POS_CLOSE);
+ /* Totem1 */
+ else if (num == 1)
+ UTILS_BOUND (servo_time[num], SRVM_RIGHT_POS_OUT,
+ SRVM_RIGHT_POS_IN);
+ /* Totem Left */
+ else if (num == 2)
+ UTILS_BOUND (servo_time[num], SRVM_LEFT_POS_IN,
+ SRVM_LEFT_POS_OUT);
+ }
+}
+
+/** What is your current position my dear servo motor ? */
+inline uint8_t
+servo_motor_get_pos (uint8_t servo_num)
+{
+ return servo_time[servo_num];
+}
+
+SIGNAL (SIG_OVERFLOW2)
+{
+ /* Overflow count for time sleeping */
+ static int8_t srvm_ov_for_sleeping = -1;
+
+ switch (servo_motor_state)
+ {
+ case SRVM_STATE_SLEEP:
+ /* We remove from the TOP the number of TIC of this servo */
+ TCNT2 = TC2_TOP - servo_time[servo_motor_state];
+ /* We activate the pin */
+ PORTB |= _BV (SRVM_TRASH_PIN);
+ /* We remove the TIC of this servo from the main one */
+ srvm_need_to_sleep -= servo_time[servo_motor_state];
+ /* Next state */
+ servo_motor_state = SRVM_STATE_TRASH;
+ break;
+ case SRVM_STATE_TRASH:
+ /* We remove from the TOP the number of TIC of this servo */
+ TCNT2 = TC2_TOP - servo_time[servo_motor_state];
+ /* Unactivate previous pin */
+ PORTB &= ~_BV (SRVM_TRASH_PIN);
+ /* We activate the pin */
+ PORTB |= _BV (SRVM_TOTEML_PIN);
+ /* We remove the TIC of this servo from the main one */
+ srvm_need_to_sleep -= servo_time[servo_motor_state];
+ /* Next state */
+ servo_motor_state = SRVM_STATE_TOTEM1;
+ break;
+ case SRVM_STATE_TOTEM1:
+ /* We remove from the TOP the number of TIC of this servo */
+ TCNT2 = TC2_TOP - servo_time[servo_motor_state];
+ /* Unactivate previous pin */
+ PORTB &= ~_BV (SRVM_TOTEML_PIN);
+ /* We activate the pin */
+ PORTB |= _BV (SRVM_TOTEMR_PIN);
+ /* We remove the TIC of this servo from the main one */
+ srvm_need_to_sleep -= servo_time[servo_motor_state];
+ /* Next state */
+ servo_motor_state = SRVM_STATE_TOTEM2;
+ break;
+ case SRVM_STATE_TOTEM2:
+ if (srvm_ov_for_sleeping == -1)
+ {
+ /* We need to wait some TIC */
+ TCNT2 = TC2_TOP - v16_to_v8 (srvm_need_to_sleep, 0);
+ /* Unactivate previous pin */
+ PORTB &= ~_BV (SRVM_TOTEMR_PIN);
+ /* Calculate the OV */
+ srvm_ov_for_sleeping = srvm_need_to_sleep >> 8;
+ }
+ else
+ {
+ if (--srvm_ov_for_sleeping == -1)
+ {
+ /* Next state */
+ servo_motor_state = SRVM_STATE_SLEEP;
+ /* Re init */
+ srvm_need_to_sleep = SRVM_CYCLE_DELAY;
+ }
+ }
+ break;
+ }
+}
+
+/** Open the trash. */
+void
+servo_motor_open_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_OPEN);
+}
+
+/** Close the trash. */
+void
+servo_motor_close_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_CLOSE);
+}