/mainbox/tool.c - Annotate - Tooltron - Robotics Club

root / mainbox / tool.c @ 15fa09f7

History | View | Annotate | Download (3.78 KB)

1	7bdb98c5	Tom Mullins	#include "tool.h"
2	15fa09f7	Tom Mullins	#include "query.h"
3	7bdb98c5	Tom Mullins	#include "tooltron_mb.h"
4			#include <modbus.h>
5			#include <stdio.h>
6			#include <errno.h>
7			#include <stdlib.h> // TODO temporary
8
9			modbus_t *ctx;
10
11			int tool_init_mb(const char *device) {
12	14688426	Tom Mullins	struct timeval timeout;
13
14	7bdb98c5	Tom Mullins	ctx = modbus_new_rtu(device, MB_BAUD, 'N', 8, 1);
15			if (ctx == NULL) {
16			fprintf(stderr, "modbus_new_rtu: %s\n", modbus_strerror(errno));
17			return 1;
18			}
19
20			if (modbus_connect(ctx) == -1) {
21			fprintf(stderr, "modbus_connect: %s\n", modbus_strerror(errno));
22			modbus_free(ctx);
23			return 1;
24			}
25
26	14688426	Tom Mullins	modbus_get_response_timeout(ctx, &timeout);
27			printf("Response timeout is %lus %luus\n", timeout.tv_sec, timeout.tv_usec);
28
29	7bdb98c5	Tom Mullins	return 0;
30			}
31
32			void tool_close_mb() {
33			modbus_close(ctx);
34			modbus_free(ctx);
35			}
36
37			static void tool_write_coil(int addr, int bit) {
38			if (modbus_write_bit(ctx, addr, bit) == -1) {
39			fprintf(stderr, "modbus_write_bit: %s\n", modbus_strerror(errno));
40			}
41			}
42
43			static void tool_init(struct tool_t *tool) {
44			// TODO write current limit values
45			tool_write_coil(MB_COIL_INIT, 1);
46			tool->state = TS_OFF;
47			}
48
49	3811693d	Tom Mullins	static void tool_read_user(struct tool_t *tool) {
50	7bdb98c5	Tom Mullins	unsigned short serno[2];
51	14688426	Tom Mullins	if (modbus_read_input_registers(ctx, MB_INP_SERNOL, 2, serno) == -1) {
52	7bdb98c5	Tom Mullins	fprintf(stderr, "modbus_read_registers: %s\n", modbus_strerror(errno));
53			tool->user = 0;
54			} else {
55			tool->user = MODBUS_GET_INT32_FROM_INT16(serno, 0);
56			}
57			}
58
59			static void tool_grant_access(struct tool_t *tool) {
60	15fa09f7	Tom Mullins	printf("Granting access to %08x on %s (%d)\n", tool->user, tool->name,
61			tool->address);
62	7bdb98c5	Tom Mullins	tool_write_coil(MB_COIL_EN, 1);
63	dc472500	Tom Mullins	tool->state = TS_ON;
64	7bdb98c5	Tom Mullins	}
65
66			static void tool_deny_access(struct tool_t *tool) {
67	15fa09f7	Tom Mullins	printf("Denying access to %08x on %s (%d)\n", tool->user, tool->name,
68			tool->address);
69	7bdb98c5	Tom Mullins	tool_write_coil(MB_COIL_EN, 0);
70	dc472500	Tom Mullins	tool->state = TS_OFF;
71	7bdb98c5	Tom Mullins	}
72
73			void tool_request_disable(struct tool_t *tool) {
74	15fa09f7	Tom Mullins	printf("Requesting disable on %s (%d)\n", tool->name, tool->address);
75	7bdb98c5	Tom Mullins	tool_write_coil(MB_COIL_REQ_DIS, 1);
76	dc472500	Tom Mullins	tool->state = TS_REQ_DIS;
77	7bdb98c5	Tom Mullins	}
78
79			void tool_poll(struct tool_t *tool) {
80			unsigned char status[N_COILS];
81
82	14688426	Tom Mullins	if (modbus_set_slave(ctx, tool->address) == -1) {
83	7bdb98c5	Tom Mullins	fprintf(stderr, "modbus_set_slave: %s\n", modbus_strerror(errno));
84			}
85
86	15fa09f7	Tom Mullins	/* If we can't read from the tool, we only want this error message to print
87			* once, thus tool->connected */
88	7bdb98c5	Tom Mullins	if (modbus_read_bits(ctx, 0, N_COILS, status) == -1) {
89	15fa09f7	Tom Mullins	if (tool->connected) {
90			fprintf(stderr, "Cannot connect to %s (%d): %s\n", tool->name,
91			tool->address, modbus_strerror(errno));
92			tool->connected = 0;
93			}
94	7bdb98c5	Tom Mullins	return;
95	15fa09f7	Tom Mullins	} else if (!tool->connected) {
96			fprintf(stderr, "Reconnected to %s (%d)\n", tool->name, tool->address);
97			tool->connected = 1;
98	7bdb98c5	Tom Mullins	}
99
100	15fa09f7	Tom Mullins	/*uint16_t current;
101			if (modbus_read_input_registers(ctx, MB_INP_CURRENT, 1, &current) == -1) {
102			fprintf(stderr, "modbus_read_registers: %s\n", modbus_strerror(errno));
103			} else {
104			printf("Current: %d\n", current);
105			}*/
106
107	1b054655	Tom Mullins	/*printf("new:%d en:%d req_dis:%d init:%d\n", status[MB_COIL_NEW],
108			status[MB_COIL_EN], status[MB_COIL_REQ_DIS], status[MB_COIL_INIT]);*/
109	dc472500	Tom Mullins
110	7bdb98c5	Tom Mullins	if (!status[MB_COIL_INIT]) {
111			tool->state = TS_INIT;
112			}
113
114			switch (tool->state) {
115
116			case TS_INIT:
117			tool_init(tool);
118			break;
119
120			case TS_OFF:
121			if (status[MB_COIL_NEW]) {
122	3811693d	Tom Mullins	tool_read_user(tool);
123	15fa09f7	Tom Mullins	if (query_user_permission(tool->address, tool->user)) {
124	7bdb98c5	Tom Mullins	tool_grant_access(tool);
125	1b054655	Tom Mullins	} else {
126	7bdb98c5	Tom Mullins	tool_deny_access(tool);
127	1b054655	Tom Mullins	}
128	7bdb98c5	Tom Mullins	}
129			break;
130
131			case TS_ON:
132			if (!status[MB_COIL_EN]) {
133	15fa09f7	Tom Mullins	printf("Tool %s (%d) is off\n", tool->name, tool->address);
134	7bdb98c5	Tom Mullins	tool->state = TS_OFF;
135			}
136			break;
137
138			case TS_REQ_DIS:
139			if (!status[MB_COIL_EN]) {
140	15fa09f7	Tom Mullins	printf("Tool %s (%d) is off after requested disable\n", tool->name,
141			tool->address);
142	7bdb98c5	Tom Mullins	tool->state = TS_OFF;
143			}
144			break;
145
146			}
147
148			}

Project

General

Profile

Tooltron

root / mainbox / tool.c @ 15fa09f7