libduck.c

#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <err.h>
#include <stdlib.h>
#include <termios.h>
#include <stdarg.h>
#include "libduck.h"

#ifndef DUCK_BAUD
# define DUCK_BAUD B115200
#endif /* DUCK_BAUD */

static int duckfd = 0;

void open_duck(const char *fname)
{
     duckfd = open(fname, O_NOCTTY | O_RDWR);
     
     if (duckfd == -1) {
	  err(EXIT_FAILURE, "Failed to open '%s'", fname);
     }
}

void configure_duck(void)
{
     struct termios tio;
     if (tcgetattr(duckfd, &tio)) {
	  err(EXIT_FAILURE, "tcgetattr failed");
     }
     cfsetospeed(&tio, DUCK_BAUD);

     if (tcsetattr(duckfd, TCSANOW, &tio)) {
	  err(EXIT_FAILURE, "tcsetattr failed");
     }
}

static int duck_printf(const char *fmt, ...)
{
     va_list ap;
     va_start(ap, fmt);
     return vdprintf(duckfd, fmt, ap) < 0;
}

static int validate_motor(int motor)
{
     return motor >= 1 && motor <= 3;
}

int duck_set_position(int motor, int angle)
{
     if (!validate_motor(motor)) return -1;
     if (abs(angle) > 180) return -1;
     return duck_printf("s %d %d\n", motor, angle);
}

int duck_delay(int ms)
{
     return duck_printf("d %d\n", ms);
}

int duck_set_velocity(int motor, int deg_per_sec)
{
     if (!validate_motor(motor)) return -1;
     if (abs(deg_per_sec) > 1000) return -1;
     return duck_printf("v %d %d\n", motor, deg_per_sec);
}

void close_duck(void)
{
     close(duckfd);
}