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