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  0xFF8
  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 struct detected_frame {
  81         uint16_t frame;
  82         uint16_t time;
  83 };
  84
  85 /* frame */
  86 struct frame_status {
  87         uint8_t led_cpt;
  88         uint16_t start_angle_time;
  89         uint16_t frame;
  90         uint16_t mask;
  91         uint16_t prev_time;
  92         uint8_t prev_tsop;
  93         uint8_t len;
  94         uint8_t val;
  95 #define FRAME_RING_ORDER 4
  96 #define FRAME_RING_SIZE  (1<<FRAME_RING_ORDER)
  97 #define FRAME_RING_MASK  (FRAME_RING_SIZE-1)
  98         uint8_t head;
  99         uint8_t tail;
 100         struct detected_frame ring[FRAME_RING_SIZE];
 101 };
 102
 103 static struct frame_status static_beacon;
 104
 105
 106
 107 /********************** CS */
 108
 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 */
 114
 115 /* in tr / 1000s */
 116 #define CS_CONSIGN (25 * 1000L)
 117
 118 /* pwm for laser:
 119  *  - clear on timer compare (CTC)
 120  *  - Toggle OC0 on compare match
 121  *  - prescaler = 1 */
 122 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
 123 #define LASER_OFF() do { TCCR0 = 0; } while (0)
 124
 125 struct beacon_tsop beacon_tsop;
 126
 127 void debug_serial(void)
 128 {
 129 #if 0
 130         while (1) {
 131                 int16_t c;
 132                 c = uart_recv_nowait(0);
 133                 if (c != -1)
 134                         printf("%c", (char)(c+1));
 135                 LED1_ON();
 136                 wait_ms(500);
 137                 LED1_OFF();
 138                 wait_ms(500);
 139         }
 140 #endif
 141 }
 142
 143 void debug_tsop(void)
 144 {
 145 #if 0
 146         while (1) {
 147                 if (TSOP_READ())
 148                         LED1_OFF();
 149                 else {
 150                         LED1_ON();
 151                         wait_ms(500);
 152                 }
 153         }
 154 #endif
 155 }
 156
 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)
 160 {
 161         uint16_t x, cksum;
 162
 163         x = (val & 0xfff);
 164         /* add the four 4-bits blocks of val together */
 165         cksum = val & 0xf;
 166         val = val >> 4;
 167         cksum += val & 0xf;
 168         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 169         val = val >> 4;
 170         cksum += val & 0xf;
 171         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 172         val = val >> 4;
 173         cksum += val & 0xf;
 174         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 175         if (cksum == 0xf)
 176                 return x;
 177         return 0xffff; /* wrong cksum */
 178 }
 179
 180 static inline void decode_frame(struct frame_status *status,
 181                                 uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
 182 {
 183         uint16_t diff_time = cur_time - status->prev_time;
 184
 185         /* first rising edge */
 186         if (status->len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) {
 187                 status->len = 1;
 188                 status->val = 1;
 189                 status->frame = 0;
 190                 status->start_angle_time = cur_time - ref_time;
 191                 status->mask = 1;
 192         }
 193         /* any short edge */
 194         else if (status->len != 0 && diff_time < TSOP_TIME_SHORT) {
 195                 if (status->len & 1) {
 196                         if (status->val)
 197                                 status->frame |= status->mask;
 198                         status->mask <<= 1;
 199                 }
 200                 status->len ++;
 201         }
 202         /* any long edge */
 203         else if (status->len != 0 && diff_time < TSOP_TIME_LONG) {
 204                 status->val = !status->val;
 205                 if (status->val)
 206                         status->frame |= status->mask;
 207                 status->mask <<= 1;
 208                 status->len += 2;
 209         }
 210         /* error case, reset */
 211         else {
 212                 status->len = 0;
 213         }
 214
 215         /* end of frame */
 216         if (status->len == FRAME_LEN*2) {
 217                 uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
 218
 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)
 224                                 LED3_TOGGLE();
 225                         status->led_cpt ++;
 226                 }
 227                 status->len = 0;
 228         }
 229
 230         status->prev_time = cur_time;
 231         status->prev_tsop = cur_tsop;
 232 }
 233
 234 /* decode frame */
 235 SIGNAL(SIG_TSOP) {
 236         static uint8_t running = 0;
 237
 238         /* tsop status */
 239         uint8_t cur_tsop;
 240         uint16_t ref_time;
 241         uint16_t cur_time;
 242
 243         ref_time = ICR3;
 244         cur_time = TCNT3;
 245         cur_tsop = TSOP_READ();
 246
 247         /* avoid interruption stacking */
 248         if (running)
 249                 return;
 250         running = 1;
 251         sei();
 252
 253         if (cur_tsop)
 254                 LED2_ON();
 255         else
 256                 LED2_OFF();
 257
 258         decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
 259
 260         running = 0;
 261 }
 262
 263 /* absolute value */
 264 static inline int32_t AbS(int32_t x)
 265 {
 266         if (x > 0)
 267                 return x;
 268         else
 269                 return -x;
 270 }
 271
 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)
 277 {
 278         int32_t best_diff = 65536L;
 279         int8_t best_cpt = -2;
 280         int32_t diff;
 281         int8_t i;
 282
 283         /* too slow (less than 1 tr/s) */
 284         if (icr_cpt > CPT_ICR_MAX)
 285                 return 1000L;
 286
 287         /* too fast (more than 100 tr/s) */
 288         if (icr_cpt < CPT_ICR_MIN)
 289                 return 100000L;
 290
 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);
 295                 diff -= icr_diff;
 296                 if (diff > 32768L)
 297                         diff -= 65536L;
 298                 if (diff < -32768)
 299                         diff += 65536L;
 300
 301                 if (AbS(diff) < AbS(best_diff)) {
 302                         best_diff = diff;
 303                         best_cpt = icr_cpt + i;
 304                 }
 305         }
 306
 307         /* real time difference in 1/2 us */
 308         diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
 309         return 2000000000L/diff;
 310 }
 311
 312 /* process the received frame ring */
 313 void process_ring(struct frame_status *status)
 314 {
 315         uint8_t head_next;
 316         uint32_t frame;
 317
 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;
 322
 323                 /* display if needed */
 324                 if (beacon_tsop.debug_frame) {
 325                         uint8_t beacon_id;
 326                         uint16_t data;
 327
 328                         /* ignore bad cksum */
 329                         if (verify_cksum(frame) == 0xFFFF)
 330                                 continue;
 331
 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",
 335                                beacon_id, data,
 336                                status->ring[status->head].time);
 337                 }
 338                 status->head = head_next;
 339         }
 340 }
 341
 342 int main(void)
 343 {
 344         uint16_t prev_cs = 0;
 345         uint16_t prev_icr = 0;
 346         uint16_t icr = 0;
 347         uint16_t diff_icr = 0;
 348         uint8_t cpt_icr = 0;
 349         uint8_t cpt = 0;
 350         int32_t speed, out, err;
 351         uint16_t tcnt3;
 352         uint8_t x = 0;
 353
 354         /* LEDS */
 355         LED1_DDR |= _BV(LED1_BIT);
 356         LED2_DDR |= _BV(LED2_BIT);
 357         LED3_DDR |= _BV(LED3_BIT);
 358         DDRB |= 0x10; /* OC0 (laser pwm) */
 359
 360         /* PID init */
 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);
 366
 367         uart_init();
 368         fdevopen(uart0_dev_send, uart0_dev_recv);
 369
 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);
 373
 374         debug_tsop();
 375         debug_serial();
 376
 377         /* configure external interrupt for TSOP */
 378         EICRx_TSOP |= _BV(ISCx0_TSOP);
 379         EIMSK |= _BV(INTx_TSOP);
 380
 381         /* pwm for motor */
 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);
 385
 386         /* pwm for laser:
 387          *  - clear on timer compare (CTC)
 388          *  - Toggle OC0 on compare match
 389          *  - prescaler = 1 */
 390         TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
 391         OCR0 = 80; /* f = 100 khz at 16 Mhz */
 392
 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);
 397
 398         sei();
 399
 400         /* Control system will be done in main loop */
 401         while (1) {
 402
 403                 /* process pending bytes on uart */
 404                 cmdline_process();
 405
 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)) {
 410                         cli();
 411                         icr = ICR3;
 412                         sei();
 413                         ETIFR = _BV(ICF3);
 414
 415                         //LED2_TOGGLE();
 416                         diff_icr = (icr - prev_icr);
 417                         cpt_icr = cpt;
 418                         prev_icr = icr;
 419                         cpt = 0;
 420                         speed = get_speed(cpt_icr, diff_icr);
 421                 }
 422
 423                 /* read time reference */
 424                 cli();
 425                 tcnt3 = TCNT3;
 426                 sei();
 427
 428                 /* wait cs period */
 429                 if (tcnt3 - prev_cs < CS_PERIOD)
 430                         continue;
 431
 432                 /* CS LED */
 433                 if (x & 0x80)
 434                         LED1_ON();
 435                 else
 436                         LED1_OFF();
 437                 x++;
 438
 439                 /* process CS... maybe we don't need to use
 440                  * control_system_manager, just PID is enough */
 441                 if (cpt == CPT_ICR_MAX)
 442                         speed = 0;
 443                 else
 444                         speed = get_speed(cpt_icr, diff_icr);
 445
 446                 /* enabled laser when rotation speed if at least 5tr/s */
 447                 if (speed > 5000)
 448                         LASER_ON();
 449                 else
 450                         LASER_OFF();
 451
 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);
 456                 if (out < 0)
 457                         out = 0;
 458                 /* XXX */
 459                 if (out > 2000)
 460                         out = 2000;
 461
 462                 pwm_ng_set(&beacon_tsop.pwm_motor, out);
 463
 464                 prev_cs = tcnt3;
 465
 466                 /* count the number of CS period between 2 ICR
 467                  * captures */
 468                 if (cpt < CPT_ICR_MAX)
 469                         cpt ++;
 470
 471                 process_ring(&static_beacon);
 472         }
 473
 474         return 0;
 475 }