test
[aversive.git] / modules / devices / servo / ax12 / ax12.c
1 /*  
2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2008)
3  *  JD Brossillon <jean.damien.brossillon@gmail.com>  (main job)
4  *  Olivier Matz <zer0@droids-corp.org>               (clean-up, fixes)
5  * 
6  *  This program is free software; you can redistribute it and/or modify
7  *  it under the terms of the GNU General Public License as published by
8  *  the Free Software Foundation; either version 2 of the License, or
9  *  (at your option) any later version.
10  *
11  *  This program is distributed in the hope that it will be useful,
12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  *  GNU General Public License for more details.
15  *
16  *  You should have received a copy of the GNU General Public License
17  *  along with this program; if not, write to the Free Software
18  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
19  *
20  *  Revision : $Id: ax12.c,v 1.1.4.3 2008-12-26 13:48:37 zer0 Exp $
21  *
22  */
23
24 /* See some help in ax12.h */
25
26 #include <stdio.h>
27 #include <string.h>
28 #include "ax12.h"
29 #ifdef CONFIG_MODULE_SCHEDULER
30 #include <scheduler.h>
31 #endif
32
33 void AX12_init(AX12 *s)
34 {
35         s->hardware_send = NULL;
36         s->hardware_recv = NULL;
37         s->hardware_switch = NULL;
38 }
39
40 /*_________________________________________________________*/
41 /*_________________________________________________________*/
42
43 void AX12_set_hardware_send(AX12 *s, int8_t(*pf)(uint8_t))
44 {
45         s->hardware_send = pf; 
46 }
47
48 void AX12_set_hardware_recv(AX12 *s, int16_t(*pf)(void))
49 {
50         s->hardware_recv = pf;
51 }
52
53 void AX12_set_hardware_switch(AX12 *s, void(*pf)(uint8_t))
54 {
55         s->hardware_switch = pf;
56 }
57
58 /*_________________________________________________________*/
59 /*_________________________________________________________*/
60
61 /* process the cksum of a packet */
62 uint8_t AX12_checksum(AX12_Packet *packet)
63 {
64         uint8_t i;
65         uint8_t checksum=0;
66
67         checksum += packet->id;
68         checksum += packet->instruction;
69   
70         /* Packet also contain length = number of params + 2 */
71         checksum += packet->nparams + 2;
72   
73         for (i=0; i<packet->nparams; i++)
74                 checksum += packet->params[i];
75   
76         return ~checksum;
77 }
78
79 /* Send a packet to ax12. Return 0 on sucess. */
80 uint8_t AX12_send(AX12 *s, AX12_Packet *packet)
81 {
82         uint8_t i;
83         uint8_t flags;
84         uint8_t err;
85
86 #ifdef CONFIG_MODULE_SCHEDULER
87         uint8_t scheduler_prio;
88
89         scheduler_prio = scheduler_disable_save();
90 #endif
91
92         if (packet->nparams > AX12_MAX_PARAMS) {
93                 err = AX12_ERROR_TYPE_INVALID_PACKET;
94                 goto fail;
95         }
96
97         /* Switch line to write */
98         s->hardware_switch(AX12_STATE_WRITE);
99
100         /* Header */
101         if (s->hardware_send(0xFF)) {
102                 err = AX12_ERROR_TYPE_XMIT_FAILED;
103                 goto fail;
104         }
105
106         if (s->hardware_send(0xFF)) {
107                 err = AX12_ERROR_TYPE_XMIT_FAILED;
108                 goto fail;
109         }
110
111         /* AX12 ID */
112         if (s->hardware_send(packet->id)) {
113                 err = AX12_ERROR_TYPE_XMIT_FAILED;
114                 goto fail;
115         }
116
117         /* Packet length */
118         if (s->hardware_send(packet->nparams + 2)) {
119                 err = AX12_ERROR_TYPE_XMIT_FAILED;
120                 goto fail;
121         }
122
123         /* Instruction */
124         if (s->hardware_send(packet->instruction)) {
125                 err = AX12_ERROR_TYPE_XMIT_FAILED;
126                 goto fail;
127         }
128  
129         /* Params */
130         for (i=0; i<(packet->nparams); i++) {
131                 if (s->hardware_send(packet->params[i])) {
132                         err = AX12_ERROR_TYPE_XMIT_FAILED;
133                         goto fail;
134                 }
135         }
136
137         IRQ_LOCK(flags); /* needed if hardware_switch is
138                             synchronous  */
139
140         /* Checksum */
141         if (s->hardware_send(AX12_checksum(packet))) {
142                 IRQ_UNLOCK(flags);
143                 err = AX12_ERROR_TYPE_XMIT_FAILED;
144                 goto fail;
145         }
146
147         /* Switch line back to read */
148         s->hardware_switch(AX12_STATE_READ);
149  
150         IRQ_UNLOCK(flags);      
151
152 #ifdef CONFIG_MODULE_SCHEDULER
153         scheduler_enable_restore(scheduler_prio);
154 #endif
155         return 0;
156
157  fail:
158         /* Switch line back to read */
159         s->hardware_switch(AX12_STATE_READ);
160 #ifdef CONFIG_MODULE_SCHEDULER
161         scheduler_enable_restore(scheduler_prio);
162 #endif
163         return err;
164 }
165
166 /* Receive a packet from ax12. Return 0 on sucess. */
167 uint8_t AX12_recv(AX12 *s, AX12_Packet *packet)
168 {
169         int16_t c;
170         uint8_t length;
171         uint8_t i;
172
173         /* Switch line to read */
174         s->hardware_switch(AX12_STATE_READ);
175    
176         c = s->hardware_recv();
177         if (c == -1)
178                 return AX12_ERROR_TYPE_NO_ANSWER;
179         if (c != 0xFF) 
180                 return AX12_ERROR_TYPE_INVALID_PACKET;
181
182         c = s->hardware_recv();
183         if (c == -1)
184                 return AX12_ERROR_TYPE_TIMEOUT;
185         if (c != 0xFF)
186                 return AX12_ERROR_TYPE_INVALID_PACKET;
187
188         c = s->hardware_recv();
189         if (c == -1)
190                 return AX12_ERROR_TYPE_TIMEOUT;
191         packet->id = c; /* we should check id ? */
192
193         c = s->hardware_recv();
194         if (c == -1)
195                 return AX12_ERROR_TYPE_TIMEOUT;
196         length = c;
197         packet->nparams = length - 2;
198         if (packet->nparams > AX12_MAX_PARAMS)
199                 return AX12_ERROR_TYPE_INVALID_PACKET;
200
201         c = s->hardware_recv();
202         if (c == -1)
203                 return AX12_ERROR_TYPE_TIMEOUT;
204         packet->error = c;
205         if (packet->error)
206                 return packet->error;
207   
208         for (i=0; i<(length-2); i++) {
209                 c = s->hardware_recv();
210                 if (c == -1)
211                         return AX12_ERROR_TYPE_TIMEOUT;
212                 packet->params[i] = c;
213         }
214   
215         /* Checksum */
216         c = s->hardware_recv();
217   
218         if (c != AX12_checksum(packet))
219                 return AX12_ERROR_TYPE_BAD_CKSUM;
220
221         return 0;
222 }
223
224 /*_________________________________________________________*/
225 /*_________________________________________________________*/
226
227 /* Write a byte to an adress. Return 0 on success. */ 
228 uint8_t AX12_write_byte(AX12 *s, uint8_t id, AX12_ADDRESS address, 
229                         uint8_t data)
230 {
231         AX12_Packet p, rp;
232         uint8_t ret;
233
234         memset(&p, 0, sizeof(p));
235         memset(&rp, 0, sizeof(rp));
236   
237         p.id = id;
238         p.instruction = AX12_WRITE;
239         p.nparams = 2;
240    
241         /* memory address */
242         p.params[0] = (uint8_t)address;
243   
244         /* value */
245         p.params[1] = data;
246
247         /* send packet */
248         ret = AX12_send(s, &p);
249         if (ret)
250                 return ret;
251
252         /* We talk broadcast, no reply */
253         if (p.id == AX12_BROADCAST_ID)
254                 return 0;
255
256         /* recv packet */
257         return AX12_recv(s, &rp);
258 }
259
260 /* Write a word to an adress. Return 0 on success. */ 
261 uint8_t AX12_write_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
262                        uint16_t data)
263 {
264         AX12_Packet p, rp;
265         uint8_t ret;
266  
267         memset(&p, 0, sizeof(p));
268         memset(&rp, 0, sizeof(rp));
269
270         p.id = id;
271         p.instruction = AX12_WRITE;
272         p.nparams = 3;
273    
274         /* memory address */
275         p.params[0] = (uint8_t)address;
276   
277         /* value (low) */
278         p.params[1] = 0xFF & data;
279
280         /* value (high) */
281         p.params[2] = data>>8;
282
283         /* send packet */
284         ret = AX12_send(s, &p);
285         if (ret)
286                 return ret;
287
288         /* We talk broadcast, no reply */
289         if (p.id == AX12_BROADCAST_ID)
290                 return 0;
291   
292         /* recv packet */
293         return AX12_recv(s, &rp);
294 }
295
296 /* Read a byte at given adress. On success, fill val and return 0,
297  * else return error and keep val unmodified. */ 
298 uint8_t AX12_read_byte(AX12 *s, uint8_t id, AX12_ADDRESS address,
299                        uint8_t *val)
300 {
301         AX12_Packet p, rp;
302         uint8_t ret;
303
304         memset(&p, 0, sizeof(p));
305         memset(&rp, 0, sizeof(rp));
306
307         p.id = id;
308         p.instruction = AX12_READ;
309         p.nparams = 2;
310
311         /* memory address */
312         p.params[0] = (uint8_t)address;
313   
314         /* data length */
315         p.params[1] = 1;
316
317         /* send packet */
318         ret = AX12_send(s, &p);
319         if (ret)
320                 return ret;
321
322         ret = AX12_recv(s, &rp);
323         if (ret)
324                 return ret;
325
326         *val = rp.params[0];
327         return 0;
328 }
329
330 uint8_t AX12_read_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
331                       uint16_t *val)
332 {
333         AX12_Packet p, rp;
334         uint8_t ret;
335
336         memset(&p, 0, sizeof(p));
337         memset(&rp, 0, sizeof(rp));
338   
339         p.id = id;
340         p.instruction = AX12_READ;
341         p.nparams = 2;
342
343         /* memory address */
344         p.params[0] = (uint8_t)address;
345   
346         /* data length */
347         p.params[1] = 2;
348
349         /* send packet */
350         ret = AX12_send(s, &p);
351         if (ret)
352                 return ret;
353
354         ret = AX12_recv(s, &rp);
355         if (ret)
356                 return ret;
357
358         *val = rp.params[0] + ((rp.params[1])<<8);
359         return 0;
360 }
361
362 /*_________________________________________________________*/
363 /*_________________________________________________________*/
364
365 uint8_t AX12_set_position(AX12 *s,uint8_t id, uint16_t position)
366 {
367         return AX12_write_int(s, id, AA_GOAL_POSITION_L, position);
368 }
369
370 uint8_t AX12_set_position2(AX12 *s, uint8_t id, uint16_t position,
371                            uint16_t speed)
372 {
373         AX12_Packet p, rp;
374         uint8_t ret;
375   
376         p.id = id;
377         p.instruction = AX12_WRITE;
378         p.nparams = 5;
379    
380         /* memory address */
381         p.params[0] = AA_GOAL_POSITION_L;
382         /* position */
383         p.params[1] = 0xFF & position;
384         p.params[2] = position>>8;
385         /* speed */
386         p.params[3] = 0xFF & speed;
387         p.params[4] = speed>>8;
388
389         /* send packet */
390         ret = AX12_send(s, &p);
391         if (ret)
392                 return ret;
393
394         /* We talk broadcast, no reply */
395         if(p.id == AX12_BROADCAST_ID)
396                 return 0;
397   
398         /* recv packet */
399         return AX12_recv(s, &rp);
400 }
401
402 uint8_t AX12_set_position3(AX12*s, uint8_t id, uint16_t position,
403                            uint16_t speed, uint16_t torque)
404 {
405         AX12_Packet p, rp;
406         uint8_t ret;
407  
408         p.id = id;
409         p.instruction = AX12_WRITE;
410         p.nparams = 7;
411    
412         /* memory address */
413         p.params[0] = AA_GOAL_POSITION_L;
414         /* position */
415         p.params[1] = 0xFF & position;
416         p.params[2] = position>>8;
417         /* speed */
418         p.params[3] = 0xFF & speed;
419         p.params[4] = speed>>8;
420         /* torque */
421         p.params[5] = 0xFF & torque;
422         p.params[6] = torque>>8;
423
424         /* send packet */
425         ret = AX12_send(s, &p);
426         if (ret)
427                 return ret;
428
429         /* We talk broadcast, no reply */
430         if(p.id == AX12_BROADCAST_ID)
431                 return 0;
432   
433         /* recv packet */
434         return AX12_recv(s, &rp);
435 }
436
437 uint8_t AX12_get_position(AX12 *s, uint8_t id, uint16_t *pos)
438 {
439         return AX12_read_int(s, id, AA_PRESENT_POSITION_L, pos);
440 }
441
442 uint8_t AX12_get_speed(AX12 *s, uint8_t id, uint16_t *speed)
443 {
444         return AX12_read_int(s, id, AA_PRESENT_SPEED_L, speed);
445 }
446
447 uint8_t AX12_get_load(AX12 *s, uint8_t id, uint16_t *load)
448 {
449         return AX12_read_int(s, id, AA_PRESENT_LOAD_L, load);
450 }
451
452 uint8_t AX12_ping(AX12 *s, uint8_t id)
453 {
454         AX12_Packet p, rp;
455         uint8_t ret;
456  
457         p.id = id;
458         p.instruction = AX12_PING;
459         p.nparams = 0;
460    
461         /* send packet */
462         ret = AX12_send(s, &p);
463         if (ret)
464                 return ret;
465
466         /* We talk broadcast, no reply */
467         if(p.id == AX12_BROADCAST_ID)
468                 return 0;
469   
470         /* recv packet */
471         return AX12_recv(s, &rp);
472 }
473
474 uint8_t AX12_reset(AX12 *s, uint8_t id)
475 {
476         AX12_Packet p, rp;
477         uint8_t ret;
478   
479         p.id = id;
480         p.instruction = AX12_RESET;
481         p.nparams = 0;
482    
483         /* send packet */
484         ret = AX12_send(s, &p);
485         if (ret)
486                 return ret;
487
488         /* We talk broadcast, no reply */
489         if(p.id == AX12_BROADCAST_ID)
490                 return 0;
491   
492         /* recv packet */
493         return AX12_recv(s, &rp);
494 }