hexapod_ng/legacy/linux/liblicks/servo/servo.c

#include "servo.h"

#include <string.h>
#include <stdio.h>
#include <fcntl.h>

#define NUM_SERVOS 24

uint32_t servo_pwm[NUM_SERVOS]={};
uint32_t servo_offsets[NUM_SERVOS]={
 0x432b, 0x3311, 0x2a88,
 0x3311, 0x2f77, 0x28b5,
 0x225d, 0x31bd, 0x2bed,
 0x3cf6, 0x322b, 0x3c65,
 0x344b, 0x2d96, 0x2ed1,
 0x279c, 0x2ffa, 0x39df,
 0x0000, 0x0000, 0x0000,
 0x0000, 0x0000, 0x0000,
};


void
load_calibration(char *filename) {
   unsigned int load_buffer[NUM_SERVOS];
   int bytes_read;
   int fd;
   fd=open(filename,O_RDONLY);
   if(fd!=-1) {
      bytes_read=read(fd,load_buffer,sizeof(load_buffer));
      if(bytes_read==sizeof(load_buffer)) {
         memcpy(servo_offsets,load_buffer,sizeof(load_buffer));
      } else {
         printf("load_calibration(%s): short read\n",filename);
      }
      close(fd);
   } else {
      perror(filename);
   }
}