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