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

root / toolbox / main.c @ master

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

Project

General

Profile

Tooltron

root / toolbox / main.c @ master