2 * Copyright Droids Corporation, Microb Technology, Eirbot (2006)
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.
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.
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
18 * Revision : $Id: multiservo.c,v 1.5.4.5 2008-03-02 17:18:16 zer0 Exp $
25 #include <aversive/error.h>
26 #include <multiservo.h>
27 #include <multiservo_archs.h>
29 struct multiservo g_multiservo;
31 //#define MULTISERVO_ENABLE_DEBUG
33 #ifdef MULTISERVO_ENABLE_DEBUG
34 #define MULTISERVO_DEBUG(args...) DEBUG(args)
36 #define MULTISERVO_DEBUG(args...) do { } while(0)
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)
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"
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"
55 SIGNAL(MULTISERVO_SIG_OUTPUT_COMPARE) /* other ints NOT allowed */
57 volatile uint8_t * port ;
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;
66 if (g_multiservo.id_prev != -1) {
67 port = g_multiservo.elts[g_multiservo.id_prev].port;
69 MULTISERVO_DEBUG(E_MULTISERVO, "reset %d",g_multiservo.id_prev);
70 cbi(*port, g_multiservo.elts[g_multiservo.id_prev].bitnum);
75 while (g_multiservo.current_multiservo < MULTISERVO_NB_MAX) {
76 port = g_multiservo.elts[g_multiservo.current_multiservo].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;
87 g_multiservo.current_multiservo ++;
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;
98 g_multiservo.current_multiservo ++;
104 void multiservo_init(void)
110 cbi(MULTISERVO_TIMSK, MULTISERVO_OCIE);
112 memset(&g_multiservo, 0, sizeof(g_multiservo));
113 g_multiservo.id_prev = -1;
115 /* Timer config (see in multiservo_archs.h) */
116 MULTISERVO_TCCRnA = MULTISERVO_TCCRnA_VALUE;
117 #ifdef MULTISERVO_TCCRnB
118 MULTISERVO_TCCRnB = MULTISERVO_TCCRnB_VALUE;
121 MULTISERVO_OCR = TIME_1MS;
122 sbi(MULTISERVO_TIMSK, MULTISERVO_OCIE);
127 int8_t multiservo_add(volatile uint8_t * port, uint8_t bitnum)
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 */
145 /* if found, return id, else -1 */
146 if(i == MULTISERVO_NB_MAX)
155 void multiservo_del(int8_t id)
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));
167 * Set multiservo angle. Specify value in us.
168 * WARNING : should be (much) bigger than 0
170 void multiservo_set(int8_t id, uint16_t val_us)
175 val_timer = (((uint32_t)val_us)*TIME_1MS)/1000;
177 /* XXX convert us to counter unit */
178 g_multiservo.elts[id].value = val_timer;