Commit 7a43fb3b authored by jp7g21's avatar jp7g21
Browse files

Added debug mode to libduck

parent a8feb213
......@@ -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);
}
}
......@@ -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);
......
#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);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment