AVR: aversive_10-03-12/modules/devices/servo/ax12/ax12.c Source File

00001 /*  
00002  *  Copyright Droids Corporation, Microb Technology, Eirbot (2008)
00003  *  JD Brossillon <jean.damien.brossillon@gmail.com>  (main job)
00004  *  Olivier Matz <zer0@droids-corp.org>               (clean-up, fixes)
00005  * 
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  *  Revision : $Id: ax12.c,v 1.2 2009-03-15 21:51:18 zer0 Exp $
00021  *
00022  */
00023 
00024 /* See some help in ax12.h */
00025 
00026 #include <stdio.h>
00027 #include <string.h>
00028 #include "ax12.h"
00029 #ifdef CONFIG_MODULE_SCHEDULER
00030 #include <scheduler.h>
00031 #endif
00032 
00033 void AX12_init(AX12 *s)
00034 {
00035         s->hardware_send = NULL;
00036         s->hardware_recv = NULL;
00037         s->hardware_switch = NULL;
00038 }
00039 
00040 /*_________________________________________________________*/
00041 /*_________________________________________________________*/
00042 
00043 void AX12_set_hardware_send(AX12 *s, int8_t(*pf)(uint8_t))
00044 {
00045         s->hardware_send = pf; 
00046 }
00047 
00048 void AX12_set_hardware_recv(AX12 *s, int16_t(*pf)(void))
00049 {
00050         s->hardware_recv = pf;
00051 }
00052 
00053 void AX12_set_hardware_switch(AX12 *s, void(*pf)(uint8_t))
00054 {
00055         s->hardware_switch = pf;
00056 }
00057 
00058 /*_________________________________________________________*/
00059 /*_________________________________________________________*/
00060 
00061 /* process the cksum of a packet */
00062 uint8_t AX12_checksum(AX12_Packet *packet)
00063 {
00064         uint8_t i;
00065         uint8_t checksum=0;
00066 
00067         checksum += packet->id;
00068         checksum += packet->instruction;
00069   
00070         /* Packet also contain length = number of params + 2 */
00071         checksum += packet->nparams + 2;
00072   
00073         for (i=0; i<packet->nparams; i++)
00074                 checksum += packet->params[i];
00075   
00076         return ~checksum;
00077 }
00078 
00079 /* Send a packet to ax12. Return 0 on sucess. */
00080 uint8_t AX12_send(AX12 *s, AX12_Packet *packet)
00081 {
00082         uint8_t i;
00083         uint8_t flags;
00084         uint8_t err;
00085 
00086 #ifdef CONFIG_MODULE_SCHEDULER
00087         uint8_t scheduler_prio;
00088 
00089         scheduler_prio = scheduler_disable_save();
00090 #endif
00091 
00092         if (packet->nparams > AX12_MAX_PARAMS) {
00093                 err = AX12_ERROR_TYPE_INVALID_PACKET;
00094                 goto fail;
00095         }
00096 
00097         /* Switch line to write */
00098         s->hardware_switch(AX12_STATE_WRITE);
00099 
00100         /* Header */
00101         if (s->hardware_send(0xFF)) {
00102                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00103                 goto fail;
00104         }
00105 
00106         if (s->hardware_send(0xFF)) {
00107                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00108                 goto fail;
00109         }
00110 
00111         /* AX12 ID */
00112         if (s->hardware_send(packet->id)) {
00113                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00114                 goto fail;
00115         }
00116 
00117         /* Packet length */
00118         if (s->hardware_send(packet->nparams + 2)) {
00119                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00120                 goto fail;
00121         }
00122 
00123         /* Instruction */
00124         if (s->hardware_send(packet->instruction)) {
00125                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00126                 goto fail;
00127         }
00128  
00129         /* Params */
00130         for (i=0; i<(packet->nparams); i++) {
00131                 if (s->hardware_send(packet->params[i])) {
00132                         err = AX12_ERROR_TYPE_XMIT_FAILED;
00133                         goto fail;
00134                 }
00135         }
00136 
00137         IRQ_LOCK(flags); /* needed if hardware_switch is
00138                             synchronous  */
00139 
00140         /* Checksum */
00141         if (s->hardware_send(AX12_checksum(packet))) {
00142                 IRQ_UNLOCK(flags);
00143                 err = AX12_ERROR_TYPE_XMIT_FAILED;
00144                 goto fail;
00145         }
00146 
00147         /* Switch line back to read */
00148         s->hardware_switch(AX12_STATE_READ);
00149  
00150         IRQ_UNLOCK(flags);      
00151 
00152 #ifdef CONFIG_MODULE_SCHEDULER
00153         scheduler_enable_restore(scheduler_prio);
00154 #endif
00155         return 0;
00156 
00157  fail:
00158         /* Switch line back to read */
00159         s->hardware_switch(AX12_STATE_READ);
00160 #ifdef CONFIG_MODULE_SCHEDULER
00161         scheduler_enable_restore(scheduler_prio);
00162 #endif
00163         return err;
00164 }
00165 
00166 /* Receive a packet from ax12. Return 0 on sucess. */
00167 uint8_t AX12_recv(AX12 *s, AX12_Packet *packet)
00168 {
00169         int16_t c;
00170         uint8_t length;
00171         uint8_t i;
00172 
00173         /* Switch line to read */
00174         s->hardware_switch(AX12_STATE_READ);
00175    
00176         c = s->hardware_recv();
00177         if (c == -1)
00178                 return AX12_ERROR_TYPE_NO_ANSWER;
00179         if (c != 0xFF) 
00180                 return AX12_ERROR_TYPE_INVALID_PACKET;
00181 
00182         c = s->hardware_recv();
00183         if (c == -1)
00184                 return AX12_ERROR_TYPE_TIMEOUT;
00185         if (c != 0xFF)
00186                 return AX12_ERROR_TYPE_INVALID_PACKET;
00187 
00188         c = s->hardware_recv();
00189         if (c == -1)
00190                 return AX12_ERROR_TYPE_TIMEOUT;
00191         packet->id = c; /* we should check id ? */
00192 
00193         c = s->hardware_recv();
00194         if (c == -1)
00195                 return AX12_ERROR_TYPE_TIMEOUT;
00196         length = c;
00197         packet->nparams = length - 2;
00198         if (packet->nparams > AX12_MAX_PARAMS)
00199                 return AX12_ERROR_TYPE_INVALID_PACKET;
00200 
00201         c = s->hardware_recv();
00202         if (c == -1)
00203                 return AX12_ERROR_TYPE_TIMEOUT;
00204         packet->error = c;
00205         if (packet->error)
00206                 return packet->error;
00207   
00208         for (i=0; i<(length-2); i++) {
00209                 c = s->hardware_recv();
00210                 if (c == -1)
00211                         return AX12_ERROR_TYPE_TIMEOUT;
00212                 packet->params[i] = c;
00213         }
00214   
00215         /* Checksum */
00216         c = s->hardware_recv();
00217   
00218         if (c != AX12_checksum(packet))
00219                 return AX12_ERROR_TYPE_BAD_CKSUM;
00220 
00221         return 0;
00222 }
00223 
00224 /*_________________________________________________________*/
00225 /*_________________________________________________________*/
00226 
00227 /* Write a byte to an adress. Return 0 on success. */ 
00228 uint8_t AX12_write_byte(AX12 *s, uint8_t id, AX12_ADDRESS address, 
00229                         uint8_t data)
00230 {
00231         AX12_Packet p, rp;
00232         uint8_t ret;
00233 
00234         memset(&p, 0, sizeof(p));
00235         memset(&rp, 0, sizeof(rp));
00236   
00237         p.id = id;
00238         p.instruction = AX12_WRITE;
00239         p.nparams = 2;
00240    
00241         /* memory address */
00242         p.params[0] = (uint8_t)address;
00243   
00244         /* value */
00245         p.params[1] = data;
00246 
00247         /* send packet */
00248         ret = AX12_send(s, &p);
00249         if (ret)
00250                 return ret;
00251 
00252         /* We talk broadcast, no reply */
00253         if (p.id == AX12_BROADCAST_ID)
00254                 return 0;
00255 
00256         /* recv packet */
00257         return AX12_recv(s, &rp);
00258 }
00259 
00260 /* Write a word to an adress. Return 0 on success. */ 
00261 uint8_t AX12_write_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
00262                        uint16_t data)
00263 {
00264         AX12_Packet p, rp;
00265         uint8_t ret;
00266  
00267         memset(&p, 0, sizeof(p));
00268         memset(&rp, 0, sizeof(rp));
00269 
00270         p.id = id;
00271         p.instruction = AX12_WRITE;
00272         p.nparams = 3;
00273    
00274         /* memory address */
00275         p.params[0] = (uint8_t)address;
00276   
00277         /* value (low) */
00278         p.params[1] = 0xFF & data;
00279 
00280         /* value (high) */
00281         p.params[2] = data>>8;
00282 
00283         /* send packet */
00284         ret = AX12_send(s, &p);
00285         if (ret)
00286                 return ret;
00287 
00288         /* We talk broadcast, no reply */
00289         if (p.id == AX12_BROADCAST_ID)
00290                 return 0;
00291   
00292         /* recv packet */
00293         return AX12_recv(s, &rp);
00294 }
00295 
00296 /* Read a byte at given adress. On success, fill val and return 0,
00297  * else return error and keep val unmodified. */ 
00298 uint8_t AX12_read_byte(AX12 *s, uint8_t id, AX12_ADDRESS address,
00299                        uint8_t *val)
00300 {
00301         AX12_Packet p, rp;
00302         uint8_t ret;
00303 
00304         memset(&p, 0, sizeof(p));
00305         memset(&rp, 0, sizeof(rp));
00306 
00307         p.id = id;
00308         p.instruction = AX12_READ;
00309         p.nparams = 2;
00310 
00311         /* memory address */
00312         p.params[0] = (uint8_t)address;
00313   
00314         /* data length */
00315         p.params[1] = 1;
00316 
00317         /* send packet */
00318         ret = AX12_send(s, &p);
00319         if (ret)
00320                 return ret;
00321 
00322         ret = AX12_recv(s, &rp);
00323         if (ret)
00324                 return ret;
00325 
00326         *val = rp.params[0];
00327         return 0;
00328 }
00329 
00330 uint8_t AX12_read_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
00331                       uint16_t *val)
00332 {
00333         AX12_Packet p, rp;
00334         uint8_t ret;
00335 
00336         memset(&p, 0, sizeof(p));
00337         memset(&rp, 0, sizeof(rp));
00338   
00339         p.id = id;
00340         p.instruction = AX12_READ;
00341         p.nparams = 2;
00342 
00343         /* memory address */
00344         p.params[0] = (uint8_t)address;
00345   
00346         /* data length */
00347         p.params[1] = 2;
00348 
00349         /* send packet */
00350         ret = AX12_send(s, &p);
00351         if (ret)
00352                 return ret;
00353 
00354         ret = AX12_recv(s, &rp);
00355         if (ret)
00356                 return ret;
00357 
00358         *val = rp.params[0] + ((rp.params[1])<<8);
00359         return 0;
00360 }
00361 
00362 /*_________________________________________________________*/
00363 /*_________________________________________________________*/
00364 
00365 uint8_t AX12_set_position(AX12 *s,uint8_t id, uint16_t position)
00366 {
00367         return AX12_write_int(s, id, AA_GOAL_POSITION_L, position);
00368 }
00369 
00370 uint8_t AX12_set_position2(AX12 *s, uint8_t id, uint16_t position,
00371                            uint16_t speed)
00372 {
00373         AX12_Packet p, rp;
00374         uint8_t ret;
00375   
00376         p.id = id;
00377         p.instruction = AX12_WRITE;
00378         p.nparams = 5;
00379    
00380         /* memory address */
00381         p.params[0] = AA_GOAL_POSITION_L;
00382         /* position */
00383         p.params[1] = 0xFF & position;
00384         p.params[2] = position>>8;
00385         /* speed */
00386         p.params[3] = 0xFF & speed;
00387         p.params[4] = speed>>8;
00388 
00389         /* send packet */
00390         ret = AX12_send(s, &p);
00391         if (ret)
00392                 return ret;
00393 
00394         /* We talk broadcast, no reply */
00395         if(p.id == AX12_BROADCAST_ID)
00396                 return 0;
00397   
00398         /* recv packet */
00399         return AX12_recv(s, &rp);
00400 }
00401 
00402 uint8_t AX12_set_position3(AX12*s, uint8_t id, uint16_t position,
00403                            uint16_t speed, uint16_t torque)
00404 {
00405         AX12_Packet p, rp;
00406         uint8_t ret;
00407  
00408         p.id = id;
00409         p.instruction = AX12_WRITE;
00410         p.nparams = 7;
00411    
00412         /* memory address */
00413         p.params[0] = AA_GOAL_POSITION_L;
00414         /* position */
00415         p.params[1] = 0xFF & position;
00416         p.params[2] = position>>8;
00417         /* speed */
00418         p.params[3] = 0xFF & speed;
00419         p.params[4] = speed>>8;
00420         /* torque */
00421         p.params[5] = 0xFF & torque;
00422         p.params[6] = torque>>8;
00423 
00424         /* send packet */
00425         ret = AX12_send(s, &p);
00426         if (ret)
00427                 return ret;
00428 
00429         /* We talk broadcast, no reply */
00430         if(p.id == AX12_BROADCAST_ID)
00431                 return 0;
00432   
00433         /* recv packet */
00434         return AX12_recv(s, &rp);
00435 }
00436 
00437 uint8_t AX12_get_position(AX12 *s, uint8_t id, uint16_t *pos)
00438 {
00439         return AX12_read_int(s, id, AA_PRESENT_POSITION_L, pos);
00440 }
00441 
00442 uint8_t AX12_get_speed(AX12 *s, uint8_t id, uint16_t *speed)
00443 {
00444         return AX12_read_int(s, id, AA_PRESENT_SPEED_L, speed);
00445 }
00446 
00447 uint8_t AX12_get_load(AX12 *s, uint8_t id, uint16_t *load)
00448 {
00449         return AX12_read_int(s, id, AA_PRESENT_LOAD_L, load);
00450 }
00451 
00452 uint8_t AX12_ping(AX12 *s, uint8_t id)
00453 {
00454         AX12_Packet p, rp;
00455         uint8_t ret;
00456  
00457         p.id = id;
00458         p.instruction = AX12_PING;
00459         p.nparams = 0;
00460    
00461         /* send packet */
00462         ret = AX12_send(s, &p);
00463         if (ret)
00464                 return ret;
00465 
00466         /* We talk broadcast, no reply */
00467         if(p.id == AX12_BROADCAST_ID)
00468                 return 0;
00469   
00470         /* recv packet */
00471         return AX12_recv(s, &rp);
00472 }
00473 
00474 uint8_t AX12_reset(AX12 *s, uint8_t id)
00475 {
00476         AX12_Packet p, rp;
00477         uint8_t ret;
00478   
00479         p.id = id;
00480         p.instruction = AX12_RESET;
00481         p.nparams = 0;
00482    
00483         /* send packet */
00484         ret = AX12_send(s, &p);
00485         if (ret)
00486                 return ret;
00487 
00488         /* We talk broadcast, no reply */
00489         if(p.id == AX12_BROADCAST_ID)
00490                 return 0;
00491   
00492         /* recv packet */
00493         return AX12_recv(s, &rp);
00494 }