projects/microb2010/tests/beacon_tsop/main.c

   1 /*
   2  *  Copyright Droids Corporation (2010)
   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 #include <pid.h>
  30 #include <pwm_ng.h>
  31 #include <parse.h>
  32 #include <rdline.h>
  33 #include <vect_base.h>
  34 #include <lines.h>
  35 #include <circles.h>
  36
  37 #include "cmdline.h"
  38 #include "uart_proto.h"
  39 #include "trigo.h"
  40 #include "main.h"
  41 #include "board2006.h"
  42 //#include "board2010.h"
  43
  44 /******************* TSOP */
  45
  46 struct detected_frame {
  47         uint16_t frame;
  48         uint16_t ref_time;
  49         uint16_t time;
  50         uint16_t tick;
  51 };
  52
  53 /* frame */
  54 struct frame_status {
  55         uint8_t led_cpt;
  56         uint16_t ref_time;
  57         uint16_t start_time;
  58         uint16_t frame;
  59         uint16_t mask;
  60         uint16_t prev_time;
  61         uint16_t time_long;
  62         uint16_t time_short;
  63         uint8_t prev_tsop;
  64         uint8_t len;
  65         uint8_t frame_len;
  66         uint8_t val;
  67 #define FRAME_RING_ORDER 4
  68 #define FRAME_RING_SIZE  (1<<FRAME_RING_ORDER)
  69 #define FRAME_RING_MASK  (FRAME_RING_SIZE-1)
  70         uint8_t head;
  71         uint8_t tail;
  72         struct detected_frame ring[FRAME_RING_SIZE];
  73 };
  74
  75 static struct frame_status static_beacon;
  76 static struct frame_status opp_beacon;
  77 static uint16_t tick = 0;
  78
  79 #define MIN_DIST 200.
  80 #define MAX_DIST 3500.
  81
  82 /* in ticks (=CS_PERIOD), age before the entry is removed from ring */
  83 #define MAX_CAP_AGE 30
  84
  85 /********************** CS */
  86
  87 /* 8ms, easier if it's a pow of 2 */
  88 #define CS_PERIOD_US (8192)
  89 #define CS_PERIOD ((uint16_t)(CS_PERIOD_US*2))
  90 #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */
  91 #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US))   /* too fast = 100 tr/s */
  92
  93 /* in tr / 1000s */
  94 #define CS_CONSIGN (10 * 1000L)
  95
  96 /* 5% tolerance to validate captures, period is in  */
  97 #define TIM3_UNIT 250000000L
  98 #define MOTOR_PERIOD_MIN ((uint32_t)((250000000L/CS_CONSIGN) * 0.95))
  99 #define MOTOR_PERIOD_MAX ((uint32_t)((250000000L/CS_CONSIGN) * 1.05))
 100
 101 /* pwm for laser:
 102  *  - clear on timer compare (CTC)
 103  *  - Toggle OC0 on compare match
 104  *  - prescaler = 1 */
 105 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
 106 #define LASER_OFF() do { TCCR0 = 0; } while (0)
 107
 108 struct beacon_tsop beacon_tsop;
 109
 110 static uint32_t current_motor_period;
 111
 112 void debug_serial(void)
 113 {
 114 #if 0
 115         while (1) {
 116                 int16_t c;
 117                 c = uart_recv_nowait(0);
 118                 if (c != -1)
 119                         printf("%c", (char)(c+1));
 120                 LED1_ON();
 121                 wait_ms(500);
 122                 LED1_OFF();
 123                 wait_ms(500);
 124         }
 125 #endif
 126 }
 127
 128 void debug_tsop(void)
 129 {
 130 #if 0
 131         while (1) {
 132                 if (TSOP_READ())
 133                         LED1_OFF();
 134                 else {
 135                         LED1_ON();
 136                         wait_ms(500);
 137                 }
 138         }
 139 #endif
 140 }
 141
 142 /* val is 16 bits, including 4 bits-cksum in MSB, return 0xFFFF is
 143  * cksum is wrong, or the 12 bits value on success. */
 144 static uint16_t verify_cksum(uint16_t val)
 145 {
 146         uint16_t x, cksum;
 147
 148         x = (val & 0xfff);
 149         /* add the four 4-bits blocks of val together */
 150         cksum = val & 0xf;
 151         val = val >> 4;
 152         cksum += val & 0xf;
 153         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 154         val = val >> 4;
 155         cksum += val & 0xf;
 156         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 157         val = val >> 4;
 158         cksum += val & 0xf;
 159         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 160         if (cksum == 0xf)
 161                 return x;
 162         return 0xffff; /* wrong cksum */
 163 }
 164
 165 static inline void decode_frame(struct frame_status *status,
 166                                 uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
 167 {
 168         uint16_t diff_time = cur_time - status->prev_time;
 169
 170         /* first rising edge */
 171         if (status->len == 0 && cur_tsop && diff_time > status->time_long) {
 172                 status->len = 1;
 173                 status->val = 1;
 174                 status->frame = 0;
 175                 status->start_time = cur_time;
 176                 status->ref_time = ref_time;
 177                 status->mask = 1;
 178         }
 179         /* any short edge */
 180         else if (status->len != 0 && diff_time < status->time_short) {
 181                 if (status->len & 1) {
 182                         if (status->val)
 183                                 status->frame |= status->mask;
 184                         status->mask <<= 1;
 185                 }
 186                 status->len ++;
 187         }
 188         /* any long edge */
 189         else if (status->len != 0 && diff_time < status->time_long) {
 190                 status->val = !status->val;
 191                 if (status->val)
 192                         status->frame |= status->mask;
 193                 status->mask <<= 1;
 194                 status->len += 2;
 195         }
 196         /* error case, reset */
 197         else {
 198                 status->len = 0;
 199         }
 200
 201         /* end of frame */
 202         if (status->len == status->frame_len*2) {
 203                 uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
 204                 uint16_t frame_mask;
 205
 206                 frame_mask = (1 << status->frame_len) - 1;
 207
 208                 if (tail_next != status->head) {
 209                         status->ring[status->tail].frame = (status->frame & frame_mask);
 210                         status->ring[status->tail].ref_time = status->ref_time;
 211                         status->ring[status->tail].time = status->start_time;
 212                         status->ring[status->tail].tick = tick;
 213                         status->tail = tail_next;
 214                         if ((status->led_cpt & 0x7) == 0)
 215                                 LED3_TOGGLE();
 216                         status->led_cpt ++;
 217                 }
 218                 status->len = 0;
 219         }
 220
 221         status->prev_time = cur_time;
 222         status->prev_tsop = cur_tsop;
 223 }
 224
 225 /* decode frame */
 226 SIGNAL(SIG_TSOP_STA) {
 227         static uint8_t running = 0;
 228
 229         /* tsop status */
 230         uint8_t cur_tsop;
 231         uint16_t ref_time;
 232         uint16_t cur_time;
 233
 234         ref_time = ICR3;
 235         cur_time = TCNT3;
 236         cur_tsop = TSOP_STA_READ();
 237
 238         /* avoid interruption stacking */
 239         if (running)
 240                 return;
 241         running = 1;
 242         sei();
 243
 244         if (cur_tsop)
 245                 LED2_ON();
 246         else
 247                 LED2_OFF();
 248
 249         decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
 250
 251         running = 0;
 252 }
 253
 254 /* decode frame */
 255 SIGNAL(SIG_TSOP_OPP) {
 256         static uint8_t running = 0;
 257
 258         /* tsop status */
 259         uint8_t cur_tsop;
 260         uint16_t ref_time;
 261         uint16_t cur_time;
 262
 263         ref_time = ICR3;
 264         cur_time = TCNT3;
 265         cur_tsop = TSOP_OPP_READ();
 266
 267         /* avoid interruption stacking */
 268         if (running)
 269                 return;
 270         running = 1;
 271         sei();
 272
 273 /*      if (cur_tsop) */
 274 /*              LED2_ON(); */
 275 /*      else */
 276 /*              LED2_OFF(); */
 277
 278         decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
 279
 280         running = 0;
 281 }
 282
 283 /* absolute value */
 284 static inline int32_t AbS(int32_t x)
 285 {
 286         if (x > 0)
 287                 return x;
 288         else
 289                 return -x;
 290 }
 291
 292 /* Get the speed of motor (tr / 1000s)
 293  * - icr_cpt is the number of CS period between 2 ICR updates
 294  * - icr_diff is the difference of ICR values between the ICR updates
 295  *   (modulo 65536 obviously) */
 296 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
 297 {
 298         int32_t best_diff = 65536L;
 299         int8_t best_cpt = -2;
 300         int32_t diff;
 301         int8_t i;
 302
 303         /* too slow (less than 1 tr/s) */
 304         if (icr_cpt > CPT_ICR_MAX)
 305                 return 1000L;
 306
 307         /* too fast (more than 100 tr/s) */
 308         if (icr_cpt < CPT_ICR_MIN)
 309                 return 100000L;
 310
 311         /* try to get the real time knowning icr_cpt and icr_diff */
 312         for (i=-1; i<2; i++) {
 313                 diff = ((icr_cpt+i)&3) * 16384L;
 314                 diff += (icr_diff & 0x3fff);
 315                 diff -= icr_diff;
 316                 if (diff > 32768L)
 317                         diff -= 65536L;
 318                 if (diff < -32768)
 319                         diff += 65536L;
 320
 321                 if (AbS(diff) < AbS(best_diff)) {
 322                         best_diff = diff;
 323                         best_cpt = icr_cpt + i;
 324                 }
 325         }
 326
 327         /* real time difference in timer unit (resolution 4us) */
 328         diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
 329         current_motor_period = diff; /* save it in global var */
 330         return 250000000L/diff;
 331 }
 332
 333 /* process the received frame ring */
 334 static void process_sta_ring(struct frame_status *status)
 335 {
 336         uint8_t head, head_next;
 337         uint16_t frame, frametick;
 338         uint8_t found = 0;
 339         uint8_t beacon_id;
 340
 341         /* beacon 0 */
 342         uint16_t data0, time0, ref_time0;
 343         double angle0;
 344         double dist0;
 345
 346         /* beacon 1 */
 347         uint16_t data1, time1, ref_time1;
 348         double angle1;
 349         double dist1;
 350
 351         point_t pos;
 352         double a;
 353
 354         /* remove too old captures from the ring */
 355         while (status->head != status->tail) {
 356                 head_next = (status->head+1) & FRAME_RING_MASK;
 357                 frametick = status->ring[status->head].tick;
 358                 if ((uint16_t)(tick - frametick) < MAX_CAP_AGE)
 359                         break;
 360                 status->head = head_next;
 361         }
 362
 363         head = status->head;
 364         /* after CS, check if we have a new frame in ring */
 365         while (head != status->tail) {
 366                 head_next = (head+1) & FRAME_RING_MASK;
 367                 frame = status->ring[head].frame;
 368
 369                 /* ignore bad cksum */
 370                 if (verify_cksum(frame) == 0xFFFF)
 371                         continue;
 372
 373                 beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
 374                 if (beacon_id != TSOP_STA_BEACON_ID0 &&
 375                     beacon_id != TSOP_STA_BEACON_ID1)
 376                         continue;
 377
 378                 /* if motor speed is not good, skip values  */
 379                 if (current_motor_period < MOTOR_PERIOD_MIN)
 380                         continue;
 381                 if (current_motor_period > MOTOR_PERIOD_MAX)
 382                         continue;
 383
 384                 /* display if needed */
 385                 if (beacon_tsop.debug_frame) {
 386                         printf("STA ID=%d time=%d\r\n",
 387                                beacon_id, status->ring[head].time);
 388                 }
 389
 390                 if (beacon_id == TSOP_STA_BEACON_ID0) {
 391                         found |= 0x1;
 392                         data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
 393                         time0 = status->ring[head].time;
 394                         ref_time0 = status->ring[head].ref_time;
 395                 }
 396                 else if (beacon_id == TSOP_STA_BEACON_ID1) {
 397                         found |= 0x2;
 398                         data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
 399                         time1 = status->ring[head].time;
 400                         ref_time1 = status->ring[head].ref_time;
 401                 }
 402
 403                 head = head_next;
 404         }
 405
 406         /* if we didn't found beacon 0 and 1, return */
 407         if (found != 0x3)
 408                 return;
 409
 410         /* update ring head */
 411         status->head = head;
 412
 413         /* beacon 0 */
 414         dist0 = data0;
 415         dist0 /= 512.;
 416         dist0 *= (MAX_DIST-MIN_DIST);
 417         dist0 += MIN_DIST;
 418
 419         time0 = time0 - ref_time0;
 420         angle0 = (double)time0 / (double)current_motor_period;
 421         if (angle0 > 1.)
 422                 angle0 -= 1.;
 423         if (angle0 > 1.)
 424                 return; /* fail */
 425         angle0 *= (2 * M_PI);
 426         if (angle0 > M_PI)
 427                 angle0 -= M_PI;
 428
 429         /* beacon 1 */
 430         dist1 = data1;
 431         dist1 /= 512.;
 432         dist1 *= (MAX_DIST-MIN_DIST);
 433         dist1 += MIN_DIST;
 434
 435         time1 = time1 - ref_time1;
 436         angle1 = (double)time1 / (double)current_motor_period;
 437         if (angle1 > 1.)
 438                 angle1 -= 1.;
 439         if (angle1 > 1.)
 440                 return; /* fail */
 441         angle1 *= (2 * M_PI);
 442         if (angle0 > M_PI)
 443                 angle0 -= M_PI;
 444
 445         if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0,
 446                          angle1, dist1) < 0)
 447                 return;
 448
 449         xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a);
 450 }
 451
 452 /* process the received frame ring */
 453 static void process_opp_ring(struct frame_status *status)
 454 {
 455         uint8_t head_next;
 456         uint16_t frame;
 457         uint8_t found = 0;
 458         uint8_t beacon_id;
 459         uint16_t data, time, ref_time;
 460         double angle;
 461         double dist;
 462
 463         /* after CS, check if we have a new frame in ring */
 464         while (status->head != status->tail) {
 465                 head_next = (status->head+1) & FRAME_RING_MASK;
 466                 frame = status->ring[status->head].frame;
 467
 468                 /* ignore bad cksum */
 469                 if (verify_cksum(frame) == 0xFFFF)
 470                         continue;
 471
 472                 beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
 473                 if (beacon_id != TSOP_OPP_BEACON_ID)
 474                         continue;
 475
 476                 /* if motor speed is not good, skip values  */
 477                 if (current_motor_period < MOTOR_PERIOD_MIN)
 478                         continue;
 479                 if (current_motor_period > MOTOR_PERIOD_MAX)
 480                         continue;
 481
 482                 found = 1;
 483                 data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK;
 484                 time = status->ring[status->head].time;
 485                 ref_time = status->ring[status->head].ref_time;
 486
 487                 /* display if needed */
 488                 if (beacon_tsop.debug_frame) {
 489                         printf("OPP ID=%d data=%d time=%d\r\n",
 490                                beacon_id, data,
 491                                status->ring[status->head].time);
 492                 }
 493                 status->head = head_next;
 494         }
 495
 496         if (found == 0)
 497                 return;
 498
 499         dist = data;
 500         dist /= 512.;
 501         dist *= (MAX_DIST-MIN_DIST);
 502         dist += MIN_DIST;
 503
 504         time = time - ref_time;
 505         angle = (double)time / (double)current_motor_period;
 506         if (angle > 1.)
 507                 angle -= 1.;
 508         if (angle > 1.)
 509                 return; /* fail */
 510         angle *= 3600; /* angle in 1/10 deg */
 511
 512         xmit_opp((uint16_t)dist, (uint16_t)angle);
 513 }
 514
 515 int main(void)
 516 {
 517         uint16_t prev_cs = 0;
 518         uint16_t prev_icr = 0;
 519         uint16_t icr = 0;
 520         uint16_t diff_icr = 0;
 521         uint8_t cpt_icr = 0;
 522         uint8_t cpt = 0;
 523         int32_t speed, out, err;
 524         uint16_t tcnt3;
 525         uint8_t x = 0;
 526
 527         opp_beacon.frame_len = TSOP_OPP_FRAME_LEN;
 528         opp_beacon.time_long = TSOP_OPP_TIME_LONG;
 529         opp_beacon.time_short = TSOP_OPP_TIME_SHORT;
 530
 531         static_beacon.frame_len = TSOP_STA_FRAME_LEN;
 532         static_beacon.time_long = TSOP_STA_TIME_LONG;
 533         static_beacon.time_short = TSOP_STA_TIME_SHORT;
 534
 535         /* LEDS */
 536         LED1_DDR |= _BV(LED1_BIT);
 537         LED2_DDR |= _BV(LED2_BIT);
 538         LED3_DDR |= _BV(LED3_BIT);
 539         DDRB |= 0x10; /* OC0 (laser pwm) */
 540
 541         /* PID init */
 542         pid_init(&beacon_tsop.pid);
 543         pid_set_gains(&beacon_tsop.pid, 500, 0, 0);
 544         pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095);
 545         pid_set_out_shift(&beacon_tsop.pid, 10);
 546         pid_set_derivate_filter(&beacon_tsop.pid, 4);
 547
 548         uart_init();
 549         fdevopen(uart0_dev_send, uart0_dev_recv);
 550
 551         rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
 552         snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");
 553         rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
 554
 555         debug_tsop();
 556         debug_serial();
 557
 558         /* configure external interrupt for TSOP */
 559         EICRx_TSOP |= _BV(ISCx0_TSOP_STA) | _BV(ISCx0_TSOP_OPP);
 560         EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP);
 561
 562         /* pwm for motor */
 563         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
 564                                  TIMER1_PRESCALER_DIV_1);
 565         PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
 566
 567         /* pwm for laser:
 568          *  - clear on timer compare (CTC)
 569          *  - Toggle OC0 on compare match
 570          *  - prescaler = 1 */
 571         TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
 572         OCR0 = 80; /* f = 100 khz at 16 Mhz */
 573
 574         /* configure timer 3: CLK/64
 575          * it is used as a reference time
 576          * enable noise canceller for ICP3 */
 577         TCCR3B = _BV(ICNC3) | _BV(CS11) | _BV(CS10);
 578
 579         sei();
 580
 581         /* Control system will be done in main loop */
 582         while (1) {
 583
 584                 /* process pending bytes on uart */
 585                 cmdline_process();
 586
 587                 /* monitor the value of ICR (which is modified
 588                  * automatically on TT rising edge). If the value
 589                  * changed, process the time difference. */
 590                 if (ETIFR & _BV(ICF3)) {
 591                         cli();
 592                         icr = ICR3;
 593                         sei();
 594                         ETIFR = _BV(ICF3);
 595
 596                         //LED2_TOGGLE();
 597                         diff_icr = (icr - prev_icr);
 598                         cpt_icr = cpt;
 599                         prev_icr = icr;
 600                         cpt = 0;
 601                         speed = get_speed(cpt_icr, diff_icr);
 602                 }
 603
 604                 /* read time reference */
 605                 cli();
 606                 tcnt3 = TCNT3;
 607                 sei();
 608
 609                 /* wait cs period */
 610                 if (tcnt3 - prev_cs < CS_PERIOD)
 611                         continue;
 612
 613                 /* CS LED */
 614                 if (x & 0x80)
 615                         LED1_ON();
 616                 else
 617                         LED1_OFF();
 618                 x++;
 619
 620                 /* process CS... maybe we don't need to use
 621                  * control_system_manager, just PID is enough */
 622                 if (cpt == CPT_ICR_MAX)
 623                         speed = 0;
 624                 else
 625                         speed = get_speed(cpt_icr, diff_icr);
 626
 627                 /* enabled laser when rotation speed if at least 5tr/s */
 628                 if (speed > 5000)
 629                         LASER_ON();
 630                 else
 631                         LASER_OFF();
 632
 633                 err = CS_CONSIGN - speed;
 634                 out = pid_do_filter(&beacon_tsop.pid, err);
 635                 if (x == 0 && beacon_tsop.debug_speed)
 636                         printf("%ld %ld\n", speed, out);
 637                 if (out < 0)
 638                         out = 0;
 639                 /* XXX */
 640                 if (out > 2000)
 641                         out = 2000;
 642
 643                 pwm_ng_set(&beacon_tsop.pwm_motor, out);
 644
 645                 prev_cs = tcnt3;
 646
 647                 /* count the number of CS period between 2 ICR
 648                  * captures */
 649                 if (cpt < CPT_ICR_MAX)
 650                         cpt ++;
 651
 652                 process_sta_ring(&static_beacon);
 653                 process_opp_ring(&opp_beacon);
 654                 cli();
 655                 tick ++; /* global imprecise time reference */
 656                 sei();
 657         }
 658
 659         return 0;
 660 }