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