diff --git a/libduck/libduck.c b/libduck/libduck.c index 54f3d6cac43c8f33113e51226fe4ad38aa671242..7556dfe3c259f412236c686982369867cc240cc7 100644 --- a/libduck/libduck.c +++ b/libduck/libduck.c @@ -13,25 +13,35 @@ static int duckfd = 0; +int duck_debug_mode = 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); + if (duck_debug_mode) { + printf("Opening duck\n"); + } else { + 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"); + if (duck_debug_mode) { + printf("Configuring duck\n"); + } else { + 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"); + } } } @@ -39,7 +49,11 @@ static int duck_printf(const char *fmt, ...) { va_list ap; va_start(ap, fmt); - return vdprintf(duckfd, fmt, ap) < 0; + if (duck_debug_mode) { + return vprintf(fmt, ap); + } else { + return vdprintf(duckfd, fmt, ap) < 0; + } } static int validate_motor(int motor) @@ -47,10 +61,17 @@ static int validate_motor(int motor) return motor >= 1 && motor <= 3; } +static int wrap_angle(int angle) +{ + while (angle > 180) angle -= 360; + while (angle < -180) angle += 360; + return angle; +} + int duck_set_position(int motor, int angle) { if (!validate_motor(motor)) return -1; - if (abs(angle) > 180) return -1; + angle = wrap_angle(angle); return duck_printf("s %d %d\n", motor, angle); } @@ -68,5 +89,9 @@ int duck_set_velocity(int motor, int deg_per_sec) void close_duck(void) { - close(duckfd); + if (duck_debug_mode) { + printf("Closing duck\n"); + } else { + close(duckfd); + } } diff --git a/libduck/libduck.h b/libduck/libduck.h index 081bdeb2c79caa24a485c356cc0147dc75bf3e97..b938702fa563c4e1d0a00352417c35d25e3eed17 100644 --- a/libduck/libduck.h +++ b/libduck/libduck.h @@ -9,6 +9,8 @@ # define DEFAULT_DUCK_FNAME "/dev/ttyUSB0" #endif /* DEFAULT_DUCK_FNAME */ +extern int duck_debug_mode; + void open_duck(const char *fname); void configure_duck(void); int duck_set_position(int motor, int angle); diff --git a/libduck/libduck_debug.c b/libduck/libduck_debug.c deleted file mode 100644 index d1da75bb4be59b146ec77fe1ffdd083ee9c62ce7..0000000000000000000000000000000000000000 --- a/libduck/libduck_debug.c +++ /dev/null @@ -1,72 +0,0 @@ -#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 vprintf(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); -}