modules/devices/servo/multiservo/multiservo.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2006)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: multiservo.c,v 1.5.4.5 2008-03-02 17:18:16 zer0 Exp $
  19  *
  20  */
  21
  22 #include <string.h>
  23
  24 #include <aversive.h>
  25 #include <aversive/error.h>
  26 #include <multiservo.h>
  27 #include <multiservo_archs.h>
  28
  29 struct multiservo g_multiservo;
  30
  31 //#define MULTISERVO_ENABLE_DEBUG
  32
  33 #ifdef MULTISERVO_ENABLE_DEBUG
  34 #define MULTISERVO_DEBUG(args...) DEBUG(args)
  35 #else
  36 #define MULTISERVO_DEBUG(args...) do { } while(0)
  37 #endif
  38
  39 /* value of timer for 1 ms */
  40 #define TIME_1MS (CONFIG_QUARTZ/((1000UL)*MULTISERVO_TIMER_PRESCALER))
  41 #define TIME_1_5MS ((TIME_1MS*3)/2)
  42
  43 /* for timers 8 bits, check that 2ms value (max) is < 256 */
  44 #if (MULTISERVO_TIMER == 0 || MULTISERVO_TIMER == 2) && ((TIME_1MS*2) >= 256L)
  45 #error "Bad PRESCALER, you should increase it in multiservo_config.h"
  46 #endif
  47
  48 /* for timers 16 bits, check that 2ms value (max) is < 65536 */
  49 #if (MULTISERVO_TIMER == 1 || MULTISERVO_TIMER == 3) && ((TIME_1MS*2) >= 65536L)
  50 #error "Bad PRESCALER, you should increase it in multiservo_config.h"
  51 #endif
  52
  53
  54
  55 SIGNAL(MULTISERVO_SIG_OUTPUT_COMPARE) /* other ints NOT allowed */
  56 {
  57         volatile uint8_t * port ;
  58
  59         if (g_multiservo.time_sum > TIME_1MS * 20) {
  60                 MULTISERVO_DEBUG(E_MULTISERVO, "end, restart");
  61                 g_multiservo.time_sum = 0;
  62                 g_multiservo.current_multiservo = 0;
  63         }
  64
  65         /* reset pin */
  66         if (g_multiservo.id_prev != -1) {
  67                 port = g_multiservo.elts[g_multiservo.id_prev].port;
  68                 if(port) {
  69                         MULTISERVO_DEBUG(E_MULTISERVO, "reset %d",g_multiservo.id_prev);
  70                         cbi(*port, g_multiservo.elts[g_multiservo.id_prev].bitnum);
  71                 }
  72         }
  73
  74         /* set pin */
  75         while (g_multiservo.current_multiservo < MULTISERVO_NB_MAX) {
  76                 port = g_multiservo.elts[g_multiservo.current_multiservo].port;
  77                 if(port) {
  78                         MULTISERVO_DEBUG(E_MULTISERVO, "set %d %d",
  79                                          g_multiservo.current_multiservo,
  80                                          g_multiservo.elts[g_multiservo.current_multiservo].value);
  81                         sbi(*port, g_multiservo.elts[g_multiservo.current_multiservo].bitnum);
  82                         g_multiservo.id_prev = g_multiservo.current_multiservo;
  83                         MULTISERVO_OCR = g_multiservo.elts[g_multiservo.current_multiservo].value;
  84                         g_multiservo.time_sum += g_multiservo.elts[g_multiservo.current_multiservo].value;
  85                         break;
  86                 }
  87                 g_multiservo.current_multiservo ++;
  88         }
  89
  90         /* wait until 20ms is reached */
  91         if (g_multiservo.current_multiservo >= MULTISERVO_NB_MAX) {
  92                 g_multiservo.id_prev = -1;
  93                 MULTISERVO_DEBUG(E_MULTISERVO, "wait 1ms %d (%ld)", g_multiservo.current_multiservo, TIME_1MS);
  94                 MULTISERVO_OCR = TIME_1MS;
  95                 g_multiservo.time_sum += TIME_1MS;
  96         }
  97         else {
  98                 g_multiservo.current_multiservo ++;
  99         }
 100 }
 101
 102
 103
 104 void multiservo_init(void)
 105 {
 106         uint8_t flags;
 107
 108         IRQ_LOCK(flags);
 109
 110         cbi(MULTISERVO_TIMSK, MULTISERVO_OCIE);
 111
 112         memset(&g_multiservo, 0, sizeof(g_multiservo));
 113         g_multiservo.id_prev = -1;
 114
 115         /* Timer config (see in multiservo_archs.h) */
 116         MULTISERVO_TCCRnA = MULTISERVO_TCCRnA_VALUE;
 117 #ifdef MULTISERVO_TCCRnB
 118         MULTISERVO_TCCRnB = MULTISERVO_TCCRnB_VALUE;
 119 #endif
 120
 121         MULTISERVO_OCR = TIME_1MS;
 122         sbi(MULTISERVO_TIMSK, MULTISERVO_OCIE);
 123         IRQ_UNLOCK(flags);
 124 }
 125
 126
 127 int8_t multiservo_add(volatile uint8_t * port, uint8_t bitnum)
 128 {
 129         uint8_t i;
 130         uint8_t flags;
 131
 132         IRQ_LOCK(flags);
 133         /* find a place and add it */
 134         for ( i=0 ; i< MULTISERVO_NB_MAX ; i++ ) {
 135                 if(! g_multiservo.elts[i].port) {
 136                         g_multiservo.elts[i].port = port;
 137                         g_multiservo.elts[i].bitnum = bitnum;
 138                         g_multiservo.elts[i].value = TIME_1_5MS; /* dummy (center multiservo) */
 139                         sbi(DDR(*port), bitnum); /* DDR */
 140                         break;
 141                 }
 142         }
 143         IRQ_UNLOCK(flags);
 144
 145         /* if found, return id, else -1 */
 146         if(i == MULTISERVO_NB_MAX)
 147                 return -1;
 148
 149         return i;
 150 }
 151
 152
 153
 154
 155 void multiservo_del(int8_t id)
 156 {
 157         uint8_t flags;
 158
 159         IRQ_LOCK(flags);
 160         cbi(DDR(*g_multiservo.elts[id].port), g_multiservo.elts[id].bitnum); /* DDR */
 161         memset(&g_multiservo.elts[id], 0, sizeof(struct multiservo_element));
 162         IRQ_UNLOCK(flags);
 163 }
 164
 165
 166 /**
 167  * Set multiservo angle. Specify value in us.
 168  * WARNING : should be (much) bigger than 0
 169  */
 170 void multiservo_set(int8_t id, uint16_t val_us)
 171 {
 172         uint16_t val_timer;
 173         uint8_t flags;
 174
 175         val_timer = (((uint32_t)val_us)*TIME_1MS)/1000;
 176         IRQ_LOCK(flags);
 177         /* XXX convert us to counter unit */
 178         g_multiservo.elts[id].value = val_timer;
 179         IRQ_UNLOCK(flags);
 180 }
 181