+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include <stdio.h>
+#include "rs485.h"
+
+#define TIMEOUT 0x2FF
+
+#define N_TRIGGERS 3
+#define N_SENSORS 12
+#define N_TRIG_SENSORS 4
+
+static int16_t distances[N_SENSORS];
+
+static void pull_trigger(uint8_t trig)
+{
+ switch (trig) {
+ case 0: PORTD |= _BV(PD7); _delay_us(10); PORTD &= ~_BV(PD7); break;
+ case 1: PORTB |= _BV(PB4); _delay_us(10); PORTB &= ~_BV(PB4); break;
+ case 2: PORTC |= _BV(PC4); _delay_us(10); PORTC &= ~_BV(PC4); break;
+ }
+}
+
+static uint16_t get_pin(uint8_t trig)
+{
+ switch (trig) {
+ case 0: return (PIND & 0x78) >> 3;
+ case 1: return PINB & 0x0F;
+ default: return PINC & 0x0F;
+ }
+}
+
+static void do_measurement(unsigned char trig)
+{
+ uint16_t starttimes[N_TRIG_SENSORS], starttime;
+ uint8_t to_start = (1 << N_TRIG_SENSORS) - 1;
+ uint8_t to_measure = 0, i;
+
+ pull_trigger(trig);
+
+ starttime = TCNT1;
+
+ while (to_start || to_measure) {
+ uint8_t bits = 0;
+ uint16_t now = TCNT1;
+
+ if (now-starttime >= TIMEOUT)
+ break;
+
+ bits = get_pin(trig);
+
+ for (i = 0; i < N_TRIG_SENSORS; i++) {
+ uint8_t mask = 1 << i;
+
+ if ((to_start & mask) && (bits & mask)) {
+ // echo start
+ starttimes[i] = now;
+ to_start &= ~mask;
+ to_measure |= mask;
+ } else if ((to_measure & mask) && !(bits & mask)) {
+ // echo end
+ to_measure &= ~mask;
+ distances[trig*N_TRIG_SENSORS + i]
+ = now - starttimes[i];
+ }
+ }
+ }
+
+ for (i = 0; i < N_TRIG_SENSORS; i++)
+ if (to_start & (1 << i))
+ distances[trig*N_TRIG_SENSORS + i] = -1;
+ else if (to_measure & (1 << i))
+ distances[trig*N_TRIG_SENSORS + i] = 0;
+}
+
+static void do_measurements()
+{
+ uint8_t trig;
+
+ for (trig = 0; trig < N_TRIGGERS; trig++) {
+ do_measurement(trig);
+ _delay_ms(200);
+ }
+}
+
+static void led_set(uint8_t led, uint8_t state)
+{
+ if (led == 0) {
+ if (state) {
+ PORTD |= _BV(PD4);
+ // PORTC |= _BV(PC5);
+ } else {
+ PORTD &= ~_BV(PD4);
+ // PORTC &= ~_BV(PC5);
+ }
+ } else {
+ if (state) {
+ PORTB |= _BV(PB5);
+ } else {
+ PORTB &= ~_BV(PB5);
+ }
+ }
+}
+
+int main()
+{
+ char obuf[120];
+
+ rs485_init();
+
+ // output pins
+ DDRD |= _BV(PD7); // Trig D
+ DDRB |= _BV(PB4) | _BV(PB5); // Trig B, LED 2
+ DDRC |= _BV(PC4) | _BV(PC5); // Trig C, LED 1
+ // temporary LED
+ DDRD |= _BV(PD4);
+
+ // set up the timer
+ TCCR1A = 0;
+ TCCR1B = _BV(CS12)|_BV(CS10); // CLK/1024
+
+ // enable interrupts
+ sei();
+
+ while(1) {
+ do_measurements();
+
+ sprintf(obuf, "%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d\r\n",
+ distances[0], distances[1], distances[2],
+ distances[3], distances[4], distances[5],
+ distances[6], distances[7], distances[8],
+ distances[9], distances[10], distances[11]);
+
+ rs485_send(obuf);
+ led_set(0,
+ distances[4] > 100 || distances[11] > 100);
+ }
+}
+