/* 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); }