libduck.c 2.19 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#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;

jp7g21's avatar
jp7g21 committed
16
17
int duck_debug_mode = 0;

18
19
void open_duck(const char *fname)
{
jp7g21's avatar
jp7g21 committed
20
21
22
23
24
25
26
27
     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);
	  }
28
29
30
31
32
     }
}

void configure_duck(void)
{
jp7g21's avatar
jp7g21 committed
33
34
35
36
37
38
39
40
     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);
jp7g21's avatar
jp7g21 committed
41
	  tio.c_oflag &= ~ONLCR;
jp7g21's avatar
jp7g21 committed
42
43
44
45
	  
	  if (tcsetattr(duckfd, TCSANOW, &tio)) {
	       err(EXIT_FAILURE, "tcsetattr failed");
	  }
46
47
48
49
50
51
52
     }
}

static int duck_printf(const char *fmt, ...)
{
     va_list ap;
     va_start(ap, fmt);
jp7g21's avatar
jp7g21 committed
53
54
55
56
57
     if (duck_debug_mode) {
	  return vprintf(fmt, ap);
     } else {
	  return vdprintf(duckfd, fmt, ap) < 0;
     }
58
59
60
61
62
63
64
}

static int validate_motor(int motor)
{
     return motor >= 1 && motor <= 3;
}

jp7g21's avatar
jp7g21 committed
65
66
67
68
69
70
71
static int wrap_angle(int angle)
{
     while (angle > 180) angle -= 360;
     while (angle < -180) angle += 360;
     return angle;
}

72
73
74
int duck_set_position(int motor, int angle)
{
     if (!validate_motor(motor)) return -1;
jp7g21's avatar
jp7g21 committed
75
     angle = wrap_angle(angle);
76
77
78
79
80
81
82
83
     return duck_printf("s %d %d\n", motor, angle);
}

int duck_delay(int ms)
{
     return duck_printf("d %d\n", ms);
}

jp7g21's avatar
stuff    
jp7g21 committed
84
85
/* Final position will be (deg_per_sec * ms / 1000), rounded down to nearest degree */
int duck_set_velocity(int motor, int deg_per_sec, int ms)
86
87
88
{
     if (!validate_motor(motor)) return -1;
     if (abs(deg_per_sec) > 1000) return -1;
jp7g21's avatar
stuff    
jp7g21 committed
89
     return duck_printf("v %d %d %d\n", motor, deg_per_sec, ms);
90
91
}

jp7g21's avatar
jp7g21 committed
92
93
94
95
96
int duck_write_text(const char *str)
{
     return duck_printf("t %s\n", str);
}

jp7g21's avatar
jp7g21 committed
97
98
99
100
101
102
103
104
105
void read_duck_to_stdout(void)
{
     while (1) {
	  char c;
	  if (read(duckfd, &c, 1) != 1) err(EXIT_FAILURE, "Read failed\n");
	  putchar(c);
     }
}

106
107
void close_duck(void)
{
jp7g21's avatar
jp7g21 committed
108
109
110
111
112
     if (duck_debug_mode) {
	  printf("Closing duck\n");
     } else {
	  close(duckfd);
     }
113
}