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 #if 0
 150 /* val is 16 bits, including 4 bits-cksum in MSB, return 0xFFFF is
 151  * cksum is wrong, or the 12 bits value on success. */
 152 static uint16_t verify_cksum(uint16_t val)
 153 {
 154         uint16_t x, cksum;
 155
 156         x = (val & 0xfff);
 157         /* add the four 4-bits blocks of val together */
 158         cksum = val & 0xf;
 159         val = val >> 4;
 160         cksum += val & 0xf;
 161         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 162         val = val >> 4;
 163         cksum += val & 0xf;
 164         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 165         val = val >> 4;
 166         cksum += val & 0xf;
 167         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 168         if (cksum == 0xf)
 169                 return x;
 170         return 0xffff; /* wrong cksum */
 171 }
 172 #endif
 173
 174 /* decode frame */
 175 SIGNAL(SIG_TSOP) {
 176         static uint8_t led_cpt = 0;
 177
 178         /* tsop status */
 179         static uint8_t prev_tsop = 0;
 180         uint8_t cur_tsop;
 181
 182         /* time */
 183         static uint16_t prev_time;
 184         uint16_t ref_time;
 185         uint16_t cur_time;
 186         uint16_t diff_time;
 187
 188         ref_time = ICR3;
 189         cur_time = TCNT3;
 190         cur_tsop = TSOP_READ();
 191         diff_time = cur_time - prev_time;
 192
 193         if (cur_tsop)
 194                 LED2_ON();
 195         else
 196                 LED2_OFF();
 197
 198         /* first rising edge */
 199         if (len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) {
 200                 len = 1;
 201                 val = 1;
 202                 frame = 0;
 203                 start_angle_time = cur_time - ref_time;
 204                 mask = 1;
 205         }
 206         /* any short edge */
 207         else if (len != 0 && diff_time < TSOP_TIME_SHORT) {
 208                 if (len & 1) {
 209                         if (val)
 210                                 frame |= mask;
 211                         mask <<= 1;
 212                 }
 213                 len ++;
 214         }
 215         /* any long edge */
 216         else if (len != 0 && diff_time < TSOP_TIME_LONG) {
 217                 val = !val;
 218                 if (val)
 219                         frame |= mask;
 220                 mask <<= 1;
 221                 len += 2;
 222         }
 223         /* error case, reset */
 224         else {
 225                 len = 0;
 226         }
 227
 228         /* end of frame */
 229         if (len == FRAME_LEN*2) {
 230                 uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK;
 231                 if (tail_next != frame_ring_head) {
 232                         frame_ring[frame_ring_tail].frame = (frame & FRAME_MASK);
 233                         frame_ring[frame_ring_tail].time = start_angle_time;
 234                         frame_ring_tail = tail_next;
 235                 }
 236                 if ((led_cpt & 0x7) == 0)
 237                         LED3_TOGGLE();
 238                 led_cpt ++;
 239         }
 240
 241         prev_time = cur_time;
 242         prev_tsop = cur_tsop;
 243 }
 244
 245 /* absolute value */
 246 static inline int32_t AbS(int32_t x)
 247 {
 248         if (x > 0)
 249                 return x;
 250         else
 251                 return -x;
 252 }
 253
 254 /* Get the speed of motor (tr / 1000s)
 255  * - icr_cpt is the number of CS period between 2 ICR updates
 256  * - icr_diff is the difference of ICR values between the ICR updates
 257  *   (modulo 65536 obviously) */
 258 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
 259 {
 260         int32_t best_diff = 65536L;
 261         int8_t best_cpt = -2;
 262         int32_t diff;
 263         int8_t i;
 264
 265         /* too slow (less than 1 tr/s) */
 266         if (icr_cpt > CPT_ICR_MAX)
 267                 return 1000L;
 268
 269         /* too fast (more than 100 tr/s) */
 270         if (icr_cpt < CPT_ICR_MIN)
 271                 return 100000L;
 272
 273         /* try to get the real time knowning icr_cpt and icr_diff */
 274         for (i=-1; i<2; i++) {
 275                 diff = ((icr_cpt+i)&3) * 16384L;
 276                 diff += (icr_diff & 0x3fff);
 277                 diff -= icr_diff;
 278                 if (diff > 32768L)
 279                         diff -= 65536L;
 280                 if (diff < -32768)
 281                         diff += 65536L;
 282
 283                 if (AbS(diff) < AbS(best_diff)) {
 284                         best_diff = diff;
 285                         best_cpt = icr_cpt + i;
 286                 }
 287         }
 288
 289         /* real time difference in 1/2 us */
 290         diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
 291         return 2000000000L/diff;
 292 }
 293
 294 int main(void)
 295 {
 296         uint16_t prev_cs = 0;
 297         uint16_t prev_icr = 0;
 298         uint16_t icr = 0;
 299         uint16_t diff_icr = 0;
 300         uint8_t cpt_icr = 0;
 301         uint8_t cpt = 0;
 302         int32_t speed, out, err;
 303         uint16_t tcnt3;
 304         uint8_t x = 0;
 305
 306         /* LEDS */
 307         LED1_DDR |= _BV(LED1_BIT);
 308         LED2_DDR |= _BV(LED2_BIT);
 309         LED3_DDR |= _BV(LED3_BIT);
 310         DDRB |= 0x10; /* OC0 (laser pwm) */
 311
 312         /* PID init */
 313         pid_init(&beacon_tsop.pid);
 314         pid_set_gains(&beacon_tsop.pid, 500, 0, 0);
 315         pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095);
 316         pid_set_out_shift(&beacon_tsop.pid, 10);
 317         pid_set_derivate_filter(&beacon_tsop.pid, 4);
 318
 319         uart_init();
 320         fdevopen(uart0_dev_send, uart0_dev_recv);
 321
 322         rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
 323         snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");
 324         rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
 325
 326         debug_tsop();
 327         debug_serial();
 328
 329         /* configure external interrupt for TSOP */
 330         EICRx_TSOP |= _BV(ISCx0_TSOP);
 331         EIMSK |= _BV(INTx_TSOP);
 332
 333         /* pwm for motor */
 334         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
 335                                  TIMER1_PRESCALER_DIV_1);
 336         PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
 337
 338         /* pwm for laser:
 339          *  - clear on timer compare (CTC)
 340          *  - Toggle OC0 on compare match
 341          *  - prescaler = 1 */
 342         TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
 343         OCR0 = 80; /* f = 100 khz at 16 Mhz */
 344
 345         /* configure timer 3: CLK/8
 346          * it is used as a reference time
 347          * enable noise canceller for ICP3 */
 348         TCCR3B = _BV(ICNC3) | _BV(CS11);
 349
 350         sei();
 351
 352         /* Control system will be done in main loop */
 353         while (1) {
 354
 355                 /* process pending bytes on uart */
 356                 cmdline_process();
 357
 358                 /* monitor the value of ICR (which is modified
 359                  * automatically on TT rising edge). If the value
 360                  * changed, process the time difference. */
 361                 if (ETIFR & _BV(ICF3)) {
 362                         cli();
 363                         icr = ICR3;
 364                         sei();
 365                         ETIFR = _BV(ICF3);
 366
 367                         //LED2_TOGGLE();
 368                         diff_icr = (icr - prev_icr);
 369                         cpt_icr = cpt;
 370                         prev_icr = icr;
 371                         cpt = 0;
 372                         speed = get_speed(cpt_icr, diff_icr);
 373                 }
 374
 375                 /* read time reference */
 376                 cli();
 377                 tcnt3 = TCNT3;
 378                 sei();
 379
 380                 /* wait cs period */
 381                 if (tcnt3 - prev_cs < CS_PERIOD)
 382                         continue;
 383
 384                 /* CS LED */
 385                 if (x & 0x80)
 386                         LED1_ON();
 387                 else
 388                         LED1_OFF();
 389                 x++;
 390
 391                 /* process CS... maybe we don't need to use
 392                  * control_system_manager, just PID is enough */
 393                 if (cpt == CPT_ICR_MAX)
 394                         speed = 0;
 395                 else
 396                         speed = get_speed(cpt_icr, diff_icr);
 397
 398                 /* enabled laser when rotation speed if at least 5tr/s */
 399                 if (speed > 5000)
 400                         LASER_ON();
 401                 else
 402                         LASER_OFF();
 403
 404                 err = CS_CONSIGN - speed;
 405                 out = pid_do_filter(&beacon_tsop.pid, err);
 406                 if (x == 0 && beacon_tsop.debug_speed)
 407                         printf("%ld %ld\n", speed, out);
 408                 if (out < 0)
 409                         out = 0;
 410                 /* XXX */
 411                 if (out > 2000)
 412                         out = 2000;
 413
 414                 pwm_ng_set(&beacon_tsop.pwm_motor, out);
 415
 416                 prev_cs = tcnt3;
 417
 418                 /* count the number of CS period between 2 ICR
 419                  * captures */
 420                 if (cpt < CPT_ICR_MAX)
 421                         cpt ++;
 422
 423                 /* after CS, check if we have a new frame in ring */
 424                 if (frame_ring_head != frame_ring_tail) {
 425                         uint8_t head_next;
 426                         uint32_t frame;
 427                         head_next = (frame_ring_head+1) & FRAME_RING_MASK;
 428                         frame = frame_ring[frame_ring_head].frame;
 429
 430                         /* display if needed */
 431                         if (beacon_tsop.debug_frame) {
 432                                 uint8_t beacon_id;
 433                                 uint16_t data;
 434
 435                                 beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK;
 436                                 data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK;
 437                                 printf("ID=%d data=%d time=%d\r\n",
 438                                        beacon_id, data,
 439                                        frame_ring[frame_ring_head].time);
 440                         }
 441                         frame_ring_head = head_next;
 442                 }
 443
 444         }
 445
 446         return 0;
 447 }