2 * Copyright Droids Corporation (2009)
3 * Olivier Matz <zer0@droids-corp.org>
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 * Revision : $Id: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $
24 #include <aversive/wait.h>
35 /* beacon identifier: must be odd, 3 bits */
36 #define BEACON_ID_MASK 0x7
37 #define BEACON_ID_SHIFT 0
38 #define FRAME_DATA_MASK 0xFF8
39 #define FRAME_DATA_SHIFT 3
41 /******************* TSOP */
43 #define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */
45 #define INTx_TSOP INT6
46 #define ISCx0_TSOP ISC60
47 #define ISCx1_TSOP ISC61
48 #define SIG_TSOP SIG_INTERRUPT6
49 #define TSOP_READ() (!(PINE & 0x40))
51 #define INTx_TSOP INT4
52 #define ISCx0_TSOP ISC40
53 #define ISCx1_TSOP ISC41
54 #define SIG_TSOP SIG_INTERRUPT4
55 #define TSOP_READ() (!(PINE & 0x10))
58 //#define MODUL_455KHZ
61 #if (defined MODUL_455KHZ)
62 #define TSOP_FREQ_MHZ 0.455
65 #define TSOP_FREQ_MHZ 0.038
69 #define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
71 #define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US)
72 #define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US)
74 #define TSOP_TIME_SHORT ((uint16_t)(TSOP_TIME_SHORT_US*2))
75 #define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2))
78 #define FRAME_MASK ((1UL << FRAME_LEN) - 1)
80 struct detected_frame {
88 uint16_t start_angle_time;
95 #define FRAME_RING_ORDER 4
96 #define FRAME_RING_SIZE (1<<FRAME_RING_ORDER)
97 #define FRAME_RING_MASK (FRAME_RING_SIZE-1)
100 struct detected_frame ring[FRAME_RING_SIZE];
103 static struct frame_status static_beacon;
107 /********************** CS */
109 /* 8ms, easier if it's a pow of 2 */
110 #define CS_PERIOD_US (8192)
111 #define CS_PERIOD ((uint16_t)(CS_PERIOD_US*2))
112 #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */
113 #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US)) /* too fast = 100 tr/s */
116 #define CS_CONSIGN (25 * 1000L)
119 * - clear on timer compare (CTC)
120 * - Toggle OC0 on compare match
122 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
123 #define LASER_OFF() do { TCCR0 = 0; } while (0)
125 struct beacon_tsop beacon_tsop;
127 void debug_serial(void)
132 c = uart_recv_nowait(0);
134 printf("%c", (char)(c+1));
143 void debug_tsop(void)
157 /* val is 16 bits, including 4 bits-cksum in MSB, return 0xFFFF is
158 * cksum is wrong, or the 12 bits value on success. */
159 static uint16_t verify_cksum(uint16_t val)
164 /* add the four 4-bits blocks of val together */
168 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
171 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
174 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
177 return 0xffff; /* wrong cksum */
180 static inline void decode_frame(struct frame_status *status,
181 uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
183 uint16_t diff_time = cur_time - status->prev_time;
185 /* first rising edge */
186 if (status->len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) {
190 status->start_angle_time = cur_time - ref_time;
194 else if (status->len != 0 && diff_time < TSOP_TIME_SHORT) {
195 if (status->len & 1) {
197 status->frame |= status->mask;
203 else if (status->len != 0 && diff_time < TSOP_TIME_LONG) {
204 status->val = !status->val;
206 status->frame |= status->mask;
210 /* error case, reset */
216 if (status->len == FRAME_LEN*2) {
217 uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
219 if (tail_next != status->head) {
220 status->ring[status->tail].frame = (status->frame & FRAME_MASK);
221 status->ring[status->tail].time = status->start_angle_time;
222 status->tail = tail_next;
223 if ((status->led_cpt & 0x7) == 0)
230 status->prev_time = cur_time;
231 status->prev_tsop = cur_tsop;
236 static uint8_t running = 0;
245 cur_tsop = TSOP_READ();
247 /* avoid interruption stacking */
258 decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
264 static inline int32_t AbS(int32_t x)
272 /* Get the speed of motor (tr / 1000s)
273 * - icr_cpt is the number of CS period between 2 ICR updates
274 * - icr_diff is the difference of ICR values between the ICR updates
275 * (modulo 65536 obviously) */
276 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
278 int32_t best_diff = 65536L;
279 int8_t best_cpt = -2;
283 /* too slow (less than 1 tr/s) */
284 if (icr_cpt > CPT_ICR_MAX)
287 /* too fast (more than 100 tr/s) */
288 if (icr_cpt < CPT_ICR_MIN)
291 /* try to get the real time knowning icr_cpt and icr_diff */
292 for (i=-1; i<2; i++) {
293 diff = ((icr_cpt+i)&3) * 16384L;
294 diff += (icr_diff & 0x3fff);
301 if (AbS(diff) < AbS(best_diff)) {
303 best_cpt = icr_cpt + i;
307 /* real time difference in 1/2 us */
308 diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
309 return 2000000000L/diff;
312 /* process the received frame ring */
313 void process_ring(struct frame_status *status)
318 /* after CS, check if we have a new frame in ring */
319 while (status->head != status->tail) {
320 head_next = (status->head+1) & FRAME_RING_MASK;
321 frame = status->ring[status->head].frame;
323 /* display if needed */
324 if (beacon_tsop.debug_frame) {
328 /* ignore bad cksum */
329 if (verify_cksum(frame) == 0xFFFF)
332 beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK;
333 data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK;
334 printf("ID=%d data=%d time=%d\r\n",
336 status->ring[status->head].time);
338 status->head = head_next;
344 uint16_t prev_cs = 0;
345 uint16_t prev_icr = 0;
347 uint16_t diff_icr = 0;
350 int32_t speed, out, err;
355 LED1_DDR |= _BV(LED1_BIT);
356 LED2_DDR |= _BV(LED2_BIT);
357 LED3_DDR |= _BV(LED3_BIT);
358 DDRB |= 0x10; /* OC0 (laser pwm) */
361 pid_init(&beacon_tsop.pid);
362 pid_set_gains(&beacon_tsop.pid, 500, 0, 0);
363 pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095);
364 pid_set_out_shift(&beacon_tsop.pid, 10);
365 pid_set_derivate_filter(&beacon_tsop.pid, 4);
368 fdevopen(uart0_dev_send, uart0_dev_recv);
370 rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
371 snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");
372 rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
377 /* configure external interrupt for TSOP */
378 EICRx_TSOP |= _BV(ISCx0_TSOP);
379 EIMSK |= _BV(INTx_TSOP);
382 PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
383 TIMER1_PRESCALER_DIV_1);
384 PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
387 * - clear on timer compare (CTC)
388 * - Toggle OC0 on compare match
390 TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
391 OCR0 = 80; /* f = 100 khz at 16 Mhz */
393 /* configure timer 3: CLK/8
394 * it is used as a reference time
395 * enable noise canceller for ICP3 */
396 TCCR3B = _BV(ICNC3) | _BV(CS11);
400 /* Control system will be done in main loop */
403 /* process pending bytes on uart */
406 /* monitor the value of ICR (which is modified
407 * automatically on TT rising edge). If the value
408 * changed, process the time difference. */
409 if (ETIFR & _BV(ICF3)) {
416 diff_icr = (icr - prev_icr);
420 speed = get_speed(cpt_icr, diff_icr);
423 /* read time reference */
429 if (tcnt3 - prev_cs < CS_PERIOD)
439 /* process CS... maybe we don't need to use
440 * control_system_manager, just PID is enough */
441 if (cpt == CPT_ICR_MAX)
444 speed = get_speed(cpt_icr, diff_icr);
446 /* enabled laser when rotation speed if at least 5tr/s */
452 err = CS_CONSIGN - speed;
453 out = pid_do_filter(&beacon_tsop.pid, err);
454 if (x == 0 && beacon_tsop.debug_speed)
455 printf("%ld %ld\n", speed, out);
462 pwm_ng_set(&beacon_tsop.pwm_motor, out);
466 /* count the number of CS period between 2 ICR
468 if (cpt < CPT_ICR_MAX)
471 process_ring(&static_beacon);