2 #include <avr/interrupt.h>
3 #include <util/delay.h>
11 #define N_TRIG_SENSORS 4
13 // static int16_t distances[N_SENSORS];
15 // hold_regs[0] is unit ID
16 #define thresholds (hold_regs+1)
17 #define led1_sensors (hold_regs[13])
18 #define led2_sensors (hold_regs[14])
20 #define led_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM])
21 #define distances (hold_regs+MB_N_HOLD_REGS_EEPROM+1)
22 #define free_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM+13])
23 #define err_bitmap (hold_regs[MB_N_HOLD_REGS_EEPROM+14])
25 static void pull_trigger(uint8_t trig)
28 case 0: PORTD |= _BV(PD7); _delay_us(10); PORTD &= ~_BV(PD7); break;
29 case 1: PORTB |= _BV(PB4); _delay_us(10); PORTB &= ~_BV(PB4); break;
30 case 2: PORTC |= _BV(PC4); _delay_us(10); PORTC &= ~_BV(PC4); break;
34 static uint16_t get_pin(uint8_t trig)
37 case 0: return (PIND & 0x78) >> 3;
38 case 1: return PINB & 0x0F;
39 default: return PINC & 0x0F;
43 static void do_measurement(unsigned char trig)
45 uint16_t starttimes[N_TRIG_SENSORS], starttime;
46 uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
47 uint8_t to_measure = 0, i;
53 while (to_start || to_measure) {
57 if (now-starttime >= TIMEOUT)
62 for (i = 0; i < N_TRIG_SENSORS; i++) {
63 uint8_t mask = 1 << i;
65 if ((to_start & mask) && (bits & mask)) {
70 } else if ((to_measure & mask) && !(bits & mask)) {
73 distances[trig*N_TRIG_SENSORS + i]
74 = now - starttimes[i];
79 for (i = 0; i < N_TRIG_SENSORS; i++)
80 if (to_start & (1 << i))
81 distances[trig*N_TRIG_SENSORS + i] = -1;
82 else if (to_measure & (1 << i))
83 distances[trig*N_TRIG_SENSORS + i] = 0;
86 static void do_measurements()
90 for (trig = 0; trig < N_TRIGGERS; trig++) {
96 static void led_set(uint8_t led, uint8_t state)
101 // PORTC |= _BV(PC5);
105 // PORTC &= ~_BV(PC5);
119 static void eval_bitmaps()
121 uint16_t free_b = 0, err_b = 0, mask;
124 for (i = 0; i < N_SENSORS; i++) {
128 if (distances[i] == -1 || distances[i] == 0) {
130 } else if (distances[i] > thresholds[i]) {
136 free_bitmap = free_b;
140 if (led1_sensors & free_bitmap) {
148 if (led2_sensors & free_bitmap) {
156 uint8_t hold_reg_is_valid(uint16_t reg, uint16_t val)
158 if (reg == MB_HOLD_REGS_BASE)
159 return val > 0 && val <= 247;
169 DDRD |= _BV(PD7); // Trig D
170 DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2
171 DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1
177 TCCR1B = _BV(CS12)|_BV(CS10); // CLK/1024
184 for (trig = 0; trig < N_TRIGGERS; trig++) {
186 do_measurement(trig);
188 while (TCNT1-now < 200)
194 // distances[4] > 100 || distances[11] > 100);