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 }