X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fbeacon_tsop%2Fmain.c;fp=projects%2Fmicrob2010%2Ftests%2Fbeacon_tsop%2Fmain.c;h=ce287ccacca815f7538377d434b9c5e6b94804c1;hp=0000000000000000000000000000000000000000;hb=dccae820e0f5c68a3a9c496b63eb8d02102d6ebc;hpb=6d8949fd4fc3a1e65ff060cfbdb5e405efe15c6c diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c new file mode 100755 index 0000000..ce287cc --- /dev/null +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -0,0 +1,386 @@ +/* + * Copyright Droids Corporation (2009) + * Olivier Matz + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include "cmdline.h" +#include "main.h" + +/******************* TSOP */ + +#define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ +#ifdef BOARD2006 +#define INTx_TSOP INT6 +#define ISCx0_TSOP ISC60 +#define ISCx1_TSOP ISC61 +#define SIG_TSOP SIG_INTERRUPT6 +#define TSOP_READ() (PINE & 0x40) +#else +#define INTx_TSOP INT4 +#define ISCx0_TSOP ISC40 +#define ISCx1_TSOP ISC41 +#define SIG_TSOP SIG_INTERRUPT4 +#define TSOP_READ() (PINE & 0x10) +#endif + +#define TSOP_FREQ_MHZ 0.455 +#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ) +#define N_PERIODS 10. + +#define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US) +#define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US) + +#define TSOP_TIME_SHORT ((uint16_t)(TSOP_TIME_SHORT_US*2)) +#define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2)) + +#define FRAME_LEN 16 + +/* frame */ +static uint16_t start_angle_time; +static uint16_t frame; +static uint16_t mask; +static uint8_t len; +static uint8_t val; + +struct detected_frame { + uint16_t frame; + uint16_t time; +}; + +#define FRAME_RING_ORDER 4 +#define FRAME_RING_SIZE (1< TSOP_TIME_LONG) { + len = 1; + val = 1; + frame = 0; + start_angle_time = cur_time - ref_time; + mask = 1; + } + /* any short edge */ + else if (diff_time < TSOP_TIME_SHORT) { + if (len & 1) { + if (val) + frame |= mask; + mask <<= 1; + } + len ++; + } + /* any long edge */ + else if (diff_time < TSOP_TIME_LONG) { + val = !val; + if (val) + frame |= mask; + mask <<= 1; + len += 2; + } + + /* end of frame */ + if (len == FRAME_LEN*2) { + uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK; + if (tail_next != frame_ring_head) { + frame_ring[frame_ring_tail].frame = frame; + frame_ring[frame_ring_tail].time = start_angle_time; + frame_ring_tail = tail_next; + } + if (led_cpt & 0x8) + LED3_TOGGLE(); + led_cpt ++; + } + + prev_time = cur_time; + prev_tsop = cur_tsop; +} + +/* absolute value */ +static inline int32_t AbS(int32_t x) +{ + if (x > 0) + return x; + else + return -x; +} + +/* Get the speed of motor (tr / 1000s) + * - icr_cpt is the number of CS period between 2 ICR updates + * - icr_diff is the difference of ICR values between the ICR updates + * (modulo 65536 obviously) */ +static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) +{ + int32_t best_diff = 65536L; + int8_t best_cpt = -2; + int32_t diff; + int8_t i; + + /* too slow (less than 1 tr/s) */ + if (icr_cpt > CPT_ICR_MAX) + return 1000L; + + /* too fast (more than 100 tr/s) */ + if (icr_cpt < CPT_ICR_MIN) + return 100000L; + + /* try to get the real time knowning icr_cpt and icr_diff */ + for (i=-1; i<2; i++) { + diff = ((icr_cpt+i)&3) * 16384L; + diff += (icr_diff & 0x3fff); + diff -= icr_diff; + if (diff > 32768L) + diff -= 65536L; + if (diff < -32768) + diff += 65536L; + + if (AbS(diff) < AbS(best_diff)) { + best_diff = diff; + best_cpt = icr_cpt + i; + } + } + + /* real time difference in 1/2 us */ + diff = (best_cpt * 16384L) + (icr_diff & 0x3fff); + return 2000000000L/diff; +} + +int main(void) +{ + uint16_t prev_cs = 0; + uint16_t prev_icr = 0; + uint16_t icr = 0; + uint16_t diff_icr = 0; + uint8_t cpt_icr = 0; + uint8_t cpt = 0; + int32_t speed, out, err; + uint16_t tcnt3; + uint8_t x = 0; + + /* LEDS */ + LED1_DDR |= _BV(LED1_BIT); + LED2_DDR |= _BV(LED2_BIT); + LED3_DDR |= _BV(LED3_BIT); + DDRB |= 0x10; /* OC0 (laser pwm) */ + + /* PID init */ + pid_init(&beacon_tsop.pid); + pid_set_gains(&beacon_tsop.pid, 500, 0, 0); + pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095); + pid_set_out_shift(&beacon_tsop.pid, 10); + pid_set_derivate_filter(&beacon_tsop.pid, 4); + + uart_init(); + fdevopen(uart0_dev_send, uart0_dev_recv); + + rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer); + snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > "); + rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt); + + debug_tsop(); + debug_serial(); + + /* configure external interrupt for TSOP */ + EICRx_TSOP |= _BV(ISCx0_TSOP); + EIMSK |= _BV(INTx_TSOP); + + /* pwm for motor */ + PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, + TIMER1_PRESCALER_DIV_1); + PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0); + + /* pwm for laser: + * - clear on timer compare (CTC) + * - Toggle OC0 on compare match + * - prescaler = 1 */ + TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); + OCR0 = 80; /* f = 100 khz at 16 Mhz */ + + /* configure timer 3: CLK/8 + * it is used as a reference time + * enable noise canceller for ICP3 */ + TCCR3B = _BV(ICNC3) | _BV(CS11); + + sei(); + + /* Control system will be done in main loop */ + while (1) { + + /* process pending bytes on uart */ + cmdline_process(); + + /* monitor the value of ICR (which is modified + * automatically on TT rising edge). If the value + * changed, process the time difference. */ + if (ETIFR & _BV(ICF3)) { + cli(); + icr = ICR3; + sei(); + ETIFR = _BV(ICF3); + + LED2_TOGGLE(); + diff_icr = (icr - prev_icr); + cpt_icr = cpt; + prev_icr = icr; + cpt = 0; + speed = get_speed(cpt_icr, diff_icr); + } + + /* read time reference */ + cli(); + tcnt3 = TCNT3; + sei(); + + /* wait cs period */ + if (tcnt3 - prev_cs < CS_PERIOD) + continue; + + /* CS LED */ + if (x & 0x80) + LED1_ON(); + else + LED1_OFF(); + x++; + + /* process CS... maybe we don't need to use + * control_system_manager, just PID is enough */ + if (cpt == CPT_ICR_MAX) + speed = 0; + else + speed = get_speed(cpt_icr, diff_icr); + + /* enabled laser when rotation speed if at least 5tr/s */ + if (speed > 5000) + LASER_ON(); + else + LASER_OFF(); + + err = CS_CONSIGN - speed; + out = pid_do_filter(&beacon_tsop.pid, err); + if (x == 0 && beacon_tsop.debug_speed) + printf("%ld %ld\n", speed, out); + if (out < 0) + out = 0; + /* XXX */ + if (out > 2000) + out = 2000; + + pwm_ng_set(&beacon_tsop.pwm_motor, out); + + prev_cs = tcnt3; + + /* count the number of CS period between 2 ICR + * captures */ + if (cpt < CPT_ICR_MAX) + cpt ++; + + /* after CS, check if we have a new frame in ring */ + if (frame_ring_head != frame_ring_tail) { + uint8_t head_next; + head_next = (frame_ring_head+1) & FRAME_RING_MASK; + if (beacon_tsop.debug_frame) + printf("%x %d\n", frame_ring[frame_ring_head].frame, + frame_ring[frame_ring_head].time); + frame_ring_head = head_next; + } + + } + + return 0; +}