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

root / toolbox / main.c @ bbf2f5ad

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

Project

General

Profile

Tooltron

root / toolbox / main.c @ bbf2f5ad