tourel beacon
[aversive.git] / projects / microb2010 / tests / beacon_tsop / main.c
1 /*  
2  *  Copyright Droids Corporation (2009)
3  *  Olivier Matz <zer0@droids-corp.org>
4  * 
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.
9  *
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.
14  *
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
18  *
19  *  Revision : $Id: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $
20  *
21  */
22
23 #include <aversive.h>
24 #include <aversive/wait.h>
25
26 #include <uart.h>
27 #include <pid.h>
28 #include <pwm_ng.h>
29 #include <parse.h>
30 #include <rdline.h>
31
32 #include "cmdline.h"
33 #include "main.h"
34
35 /******************* TSOP */
36
37 #define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */
38 #ifdef BOARD2006
39 #define INTx_TSOP  INT6
40 #define ISCx0_TSOP ISC60
41 #define ISCx1_TSOP ISC61
42 #define SIG_TSOP   SIG_INTERRUPT6
43 #define TSOP_READ() (PINE & 0x40)
44 #else
45 #define INTx_TSOP  INT4
46 #define ISCx0_TSOP ISC40
47 #define ISCx1_TSOP ISC41
48 #define SIG_TSOP   SIG_INTERRUPT4
49 #define TSOP_READ() (PINE & 0x10)
50 #endif
51
52 #define TSOP_FREQ_MHZ 0.455
53 #define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
54 #define N_PERIODS   10.
55
56 #define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US)
57 #define TSOP_TIME_LONG_US  (2.5 * N_PERIODS * TSOP_PERIOD_US)
58
59 #define TSOP_TIME_SHORT ((uint16_t)(TSOP_TIME_SHORT_US*2))
60 #define TSOP_TIME_LONG  ((uint16_t)(TSOP_TIME_LONG_US*2))
61
62 #define FRAME_LEN 16
63
64 /* frame */
65 static uint16_t start_angle_time;
66 static uint16_t frame;
67 static uint16_t mask;
68 static uint8_t len;
69 static uint8_t val;
70
71 struct detected_frame {
72         uint16_t frame;
73         uint16_t time;
74 };
75
76 #define FRAME_RING_ORDER 4
77 #define FRAME_RING_SIZE  (1<<FRAME_RING_ORDER)
78 #define FRAME_RING_MASK  (FRAME_RING_SIZE-1)
79 static uint8_t frame_ring_head = 0;
80 static uint8_t frame_ring_tail = 0;
81 static struct detected_frame frame_ring[FRAME_RING_SIZE];
82
83 /********************** CS */
84
85 /* 8ms, easier if it's a pow of 2 */
86 #define CS_PERIOD_US (8192)
87 #define CS_PERIOD ((uint16_t)(CS_PERIOD_US*2))
88 #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */
89 #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US))   /* too fast = 100 tr/s */
90
91 /* in tr / 1000s */
92 #define CS_CONSIGN (25 * 1000L)
93
94 /* pwm for laser:
95  *  - clear on timer compare (CTC)
96  *  - Toggle OC0 on compare match
97  *  - prescaler = 1 */
98 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
99 #define LASER_OFF() do { TCCR0 = 0; } while (0)
100
101 struct beacon_tsop beacon_tsop;
102
103 void debug_serial(void)
104 {
105 #if 0
106         while (1) {
107                 int16_t c;
108                 c = uart_recv_nowait(0);
109                 if (c != -1) 
110                         printf("%c", (char)(c+1));
111                 LED1_ON();
112                 wait_ms(500);
113                 LED1_OFF();
114                 wait_ms(500);
115         }
116 #endif
117 }
118                   
119 void debug_tsop(void)
120 {
121 #if 0
122         while (1) {
123                 if (TSOP_READ())
124                         LED1_OFF();
125                 else {
126                         LED1_ON();
127                         wait_ms(500);
128                 }
129         }
130 #endif
131 }
132
133 /* decode frame */
134 SIGNAL(SIG_TSOP) {
135         static uint8_t led_cpt = 0;
136
137         /* tsop status */
138         static uint8_t prev_tsop = 0;
139         uint8_t cur_tsop;
140
141         /* time */
142         static uint16_t prev_time;
143         uint16_t ref_time;
144         uint16_t cur_time;
145         uint16_t diff_time;
146
147         ref_time = ICR3;
148         cur_time = TCNT3;
149         cur_tsop = TSOP_READ();
150         diff_time = cur_time - prev_time;
151
152         /* first rising edge */
153         if (cur_tsop && diff_time > TSOP_TIME_LONG) {
154                 len = 1;
155                 val = 1;
156                 frame = 0;
157                 start_angle_time = cur_time - ref_time;
158                 mask = 1;
159         }
160         /* any short edge */
161         else if (diff_time < TSOP_TIME_SHORT) {
162                 if (len & 1) {
163                         if (val)
164                                 frame |= mask;
165                         mask <<= 1;
166                 }
167                 len ++;
168         }
169         /* any long edge */
170         else if (diff_time < TSOP_TIME_LONG) {
171                 val = !val;
172                 if (val)
173                         frame |= mask;
174                 mask <<= 1;
175                 len += 2;
176         }
177
178         /* end of frame */
179         if (len == FRAME_LEN*2) {
180                 uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK;
181                 if (tail_next != frame_ring_head) {
182                         frame_ring[frame_ring_tail].frame = frame;
183                         frame_ring[frame_ring_tail].time = start_angle_time;
184                         frame_ring_tail = tail_next;
185                 }
186                 if (led_cpt & 0x8)
187                         LED3_TOGGLE();
188                 led_cpt ++;
189         }
190
191         prev_time = cur_time;
192         prev_tsop = cur_tsop;
193 }
194
195 /* absolute value */
196 static inline int32_t AbS(int32_t x)
197 {
198         if (x > 0)
199                 return x;
200         else
201                 return -x;
202 }
203
204 /* Get the speed of motor (tr / 1000s)
205  * - icr_cpt is the number of CS period between 2 ICR updates
206  * - icr_diff is the difference of ICR values between the ICR updates
207  *   (modulo 65536 obviously) */
208 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
209 {
210         int32_t best_diff = 65536L;
211         int8_t best_cpt = -2;
212         int32_t diff;
213         int8_t i;
214
215         /* too slow (less than 1 tr/s) */
216         if (icr_cpt > CPT_ICR_MAX)
217                 return 1000L;
218
219         /* too fast (more than 100 tr/s) */
220         if (icr_cpt < CPT_ICR_MIN)
221                 return 100000L;
222
223         /* try to get the real time knowning icr_cpt and icr_diff */
224         for (i=-1; i<2; i++) {
225                 diff = ((icr_cpt+i)&3) * 16384L;
226                 diff += (icr_diff & 0x3fff);
227                 diff -= icr_diff;
228                 if (diff > 32768L)
229                         diff -= 65536L;
230                 if (diff < -32768)
231                         diff += 65536L;
232         
233                 if (AbS(diff) < AbS(best_diff)) {
234                         best_diff = diff;
235                         best_cpt = icr_cpt + i;
236                 }
237         }
238
239         /* real time difference in 1/2 us */
240         diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
241         return 2000000000L/diff;
242 }
243
244 int main(void)
245 {
246         uint16_t prev_cs = 0;
247         uint16_t prev_icr = 0;
248         uint16_t icr = 0;
249         uint16_t diff_icr = 0;
250         uint8_t cpt_icr = 0;
251         uint8_t cpt = 0;
252         int32_t speed, out, err;
253         uint16_t tcnt3;
254         uint8_t x = 0;
255
256         /* LEDS */
257         LED1_DDR |= _BV(LED1_BIT);
258         LED2_DDR |= _BV(LED2_BIT);
259         LED3_DDR |= _BV(LED3_BIT);
260         DDRB |= 0x10; /* OC0 (laser pwm) */
261
262         /* PID init */
263         pid_init(&beacon_tsop.pid);
264         pid_set_gains(&beacon_tsop.pid, 500, 0, 0);
265         pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095);
266         pid_set_out_shift(&beacon_tsop.pid, 10);
267         pid_set_derivate_filter(&beacon_tsop.pid, 4);
268
269         uart_init();
270         fdevopen(uart0_dev_send, uart0_dev_recv);
271
272         rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
273         snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");  
274         rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
275
276         debug_tsop();
277         debug_serial();
278
279         /* configure external interrupt for TSOP */
280         EICRx_TSOP |= _BV(ISCx0_TSOP);
281         EIMSK |= _BV(INTx_TSOP);
282
283         /* pwm for motor */
284         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, 
285                                  TIMER1_PRESCALER_DIV_1);
286         PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
287
288         /* pwm for laser:
289          *  - clear on timer compare (CTC)
290          *  - Toggle OC0 on compare match
291          *  - prescaler = 1 */
292         TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
293         OCR0 = 80; /* f = 100 khz at 16 Mhz */
294
295         /* configure timer 3: CLK/8
296          * it is used as a reference time
297          * enable noise canceller for ICP3 */
298         TCCR3B = _BV(ICNC3) | _BV(CS11);
299
300         sei();
301
302         /* Control system will be done in main loop */
303         while (1) {
304
305                 /* process pending bytes on uart */
306                 cmdline_process();
307
308                 /* monitor the value of ICR (which is modified
309                  * automatically on TT rising edge). If the value
310                  * changed, process the time difference. */
311                 if (ETIFR & _BV(ICF3)) {
312                         cli();
313                         icr = ICR3;
314                         sei();
315                         ETIFR = _BV(ICF3);
316
317                         LED2_TOGGLE();
318                         diff_icr = (icr - prev_icr);
319                         cpt_icr = cpt;
320                         prev_icr = icr;
321                         cpt = 0;
322                         speed = get_speed(cpt_icr, diff_icr);
323                 }
324
325                 /* read time reference */
326                 cli();
327                 tcnt3 = TCNT3;
328                 sei();
329
330                 /* wait cs period */
331                 if (tcnt3 - prev_cs < CS_PERIOD)
332                         continue;
333
334                 /* CS LED */
335                 if (x & 0x80)
336                         LED1_ON();
337                 else
338                         LED1_OFF();
339                 x++;
340
341                 /* process CS... maybe we don't need to use
342                  * control_system_manager, just PID is enough */
343                 if (cpt == CPT_ICR_MAX)
344                         speed = 0;
345                 else
346                         speed = get_speed(cpt_icr, diff_icr);
347                 
348                 /* enabled laser when rotation speed if at least 5tr/s */
349                 if (speed > 5000)
350                         LASER_ON();
351                 else
352                         LASER_OFF();
353
354                 err = CS_CONSIGN - speed;
355                 out = pid_do_filter(&beacon_tsop.pid, err);
356                 if (x == 0 && beacon_tsop.debug_speed)
357                         printf("%ld %ld\n", speed, out);
358                 if (out < 0)
359                         out = 0;
360                 /* XXX */
361                 if (out > 2000)
362                         out = 2000;
363
364                 pwm_ng_set(&beacon_tsop.pwm_motor, out);
365
366                 prev_cs = tcnt3;
367
368                 /* count the number of CS period between 2 ICR
369                  * captures */
370                 if (cpt < CPT_ICR_MAX)
371                         cpt ++;
372
373                 /* after CS, check if we have a new frame in ring */
374                 if (frame_ring_head != frame_ring_tail) {
375                         uint8_t head_next;
376                         head_next = (frame_ring_head+1) & FRAME_RING_MASK;
377                         if (beacon_tsop.debug_frame)
378                                 printf("%x %d\n", frame_ring[frame_ring_head].frame,
379                                        frame_ring[frame_ring_head].time);
380                         frame_ring_head = head_next;
381                 }
382
383         }
384
385         return 0;
386 }