From 7a43fb3b6d7ab503304497ce28119365553cf8e4 Mon Sep 17 00:00:00 2001
From: jp7g21 <jp7g21@soton.ac.uk>
Date: Sat, 13 Nov 2021 21:33:05 +0000
Subject: [PATCH] Added debug mode to libduck
---
libduck/libduck.c | 55 ++++++++++++++++++++++---------
libduck/libduck.h | 2 ++
libduck/libduck_debug.c | 72 -----------------------------------------
3 files changed, 42 insertions(+), 87 deletions(-)
delete mode 100644 libduck/libduck_debug.c
diff --git a/libduck/libduck.c b/libduck/libduck.c
index 54f3d6c..7556dfe 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 081bdeb..b938702 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 d1da75b..0000000
--- 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);
-}
--
GitLab