diff --git a/libduck/Makefile b/libduck/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..01d8135685a171bcf9ddb83a1a76b533fdf12c09 --- /dev/null +++ b/libduck/Makefile @@ -0,0 +1,13 @@ + + + +libduck.so: libduck.c libduck.h + cc -c libduck.c -shared -pie -o libduck.so + +debug: libduck.c libduck.h + cc -c libduck_debug.c -shared -pie -o libduck.so + +.PHONY: clean + +clean: + rm -f libduck.so diff --git a/libduck/libduck.c b/libduck/libduck.c new file mode 100644 index 0000000000000000000000000000000000000000..54f3d6cac43c8f33113e51226fe4ad38aa671242 --- /dev/null +++ b/libduck/libduck.c @@ -0,0 +1,72 @@ +#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); +} diff --git a/libduck/libduck.h b/libduck/libduck.h new file mode 100644 index 0000000000000000000000000000000000000000..e08bc5311a1276b32d063fa61e5242a860693df3 --- /dev/null +++ b/libduck/libduck.h @@ -0,0 +1,14 @@ +#define MOTOR_1 1 +#define MOTOR_2 2 +#define MOTOR_3 3 + +#ifndef DEFAULT_DUCK_FNAME +# define DEFAULT_DUCK_FNAME "/dev/ttyUSB0" +#endif /* DEFAULT_DUCK_FNAME */ + +void open_duck(const char *fname); +void configure_duck(void); +int duck_set_position(int motor, int angle); +int duck_delay(int ms); +int duck_set_velocity(int motor, int deg_per_sec); +void close_duck(void); diff --git a/libduck/libduck_debug.c b/libduck/libduck_debug.c new file mode 100644 index 0000000000000000000000000000000000000000..d1da75bb4be59b146ec77fe1ffdd083ee9c62ce7 --- /dev/null +++ b/libduck/libduck_debug.c @@ -0,0 +1,72 @@ +#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); +}