Rev 282 | Rev 285 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
281 | giacomo | 1 | /* |
2 | * Project: S.Ha.R.K. |
||
3 | * |
||
4 | * Coordinators: |
||
5 | * Giorgio Buttazzo <giorgio@sssup.it> |
||
6 | * Paolo Gai <pj@gandalf.sssup.it> |
||
7 | * |
||
8 | * Authors : |
||
9 | * Giacomo Guidi <giacomo@gandalf.sssup.it> |
||
10 | * (see the web pages for full authors list) |
||
11 | * |
||
12 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
13 | * |
||
14 | * http://www.sssup.it |
||
15 | * http://retis.sssup.it |
||
16 | * http://shark.sssup.it |
||
17 | */ |
||
18 | |||
19 | /* |
||
20 | * Copyright (C) 2002 Paolo Gai |
||
21 | * |
||
22 | * This program is free software; you can redistribute it and/or modify |
||
23 | * it under the terms of the GNU General Public License as published by |
||
24 | * the Free Software Foundation; either version 2 of the License, or |
||
25 | * (at your option) any later version. |
||
26 | * |
||
27 | * This program is distributed in the hope that it will be useful, |
||
28 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
29 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
30 | * GNU General Public License for more details. |
||
31 | * |
||
32 | * You should have received a copy of the GNU General Public License |
||
33 | * along with this program; if not, write to the Free Software |
||
34 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
35 | * |
||
36 | */ |
||
37 | |||
38 | #include <ll/sys/ll/ll-instr.h> |
||
282 | giacomo | 39 | #include "kernel/kern.h" |
281 | giacomo | 40 | #include "servo.h" |
41 | |||
42 | #define THR 0 |
||
43 | #define RBR 0 |
||
44 | #define IER 1 |
||
45 | #define FCR 2 |
||
46 | #define IIR 2 |
||
47 | #define LCR 3 |
||
48 | #define MCR 4 |
||
49 | #define LSR 5 |
||
50 | #define MSR 6 |
||
51 | #define SPad 7 |
||
52 | |||
53 | #define barrier() __asm__ __volatile__("" ::: "memory"); |
||
54 | |||
282 | giacomo | 55 | #define SERVO_TIMEOUT 100000 /* us */ |
281 | giacomo | 56 | |
282 | giacomo | 57 | #define SERVO_PORT 1 |
58 | |||
281 | giacomo | 59 | int timer_expired = 0; |
283 | giacomo | 60 | int timeout_event; |
281 | giacomo | 61 | unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
62 | |||
282 | giacomo | 63 | void set_timer_expired(void *arg) |
281 | giacomo | 64 | { |
283 | giacomo | 65 | timeout_event = NIL; |
281 | giacomo | 66 | timer_expired = 1; |
67 | } |
||
68 | |||
69 | unsigned com_read(unsigned port,unsigned reg) |
||
70 | { |
||
71 | unsigned b; |
||
72 | if (port > 3 || reg > 7) return(0); |
||
73 | b = ll_in(com_base[port]+reg); |
||
74 | return(b); |
||
75 | } |
||
76 | |||
77 | void com_write(unsigned port,unsigned reg,unsigned value) |
||
78 | { |
||
79 | if (port > 3 || reg > 7) return; |
||
80 | ll_out(com_base[port]+reg,value); |
||
81 | } |
||
82 | |||
83 | int com_send(unsigned port,BYTE b) |
||
84 | { |
||
85 | while ((com_read(port,LSR) & 32) == 0 && !timer_expired) |
||
86 | barrier(); |
||
87 | if (!timer_expired) { |
||
88 | com_write(port,THR,b); |
||
89 | return 0; |
||
90 | } else { |
||
91 | return -1; |
||
92 | } |
||
93 | } |
||
94 | |||
95 | int com_receive(unsigned port) |
||
96 | { |
||
97 | while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
||
98 | barrier(); |
||
99 | if (!timer_expired) { |
||
100 | return((int)(com_read(port,RBR))); |
||
101 | } else { |
||
102 | return -1; |
||
103 | } |
||
104 | } |
||
105 | |||
106 | int servo_set_RS232_baudrate(int baud) |
||
107 | { |
||
108 | |||
109 | |||
110 | return 0; |
||
111 | |||
112 | } |
||
113 | |||
114 | int servo_get_RS232_baudrate(void) |
||
115 | { |
||
116 | |||
117 | return 0; |
||
118 | |||
119 | } |
||
120 | |||
121 | int servo_store_RS232_buadrate(void) |
||
122 | { |
||
123 | |||
124 | return 0; |
||
125 | |||
126 | } |
||
127 | |||
128 | int servo_set_period(int period) |
||
129 | { |
||
130 | |||
131 | return 0; |
||
132 | |||
133 | } |
||
134 | |||
135 | int servo_get_period(void) |
||
136 | { |
||
137 | |||
138 | return 0; |
||
139 | |||
140 | } |
||
141 | |||
142 | int servo_store_period(void) |
||
143 | { |
||
144 | |||
145 | return 0; |
||
146 | |||
147 | } |
||
148 | |||
283 | giacomo | 149 | /* 1000.1100 */ |
281 | giacomo | 150 | int servo_get_setup_switch(void) |
151 | { |
||
283 | giacomo | 152 | struct timespec current_time; |
153 | unsigned char b; |
||
154 | int err,res; |
||
155 | timer_expired = 0; |
||
156 | kern_gettime(¤t_time); |
||
157 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
158 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 159 | |
283 | giacomo | 160 | b = 0x8C; |
161 | err = com_send(SERVO_PORT, b); |
||
162 | err = com_receive(SERVO_PORT); |
||
163 | if (err != (int)(b)) timer_expired = 1; |
||
164 | res = com_receive(SERVO_PORT); |
||
281 | giacomo | 165 | |
283 | giacomo | 166 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
167 | |||
168 | if (!timer_expired) |
||
169 | return res; |
||
170 | else |
||
171 | return -1; |
||
172 | |||
281 | giacomo | 173 | } |
174 | |||
283 | giacomo | 175 | /* 1000.111s */ |
281 | giacomo | 176 | int servo_set_RC5_switch(int data) |
177 | { |
||
283 | giacomo | 178 | struct timespec current_time; |
179 | unsigned char b; |
||
180 | int err; |
||
181 | |||
182 | timer_expired = 0; |
||
183 | kern_gettime(¤t_time); |
||
184 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
185 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 186 | |
283 | giacomo | 187 | b = 0x8E | (data & 0x01); |
188 | err = com_send(SERVO_PORT, b); |
||
189 | err = com_receive(SERVO_PORT); |
||
190 | if (err != (int)(b)) timer_expired = 1; |
||
191 | |||
192 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
193 | |||
194 | if (!timer_expired) |
||
195 | return 0; |
||
196 | else |
||
197 | return -1; |
||
198 | |||
281 | giacomo | 199 | } |
200 | |||
282 | giacomo | 201 | /* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
281 | giacomo | 202 | int servo_set_angle(int servo, int angle) |
203 | { |
||
204 | |||
282 | giacomo | 205 | struct timespec current_time; |
206 | unsigned char b; |
||
283 | giacomo | 207 | int err; |
282 | giacomo | 208 | |
209 | if (servo > 7) return -1; |
||
210 | |||
211 | timer_expired = 0; |
||
212 | kern_gettime(¤t_time); |
||
213 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
214 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
215 | |||
216 | b = 0x00 | (servo & 0x07); |
||
217 | err = com_send(SERVO_PORT, b); |
||
218 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 219 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 220 | |
221 | b = 0x00 | ((angle >> 8) & 0x0F); |
||
222 | err = com_send(SERVO_PORT, b); |
||
223 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 224 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 225 | |
226 | b = 0x00 | (angle & 0xFF); |
||
227 | err = com_send(SERVO_PORT, b); |
||
228 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 229 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 230 | |
283 | giacomo | 231 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 232 | |
283 | giacomo | 233 | if (!timer_expired) |
234 | return 0; |
||
235 | else |
||
236 | return -1; |
||
281 | giacomo | 237 | |
238 | } |
||
239 | |||
282 | giacomo | 240 | /* 0000.1ppp */ |
281 | giacomo | 241 | int servo_get_angle(int servo) |
242 | { |
||
282 | giacomo | 243 | struct timespec current_time; |
244 | unsigned char b; |
||
283 | giacomo | 245 | int err,res; |
282 | giacomo | 246 | |
247 | if (servo > 7) return -1; |
||
248 | |||
249 | timer_expired = 0; |
||
250 | kern_gettime(¤t_time); |
||
251 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
252 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 253 | |
282 | giacomo | 254 | b = 0x08 | (servo & 0x07); |
255 | err = com_send(SERVO_PORT, b); |
||
256 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 257 | if (err != (int)(b)) timer_expired = 1; |
258 | res = com_receive(SERVO_PORT) << 8; |
||
259 | res |= com_receive(SERVO_PORT); |
||
282 | giacomo | 260 | |
283 | giacomo | 261 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 262 | |
263 | if (!timer_expired) |
||
264 | return res; |
||
265 | else |
||
266 | return -1; |
||
267 | |||
281 | giacomo | 268 | } |
269 | |||
282 | giacomo | 270 | /* 0100:0aaa */ |
281 | giacomo | 271 | int servo_get_analog(int port) |
272 | { |
||
273 | |||
282 | giacomo | 274 | struct timespec current_time; |
275 | unsigned char b; |
||
283 | giacomo | 276 | int err,res; |
282 | giacomo | 277 | |
278 | if (port > 4) return -1; |
||
279 | |||
280 | timer_expired = 0; |
||
281 | kern_gettime(¤t_time); |
||
282 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
283 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 284 | |
282 | giacomo | 285 | b = 0x40 | (port & 7); |
286 | err = com_send(SERVO_PORT, b); |
||
287 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 288 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 289 | res = com_receive(SERVO_PORT) << 8; |
290 | res |= com_receive(SERVO_PORT); |
||
291 | |||
283 | giacomo | 292 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 293 | |
294 | if (!timer_expired) |
||
295 | return res; |
||
296 | else |
||
297 | return -1; |
||
298 | |||
281 | giacomo | 299 | } |
300 |