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

root / toolbox / main.c @ 1085ef77

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

Project

General

Profile

Tooltron

root / toolbox / main.c @ 1085ef77