From bb0a39ec2db99ed3eb2275868714d57ac7813464 Mon Sep 17 00:00:00 2001
From: jp7g21 <jp7g21@soton.ac.uk>
Date: Sat, 13 Nov 2021 17:11:20 +0000
Subject: [PATCH] Wrote libduck for interfacing with duck

---
 libduck/Makefile        | 13 ++++++++
 libduck/libduck.c       | 72 +++++++++++++++++++++++++++++++++++++++++
 libduck/libduck.h       | 14 ++++++++
 libduck/libduck_debug.c | 72 +++++++++++++++++++++++++++++++++++++++++
 4 files changed, 171 insertions(+)
 create mode 100644 libduck/Makefile
 create mode 100644 libduck/libduck.c
 create mode 100644 libduck/libduck.h
 create mode 100644 libduck/libduck_debug.c

diff --git a/libduck/Makefile b/libduck/Makefile
new file mode 100644
index 0000000..01d8135
--- /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 0000000..54f3d6c
--- /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 0000000..e08bc53
--- /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 0000000..d1da75b
--- /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);
+}
-- 
GitLab