projects/microb2010/tests/static_beacon/static_beacon.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 <math.h>
  24
  25 #include <aversive.h>
  26 #include <aversive/wait.h>
  27 #include <aversive/pgmspace.h>
  28
  29 #include <uart.h>
  30
  31 /*
  32  * Leds: PD5 -> PD7
  33  * Photodiodes: PC0 PC1
  34  * IR: PB1, OC1A
  35  */
  36
  37 /*
  38  * hfuse:  RSTDISBL=1 WTDON=1 SPIEN=0 CKOPT=0 EESAVE=1 BOOTSZ1=0 BOOTSZ0=0 BOOTRST=1
  39  * lfuse:  BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1
  40  */
  41
  42 //#define NO_MODULATION
  43 #define WAIT_LASER
  44 //#define MODUL_455KHZ
  45 #define MODUL_56KHZ
  46 //#define MODUL_38KHZ
  47 //#define INC_FRAME
  48 //#define FIXED_FRAME
  49 //#define SEND_RAWTIME
  50 #define LOCK_BUS
  51 #define SPEED_20RPS
  52
  53 /* beacon identifier: must be odd, 3 bits */
  54 #define BEACON_ID 0x1
  55 #define BEACON_ID_MASK 0x7
  56 #define BEACON_ID_SHIFT 0
  57
  58 /* */
  59 #define FRAME_DATA_MASK  0x0FF8
  60 #define FRAME_DATA_SHIFT 3
  61
  62 #if (defined MODUL_455KHZ)
  63 #define N_PERIODS   15
  64 #define N_CYCLES_0  17
  65 #define N_CYCLES_1  17
  66 #elif (defined MODUL_56KHZ)
  67 #define N_PERIODS   15
  68 #define N_CYCLES_0  143
  69 #define N_CYCLES_1  143
  70 #elif (defined MODUL_38KHZ)
  71 #define N_PERIODS   15
  72 #define N_CYCLES_0  210
  73 #define N_CYCLES_1  210
  74 #else
  75 #error "no freq defined"
  76 #endif
  77
  78 #define MIN_DIST 150.
  79 #define MAX_DIST 3600.
  80 #define LASER_DIST 25.
  81
  82 #define N_CYCLES_PERIOD (N_CYCLES_0 + N_CYCLES_1)
  83
  84 #define LED_PORT PORTD
  85 #define LED_DDR  DDRD
  86 #define LED1_BIT  5
  87 #define LED2_BIT  6
  88 #define LED3_BIT  7
  89
  90 #define LED_TOGGLE(port, bit) do {              \
  91           if (port & _BV(bit))                  \
  92                   port &= ~_BV(bit);            \
  93           else                                  \
  94                   port |= _BV(bit);             \
  95   } while(0)
  96
  97 #define LED1_ON()  sbi(LED_PORT, LED1_BIT)
  98 #define LED1_OFF() cbi(LED_PORT, LED1_BIT)
  99 #define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT)
 100 #define LED2_ON()  sbi(LED_PORT, LED2_BIT)
 101 #define LED2_OFF() cbi(LED_PORT, LED2_BIT)
 102 #define LED2_TOGGLE() LED_TOGGLE(LED_PORT, LED2_BIT)
 103 #define LED3_ON()  sbi(LED_PORT, LED3_BIT)
 104 #define LED3_OFF() cbi(LED_PORT, LED3_BIT)
 105 #define LED3_TOGGLE() LED_TOGGLE(LED_PORT, LED3_BIT)
 106
 107 #define IR_PORT PORTB
 108 #define IR_DDR  DDRB
 109 #define IR_BIT  1
 110
 111 #define LOCKBUS_PORT PORTD
 112 #define LOCKBUS_PIN  PIND
 113 #define LOCKBUS_DDR  DDRD
 114 #define LOCKBUS_BIT  3
 115
 116 /* FRAME must be odd */
 117 /* #define FRAME 0x0B /\* in little endian 1-1-0-1 *\/ */
 118 /* #define FRAME_LEN 4 */
 119 #define FRAME 0x621 /* in little endian */
 120 #define FRAME_LEN 16
 121
 122 /* pin returns !0 when nothing, and 0 when laser is on photodiode */
 123 #define PHOTO_PIN PINB
 124 #define PHOTO_BIT 0
 125 #define READ_PHOTO() (!!(PHOTO_PIN & (_BV(PHOTO_BIT))))
 126
 127 /* IR_DELAY must be < 32768 */
 128 #define MIN_INTER_TIME    ((uint16_t)(100*16U))   /* t~=160us dist=400cm */
 129 #define MAX_INTER_TIME    ((uint16_t)(4000*16U)) /* t=4ms dist=16cm */
 130 #define IR_DELAY          ((uint16_t)(MAX_INTER_TIME/2))
 131 #define INTER_LASER_TIME   10 /* in ms */
 132
 133 extern prog_uint16_t framedist_table[];
 134 /* basic functions to transmit on IR */
 135
 136 static inline void xmit_0(void)
 137 {
 138         uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4);
 139 #ifdef NO_MODULATION
 140         cbi(IR_PORT, IR_BIT);
 141 #else
 142         TCCR1B = 0;
 143         TCCR1A = 0;
 144 #endif
 145         _delay_loop_2(t); /* 4 cycles per loop */
 146 }
 147
 148 static inline void xmit_1(void)
 149 {
 150         uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4);
 151 #ifdef NO_MODULATION
 152         sbi(IR_PORT, IR_BIT);
 153 #else
 154         TCCR1B = _BV(WGM13) | _BV(WGM12);
 155         TCNT1 = N_CYCLES_PERIOD-1;
 156         TCCR1A = _BV(COM1A1) | _BV(WGM11);
 157         ICR1 = N_CYCLES_PERIOD;
 158         TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
 159 #endif
 160         _delay_loop_2(t); /* 4 cycles per loop */
 161 }
 162
 163 /* transmit in manchester code */
 164
 165 static inline void xmit_manchester_0(void)
 166 {
 167         xmit_0();
 168         xmit_1();
 169 }
 170
 171 static inline void xmit_manchester_1(void)
 172 {
 173         xmit_1();
 174         xmit_0();
 175 }
 176
 177 /* transmit a full frame. Each byte is lsb first. */
 178
 179 static void xmit_bits(uint32_t frame, uint8_t nbit)
 180 {
 181         uint8_t i;
 182
 183         for (i=0; i<nbit; i++) {
 184                 if (frame & 1)
 185                         xmit_manchester_1();
 186                 else
 187                         xmit_manchester_0();
 188
 189                 /* next bit */
 190                 frame >>= 1UL;
 191         }
 192         xmit_0();
 193 }
 194
 195 /* val is 12 bits. Return the 16 bits value that includes the 4 bits
 196  * cksum in MSB. */
 197 static uint16_t do_cksum(uint16_t val)
 198 {
 199         uint16_t x, cksum;
 200
 201         x = (val & 0xfff);
 202         /* add the three 4-bits blocks of x together */
 203         cksum = x & 0xf;
 204         x = x >> 4;
 205         cksum += x & 0xf;
 206         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 207         x = x >> 4;
 208         cksum += x & 0xf;
 209         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 210         cksum = (~cksum) & 0xf;
 211         return (cksum << 12) + (val & 0xfff);
 212 }
 213
 214 /* get the frame from laser time difference, return 0 on error (this
 215  * frame cannot be sent). */
 216 static uint32_t get_frame(uint16_t laserdiff)
 217 {
 218         uint32_t frame = 0;
 219         uint16_t val, mid, min, max;
 220
 221         /* for calibration, return the time */
 222 #ifdef INC_FRAME
 223         static uint16_t fr = 1;
 224         fr += 2;
 225         return fr;
 226 #endif
 227 #ifdef SEND_RAWTIME
 228         return laserdiff | 1; /* frame must be odd */
 229 #endif
 230
 231         min = 0;
 232         max = 511;
 233         while (min != max) {
 234                 mid = (max + min + 1) / 2;
 235                 val = pgm_read_word(&framedist_table[mid]);
 236
 237                 if (laserdiff > val)
 238                         max = mid - 1;
 239                 else
 240                         min = mid;
 241         }
 242
 243         frame |= ((uint32_t)((min << FRAME_DATA_SHIFT)
 244                              & FRAME_DATA_MASK));
 245         frame |= BEACON_ID;
 246
 247         /* process cksum and return */
 248         return do_cksum(frame);
 249 }
 250
 251 /* Wait 2 consecutive rising edges on photodiode. Return 0 on success,
 252  * in this case, the 'when' pointed area is assigned to the time when
 253  * IR signal should be sent. The 'laserdiff' pointer is the time
 254  * between the 2 lasers, in timer unit. */
 255 static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff)
 256 {
 257         uint16_t time1, time2;
 258         uint16_t diff;
 259
 260         /* set timer to 16Mhz, we will use ICP */
 261         TCCR1A = 0;
 262         TCCR1B = _BV(CS10);
 263
 264         /* wait until all is off (inverted logic) */
 265         while (READ_PHOTO() == 0);
 266         TIFR = _BV(ICF1);
 267
 268         /* wait falling edge */
 269         while ((TIFR & _BV(ICF1)) == 0);
 270         time1 = ICR1;
 271
 272         LED1_ON();
 273
 274         /* wait a bit to avoid oscillations */
 275         while ((uint16_t)(TCNT1-time1) < MIN_INTER_TIME);
 276         TIFR = _BV(ICF1);
 277
 278         /* wait next falling edge + timeout */
 279         while ((TIFR & _BV(ICF1)) == 0) {
 280                 diff = TCNT1 - time1;
 281                 if (diff > MAX_INTER_TIME)
 282                         return -1;
 283         }
 284
 285         LED2_ON();
 286
 287         /* get time of 2nd laser */
 288         time2 = ICR1;
 289         TIFR = _BV(ICF1);
 290
 291         /* process time difference */
 292         diff = time2 - time1;
 293         if (diff > MAX_INTER_TIME)
 294                 return -1;
 295
 296         /*
 297          * diff/2 is < 32768
 298          * ir_delay is < 32768
 299          * current time is ~time1 + diff
 300          */
 301         *when = time1 + (diff/2) + IR_DELAY;
 302         *laserdiff = diff;
 303
 304         /* laser ok */
 305         return 0;
 306 }
 307
 308 /* */
 309
 310 int main(void)
 311 {
 312         /* must be odd */
 313         uint32_t frame = FRAME;
 314         int8_t ret;
 315         uint16_t when = 0;
 316         uint16_t diff = 0;
 317
 318         /* LEDS */
 319         LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT);
 320         IR_DDR |= _BV(IR_BIT);
 321
 322         uart_init();
 323         fdevopen(uart0_dev_send, uart0_dev_recv);
 324         sei();
 325
 326 #if 0
 327         while (1) {
 328                 int16_t c;
 329                 c = uart_recv_nowait(0);
 330                 if (c != -1)
 331                         printf("%c", (char)(c+1));
 332                 LED1_ON();
 333                 wait_ms(500);
 334                 LED1_OFF();
 335                 wait_ms(500);
 336         }
 337 #endif
 338
 339 #if 0
 340         while (1) {
 341                 if (READ_PHOTO())
 342                         LED1_ON();
 343                 else
 344                         LED1_OFF();
 345         }
 346 #endif
 347
 348 #ifndef NO_MODULATION
 349         /* configure PWM */
 350         ICR1 = N_CYCLES_PERIOD;
 351         OCR1A = N_CYCLES_1;
 352         TCCR1A = _BV(COM1A1) | _BV(WGM11);
 353         TCCR1B = _BV(WGM13) | _BV(WGM12);
 354 #endif
 355
 356 #if 0
 357         /* test freq */
 358         ICR1 = N_CYCLES_PERIOD;
 359         OCR1A = N_CYCLES_1;
 360         TCCR1B = _BV(WGM13) | _BV(WGM12);
 361         TCNT1 = N_CYCLES_PERIOD-1;
 362         TCCR1A = _BV(COM1A1) | _BV(WGM11);
 363         TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
 364         while (1);
 365 #endif
 366
 367 #if 0
 368         /* test freq ~ 400khz */
 369         ICR1 = 38;
 370         OCR1A = 19;
 371         TCCR1B = _BV(WGM13) | _BV(WGM12);
 372         TCNT1 = 37;
 373         TCCR1A = _BV(COM1A1) | _BV(WGM11);
 374         TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
 375
 376         /* motor PWM 50%, 8khz */
 377         DDRB |= 0x08;
 378         OCR2 = 50;
 379         TCCR2 = _BV(WGM21) | _BV(WGM20) | _BV(COM21) | _BV(CS21) ;
 380
 381         while (1);
 382 #endif
 383
 384 #ifdef LOCK_BUS
 385         LOCKBUS_PORT &= (~_BV(LOCKBUS_BIT));
 386 #endif
 387
 388         /* configure timer 0, prescaler = 64 */
 389         TCCR0 = _BV(CS01) | _BV(CS00);
 390
 391         while (1) {
 392
 393 #ifdef WAIT_LASER
 394                 ret = wait_laser(&when, &diff);
 395
 396                 LED1_OFF();
 397                 LED2_OFF();
 398
 399                 if (ret)
 400                         continue;
 401 #endif /* WAIT_LASER */
 402
 403 #ifdef LOCK_BUS
 404                 if ((LOCKBUS_PIN & _BV(LOCKBUS_BIT)) == 0)
 405                         continue;
 406                 LOCKBUS_DDR |= _BV(LOCKBUS_BIT);
 407 #endif
 408
 409 #ifdef FIXED_FRAME
 410                 frame = FRAME;
 411 #else
 412                 frame = get_frame(diff);
 413 #endif /* FIXED_FRAME */
 414                 /* cannot convert into frame... skip it */
 415                 if (frame == 0) {
 416                         wait_ms(INTER_LASER_TIME);
 417                         continue;
 418                 }
 419
 420
 421 #ifdef WAIT_LASER
 422                 /* wait before IR xmit */
 423                 while ((int16_t)(when-TCNT1) > 0);
 424 #endif /* WAIT_LASER */
 425
 426                 LED3_ON();
 427                 /* ok, transmit frame */
 428                 xmit_bits(frame, FRAME_LEN);
 429
 430                 LED3_OFF();
 431
 432 #ifdef LOCK_BUS
 433                 LOCKBUS_DDR &= (~_BV(LOCKBUS_BIT));
 434 #endif
 435
 436                 /* don't watch a laser during this time */
 437                 wait_ms(INTER_LASER_TIME);
 438         }
 439         return 0;
 440 }