#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); }