/toolbox/main.c - Annotate - Tooltron - Robotics Club

root / toolbox / main.c @ 4b7d087b

History | View | Annotate | Download (5.88 KB)

1	20e5429c	Tom Mullins	#include <stdint.h>
2	12ea39cc	Tom Mullins	#include <string.h>
3			#include <avr/io.h>
4	20e5429c	Tom Mullins	#include <util/delay.h>
5			#include "mb.h"
6			#include "mbport.h"
7			#include "tooltron_mb.h"
8	9b2b6d91	Tom Mullins	#include "rfid.h"
9	532ba0bd	Tom Mullins	#include "led.h"
10	20e5429c	Tom Mullins
11	12ea39cc	Tom Mullins	enum toolstate_t {
12			TS_INIT,
13			TS_OFF,
14			TS_WAIT_ACCESS,
15	f5d1f56a	Tom Mullins	TS_DENY,
16	12ea39cc	Tom Mullins	TS_REQ_DIS,
17			TS_MISSING_ID,
18			TS_ON
19			};
20
21			static enum toolstate_t toolstate = TS_INIT;
22	6ca98a3f	Tom Mullins	static uint8_t coils;
23	1b054655	Tom Mullins	static uint8_t latest_reading[RFID_SERNO_SIZE];
24	6ca98a3f	Tom Mullins	static uint8_t current_user[RFID_SERNO_SIZE];
25	208a6fb2	Tom Mullins
26			static inline void set_coil(char coil, char bit) {
27	6ca98a3f	Tom Mullins	coils = (coils & ~(1 << coil)) \| (bit << coil);
28	208a6fb2	Tom Mullins	}
29	12ea39cc	Tom Mullins	static inline char get_coil(char coil) {
30			return (coils >> coil) & 1;
31			}
32
33			static inline void tool_init() {DDRA \|= _BV(DDA1);}
34			static inline void tool_enable() {PORTA \|= _BV(PA1);}
35	76915f20	Tom Mullins	static inline void tool_disable() {PORTA &= ~ _BV(PA1);}
36	12ea39cc	Tom Mullins
37	1085ef77	Tom Mullins	static inline void serno_zero(uint8_t *serno) {
38	1b054655	Tom Mullins	memset(serno, 0, RFID_SERNO_SIZE);
39			}
40
41			static char serno_is_nonzero(uint8_t *serno) {
42			int i;
43			for (i = 0; i < RFID_SERNO_SIZE; i++) {
44			if (serno[i]) {
45			return 1;
46			}
47			}
48			return 0;
49			}
50
51			static char serno_equal(uint8_t a, uint8_t b) {
52			return memcmp(a, b, RFID_SERNO_SIZE) == 0;
53			}
54
55			static void serno_cpy(uint8_t dest, uint8_t src) {
56			memcpy(dest, src, RFID_SERNO_SIZE);
57			}
58
59	12ea39cc	Tom Mullins	static void tool_main() {
60
61			switch (toolstate) {
62
63			case TS_INIT:
64			if (get_coil(MB_COIL_INIT)) {
65			set_coil(MB_COIL_NEW, 0);
66			set_coil(MB_COIL_EN, 0);
67			set_coil(MB_COIL_REQ_DIS, 0);
68			toolstate = TS_OFF;
69			}
70			break;
71
72			case TS_OFF:
73	f5d1f56a	Tom Mullins	led_off();
74	4b7d087b	Tom Mullins	set_coil(MB_COIL_EN, 0);
75	1b054655	Tom Mullins	if (serno_is_nonzero(latest_reading)) {
76			serno_cpy(current_user, latest_reading);
77	12ea39cc	Tom Mullins	set_coil(MB_COIL_NEW, 1);
78			toolstate = TS_WAIT_ACCESS;
79			}
80			break;
81
82			case TS_WAIT_ACCESS:
83	1b054655	Tom Mullins	led_yellow();
84	12ea39cc	Tom Mullins	if (get_coil(MB_COIL_EN)) {
85			tool_enable();
86			toolstate = TS_ON;
87	dc472500	Tom Mullins	} else if (!get_coil(MB_COIL_NEW)) {
88	f5d1f56a	Tom Mullins	toolstate = TS_DENY;
89	4b7d087b	Tom Mullins	} else if (!serno_equal(current_user, latest_reading)) {
90			set_coil(MB_COIL_NEW, 0);
91			toolstate = TS_OFF;
92	12ea39cc	Tom Mullins	}
93			break;
94
95	f5d1f56a	Tom Mullins	case TS_DENY:
96			led_red();
97			if (!serno_equal(current_user, latest_reading)) {
98			toolstate = TS_OFF;
99			serno_zero(current_user);
100			}
101
102	12ea39cc	Tom Mullins	case TS_REQ_DIS:
103	e53aa5c5	Tom Mullins	if (!get_coil(MB_COIL_EN)) {
104			tool_disable();
105			toolstate = TS_OFF;
106			} else if (!get_coil(MB_COIL_REQ_DIS)) {
107			toolstate = TS_ON;
108			} else {
109			// TODO blink yellow for 10 seconds or something
110			set_coil(MB_COIL_EN, 0);
111			set_coil(MB_COIL_REQ_DIS, 0);
112			tool_disable();
113	1b054655	Tom Mullins	serno_zero(current_user);
114	e53aa5c5	Tom Mullins	toolstate = TS_OFF;
115			}
116	12ea39cc	Tom Mullins	break;
117
118			case TS_MISSING_ID:
119	e53aa5c5	Tom Mullins	if (!get_coil(MB_COIL_EN)) {
120			tool_disable();
121			toolstate = TS_OFF;
122			} else if (get_coil(MB_COIL_REQ_DIS)) {
123			toolstate = TS_REQ_DIS;
124	1b054655	Tom Mullins	} else if (serno_equal(current_user, latest_reading)) {
125	12ea39cc	Tom Mullins	toolstate = TS_ON;
126			} else {
127	dc472500	Tom Mullins	if (led_blink_done()) {
128			set_coil(MB_COIL_EN, 0);
129			tool_disable();
130	1b054655	Tom Mullins	serno_zero(current_user);
131	dc472500	Tom Mullins	toolstate = TS_OFF;
132			}
133	12ea39cc	Tom Mullins	}
134			break;
135
136			case TS_ON:
137	1b054655	Tom Mullins	led_green();
138	12ea39cc	Tom Mullins	if (!get_coil(MB_COIL_EN)) {
139			tool_disable();
140	1b054655	Tom Mullins	serno_zero(current_user);
141	12ea39cc	Tom Mullins	toolstate = TS_OFF;
142	e53aa5c5	Tom Mullins	} else if(get_coil(MB_COIL_REQ_DIS)) {
143	12ea39cc	Tom Mullins	toolstate = TS_REQ_DIS;
144	1b054655	Tom Mullins	} else if (!serno_equal(current_user, latest_reading)) {
145	12ea39cc	Tom Mullins	toolstate = TS_MISSING_ID;
146	1085ef77	Tom Mullins	led_blink_start(666, 15, YELLOW);
147	12ea39cc	Tom Mullins	}
148			break;
149
150			}
151
152			}
153	208a6fb2	Tom Mullins
154	04b8cd51	Tom Mullins	eMBErrorCode eMBRegCoilsCB(UCHAR *reg_buf, USHORT addr, USHORT n_coils,
155	20e5429c	Tom Mullins	eMBRegisterMode mode) {
156	208a6fb2	Tom Mullins
157	dc472500	Tom Mullins	addr--;
158
159	532ba0bd	Tom Mullins	if (addr+n_coils > N_COILS) {
160	20e5429c	Tom Mullins	return MB_ENOREG;
161	208a6fb2	Tom Mullins	}
162
163			if (mode == MB_REG_WRITE) {
164
165			switch (addr) {
166
167			case MB_COIL_NEW:
168			/* nop */
169			reg_buf[0] >>= 1;
170			n_coils--;
171			if (n_coils == 0) {
172			return MB_ENOERR;
173			}
174
175			case MB_COIL_EN:
176			set_coil(MB_COIL_NEW, 0);
177			set_coil(MB_COIL_EN, reg_buf[0] & 1);
178			reg_buf[0] >>= 1;
179			n_coils--;
180			if (n_coils == 0) {
181			return MB_ENOERR;
182			}
183
184			case MB_COIL_REQ_DIS:
185	12ea39cc	Tom Mullins	set_coil(MB_COIL_REQ_DIS, reg_buf[0] & 1);
186	208a6fb2	Tom Mullins	reg_buf[0] >>= 1;
187			n_coils--;
188			if (n_coils == 0) {
189			return MB_ENOERR;
190			}
191
192			case MB_COIL_INIT:
193			set_coil(MB_COIL_INIT, reg_buf[0] & 1);
194			reg_buf[0] >>= 1;
195			n_coils--;
196			if (n_coils == 0) {
197			return MB_ENOERR;
198			}
199			}
200
201	20e5429c	Tom Mullins	} else if (mode == MB_REG_READ) {
202	208a6fb2	Tom Mullins
203	dc472500	Tom Mullins	reg_buf[0] = (coils >> addr) & ((1 << n_coils) - 1);
204	208a6fb2	Tom Mullins	return MB_ENOERR;
205
206	20e5429c	Tom Mullins	}
207	208a6fb2	Tom Mullins
208			return MB_EIO;
209	20e5429c	Tom Mullins	}
210
211	04b8cd51	Tom Mullins	eMBErrorCode eMBRegDiscreteCB(UCHAR *reg_buf, USHORT addr, USHORT n_coils) {
212	20e5429c	Tom Mullins	return MB_ENOREG;
213			}
214
215	04b8cd51	Tom Mullins	eMBErrorCode eMBRegInputCB(UCHAR *reg_buf, USHORT addr, USHORT n_regs) {
216
217	dc472500	Tom Mullins	addr--;
218
219	20e5429c	Tom Mullins	switch (addr) {
220	04b8cd51	Tom Mullins
221			case MB_INP_SERNOL:
222	76915f20	Tom Mullins	*reg_buf++ = current_user[0];
223			*reg_buf++ = current_user[1];
224	04b8cd51	Tom Mullins	n_regs--;
225			if (n_regs == 0) {
226			return MB_ENOERR;
227			}
228
229			case MB_INP_SERNOH:
230	76915f20	Tom Mullins	*reg_buf++ = current_user[2];
231			*reg_buf++ = current_user[3];
232	04b8cd51	Tom Mullins	n_regs--;
233			if (n_regs == 0) {
234			return MB_ENOERR;
235			}
236
237			case MB_INP_CURRENT:
238			*reg_buf++ = 0;
239			*reg_buf++ = 0;
240			n_regs--;
241			if (n_regs == 0) {
242			return MB_ENOERR;
243	20e5429c	Tom Mullins	}
244	04b8cd51	Tom Mullins
245	20e5429c	Tom Mullins	default:
246			return MB_ENOREG;
247			}
248			}
249
250	04b8cd51	Tom Mullins	eMBErrorCode eMBRegHoldingCB(UCHAR *reg_buf, USHORT addr, USHORT n_regs,
251	20e5429c	Tom Mullins	eMBRegisterMode mode) {
252			if (mode == MB_REG_WRITE) {
253			return MB_ENOREG;
254			} else if (mode == MB_REG_READ) {
255			return MB_ENOREG;
256			} else {
257			return MB_EIO;
258			}
259			}
260
261			int main() {
262	12ea39cc	Tom Mullins
263	532ba0bd	Tom Mullins	led_init();
264	12ea39cc	Tom Mullins	tool_init();
265	9b2b6d91	Tom Mullins	rfid_init();
266	20e5429c	Tom Mullins
267			eMBInit(MB_RTU, SLAVE_ADDR, 0, MB_BAUD, MB_PAR_NONE);
268			eMBEnable();
269
270	208a6fb2	Tom Mullins	sei();
271
272	532ba0bd	Tom Mullins	rfid_start_read();
273	20e5429c	Tom Mullins	while (1) {
274	532ba0bd	Tom Mullins	if (rfid_poll()) {
275	1b054655	Tom Mullins	rfid_get_serno(latest_reading);
276	532ba0bd	Tom Mullins	rfid_start_read();
277			}
278	dc472500	Tom Mullins	tool_main();
279	20e5429c	Tom Mullins	eMBPoll();
280	532ba0bd	Tom Mullins	_delay_ms(50);
281	20e5429c	Tom Mullins	}
282
283			return 0;
284			}

Project

General

Profile

Tooltron

root / toolbox / main.c @ 4b7d087b