added sonar and fixed _goto in uart ansi

lib/pins.h

@@ -22,31 +22,37 @@
 //
 //  - The macros io2XXX() can be used to get literal name of register associated with the pin.
 //  - io2n() provides pin number.
-//  - The XXX_aux() macros are internal and should not be used elsewhere.
+//  - The underscored and _aux macros are internal and should not be used elsewhere.
 //  - The io_pack() macro is used to pass pin (io) to other macro without expanding it.
 //
 
 #include <avr/io.h>
 #include "calc.h"
 
-
+// Helpers
 // Get particular register associated with the name X (eg. D -> PORTD)
-#define reg_ddr(X) DDR ## X
+#define _reg_ddr(X) DDR ## X
-#define reg_port(X) PORT ## X
+#define _reg_port(X) PORT ## X
-#define reg_pin(X) PIN ## X
+#define _reg_pin(X) PIN ## X
-
+#define _io2ddr_aux(reg, bit) _reg_ddr(reg)
-#define io2ddr_aux(reg, bit) reg_ddr(reg)
+#define _io2port_aux(reg, bit) _reg_port(reg)
-#define io2ddr(io) io2ddr_aux(io)
+#define _io2pin_aux(reg, bit) _reg_pin(reg)
-#define io2port_aux(reg, bit) reg_port(reg)
+#define _io2n_aux(reg, bit) bit
-#define io2port(io) io2port_aux(io)
+
-#define io2pin_aux(reg, bit) reg_pin(reg)
+
-#define io2pin(io) io2pin_aux(io)
+// === Convert A,1 to corresponding register and pin number ===
-#define io2n_aux(reg, bit) bit
+
-#define io2n(io) io2n_aux(io)
+#define io2ddr(io) _io2ddr_aux(io)
+#define io2port(io) _io2port_aux(io)
+#define io2pin(io) _io2pin_aux(io)
+#define io2n(io) _io2n_aux(io)
 
+// === covert "A", "1" to "A,1" for passing on to another macro ===
 #define io_pack(port, bit) port, bit
 
 
+// === Useful types for ports and pins ===
+
 // pointer to port
 typedef volatile uint8_t* PORT_P;
 // number of bit in port
@@ -54,54 +60,70 @@ typedef uint8_t BIT_N;
 
 
 // === pin manipulation ===
-#define set_pin_aux(port, bit) sbi(reg_port(port), (bit))
-#define clear_pin_aux(port, bit) cbi(reg_port(port), (bit))
-#define read_pin_aux(port, bit) get_bit(reg_pin(port), (bit))
-#define write_pin_aux(port, bit, value) set_bit(reg_port(port), (bit), (value))
-#define toggle_pin_aux(port, bit) sbi(reg_pin(port), (bit))
 
+// Helpers
+#define _set_pin_aux(port, bit) sbi(_reg_port(port), (bit))
+#define _clear_pin_aux(port, bit) cbi(_reg_port(port), (bit))
+#define _read_pin_aux(port, bit) get_bit(_reg_pin(port), (bit))
+#define _write_pin_aux(port, bit, value) set_bit(_reg_port(port), (bit), (value))
+#define _toggle_pin_aux(port, bit) sbi(_reg_pin(port), (bit))
+
+
+// Set pin to HIGH
+#define pin_up(io) _set_pin_aux(io)
+#define pin_high(io) _set_pin_aux(io)
 
-#define pin_up(io) set_pin_aux(io)
+// Set pin to LOW
-#define pin_high(io) set_pin_aux(io)
+#define pin_down(io) _clear_pin_aux(io)
+#define pin_low(io) _clear_pin_aux(io)
 
-#define pin_down(io) clear_pin_aux(io)
+// Get input pin value
-#define pin_low(io) clear_pin_aux(io)
+#define get_pin(io) _read_pin_aux(io)
+#define read_pin(io) _read_pin_aux(io)
 
-#define get_pin(io) read_pin_aux(io)
+// Check if pin is low or high
-#define read_pin(io) read_pin_aux(io)
+#define pin_is_low(io) !_read_pin_aux(io)
+#define pin_is_high(io) _read_pin_aux(io)
 
-#define pin_is_low(io) !read_pin_aux(io)
+// Write a value to pin
-#define pin_is_high(io) read_pin_aux(io)
+#define set_pin(io, value) _write_pin_aux(io, (value))
+#define write_pin(io, value) _write_pin_aux(io, (value))
+#define toggle_pin(io) _toggle_pin_aux(io)
 
-#define set_pin(io, value) write_pin_aux(io, (value))
-#define write_pin(io, value) write_pin_aux(io, (value))
-#define toggle_pin(io) toggle_pin_aux(io)
 
+// === Setting pin direction ===
 
+// Helpers
+#define _as_input_aux(port, bit) cbi(_reg_ddr(port), (bit))
+#define _as_output_aux(port, bit) sbi(_reg_ddr(port), (bit))
+#define _set_dir_aux(port, bit, dir) write_bit(_reg_ddr(port), (bit), (dir))
 
-// setting pin direction
-#define as_input_aux(port, bit) cbi(reg_ddr(port), (bit))
-#define as_output_aux(port, bit) sbi(reg_ddr(port), (bit))
-#define set_dir_aux(port, bit, dir) write_bit(reg_ddr(port), (bit), (dir))
 
+// Pin as input (_pu ... with pull-up)
+#define as_input(io) _as_input_aux(io)
+#define as_input_pu(io) do { _as_input_aux(io); _pullup_enable_aux(io); } while(0)
 
-#define as_input(io) as_input_aux(io)
+// Pin as output
-#define as_input_pu(io) do { as_input_aux(io); pullup_enable_aux(io); } while(0)
+#define as_output(io) _as_output_aux(io)
 
-#define as_output(io) as_output_aux(io)
+// Set direction (1 ... output)
-#define set_dir(io, dir) set_dir_aux(io, (dir))
+#define set_dir(io, dir) _set_dir_aux(io, (dir))
 
 
-// setting pullup
+// === Setting pullup ===
-#define pullup_enable_aux(port, bit) sbi(reg_port(port), (bit))
-#define pullup_disable_aux(port, bit) cbi(reg_port(port), (bit))
-#define set_pullup_aux(port, bit, on) write_bit(reg_port(port), (bit), (on))
 
+// Helpers
+#define _pullup_enable_aux(port, bit) sbi(_reg_port(port), (bit))
+#define _pullup_disable_aux(port, bit) cbi(_reg_port(port), (bit))
+#define _set_pullup_aux(port, bit, on) write_bit(_reg_port(port), (bit), (on))
 
-#define pullup_enable(io) pullup_enable_aux(io)
+// Enable pullup
-#define pullup_on(io) pullup_enable_aux(io)
+#define pullup_enable(io) _pullup_enable_aux(io)
+#define pullup_on(io) _pullup_enable_aux(io)
 
-#define pullup_disable(io) pullup_disable_aux(io)
+// Disable pullup
-#define pullup_off(io) pullup_disable_aux(io)
+#define pullup_disable(io) _pullup_disable_aux(io)
+#define pullup_off(io) _pullup_disable_aux(io)
 
-#define set_pullup(io, on) set_pullup_aux(io, on)
+// Set pullup to value (1 ... pullup enabled)
+#define set_pullup(io, on) _set_pullup_aux(io, on)

lib/sonar.c

@@ -0,0 +1,169 @@
+#include <avr/io.h>
+#include <util/delay.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "pins.h"
+#include "sonar.h"
+
+// Currently measured sonar
+volatile sonar_t *_sonar_active_so;
+
+// Flag that measurement is in progress
+volatile bool _busy;
+
+// Result of last measurement, in millimeters
+volatile int16_t _result = 0xFFFF;
+
+
+void _sonar_init_do(sonar_t* so, PORT_P port, uint8_t ntx, PORT_P pin, uint8_t nrx)
+{
+	so->port = port;
+	so->ntx = ntx;
+	so->pin = pin;
+	so->nrx = nrx;
+
+	switch((const uint16_t) pin) {
+		case (const uint16_t)&PINB:
+			so->bank = 0;
+			break;
+		case (const uint16_t)&PINC:
+			so->bank = 1;
+			break;
+		case (const uint16_t)&PIND:
+			so->bank = 2;
+			break;
+	}
+}
+
+
+/**
+ * Start sonar measurement
+ * Interrupts must be enabled
+ * TIMER 1 will be used for the async measurement
+ * Timer 1 overflow and Pin Change interrupts must invoke Sonar handlers.
+ */
+bool sonar_start(sonar_t* so)
+{
+	if (_busy) return false;
+
+	_sonar_active_so = so;
+
+	_busy = true;
+
+	// make sure the timer is stopped (set clock to NONE)
+	TCCR1B = 0;
+
+	// Timer overflow interrupt enable
+	// We'll stop measuring on overflow
+	TIMSK1 |= (1 << TOIE1);
+
+	// Clear the timer value
+	TCNT1 = 0;
+
+	// Set up pin change interrupt mask for the RX pin
+	switch(so->bank) {
+		case 0:
+			PCMSK0 |= (1 << (so->nrx));
+			break;
+		case 1:
+			PCMSK1 |= (1 << (so->nrx));
+			break;
+		case 2:
+			PCMSK2 |= (1 << (so->nrx));
+			break;
+	}
+
+	// send positive pulse
+	*(so->port) |= (1 << so->ntx);
+	_delay_us(_SNR_TRIG_TIME);
+	*(so->port) &= ~(1 << so->ntx);
+
+	// Wait for start of response
+	while ( (*(so->pin) & (1 << so->nrx)) == 0 );
+
+	// Set timer clock source: F_CPU / 8 (0.5 us resolution)
+	TCCR1B = (0b010 << CS10);
+
+	// Enable pin change interrupt
+	PCICR |= (1 << (so->bank));
+
+	return true;
+}
+
+
+/** Stop the timer */
+void _sonar_stop()
+{
+	// stop timer
+	TCCR1B = 0;
+
+	// Disable RX pin interrupt mask
+	switch(_sonar_active_so->bank) {
+		case 0:
+			PCMSK0 &= ~(1 << (_sonar_active_so->nrx));
+			break;
+		case 1:
+			PCMSK1 &= ~(1 << (_sonar_active_so->nrx));
+			break;
+		case 2:
+			PCMSK2 &= ~(1 << (_sonar_active_so->nrx));
+			break;
+	}
+
+	// Disable timer1 overflow interrupt
+	TIMSK1 &= ~(1 << TOIE1);
+
+	_busy = false;
+}
+
+
+/** Handle TIMER1_OVF (returns true if consumed) */
+bool sonar_handle_t1ovf()
+{
+	if (!_busy) return false; // nothing
+
+	_result = -1;
+	_sonar_stop();
+
+	return true;
+}
+
+
+/** Handle pin change interrupt (returns true if consumed) */
+bool sonar_handle_pci()
+{
+	if (!_busy) {
+		return false; // nothing
+	}
+
+	if (*(_sonar_active_so->pin) & (1 << _sonar_active_so->nrx)) {
+		// rx is high, not our pin change event
+		return false;
+	}
+
+	uint64_t x = TCNT1;
+	x /= _SNR_DIV_CONST;
+	x *= 100000000L;
+	x /= F_CPU;
+	_result = (int16_t) x;
+
+	// no obstacle
+	if (_result > _SNR_MAX_DIST) _result = -1;
+
+	_sonar_stop();
+
+	return true;
+}
+
+
+bool sonar_busy()
+{
+	return _busy;
+}
+
+
+int16_t sonar_result()
+{
+	return _result;
+}

lib/sonar.h

@@ -0,0 +1,70 @@
+#pragma once
+
+//
+// Utilities for working with the HC-SR04 ultrasonic sensor
+// Can be easily modified to work with other similar modules
+//
+// It's required that you call the sonar_handle_* functions from your ISRs
+// See example program for more info.
+//
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "lib/pins.h"
+
+// Calib constant for the module
+// CM = uS / _DIV_CONST
+#define _SNR_DIV_CONST 58
+
+// Max module distance in MM
+#define _SNR_MAX_DIST 4000
+
+// Trigger time in uS
+#define _SNR_TRIG_TIME 10
+
+
+// Sonar data object
+typedef struct {
+	PORT_P port; // Tx PORT
+	uint8_t ntx; // Tx bit number
+	PORT_P pin;  // Rx PIN
+	uint8_t nrx; // Rx bit number
+	uint8_t bank; // Rx PCINT bank
+} sonar_t;
+
+
+// Create a Sonar port
+// Args: sonar_t* so, Trig pin, Echo pin
+#define sonar_init(so, trig, echo) do { \
+	as_output(io_pack(trig)); \
+	as_input_pu(io_pack(echo)); \
+	_sonar_init_do(so, &io2port(io_pack(trig)), io2n(io_pack(trig)), &io2pin(io_pack(echo)), io2n(io_pack(echo))); \
+} while(0)
+
+// private, in header because of the macro.
+void _sonar_init_do(sonar_t* so, PORT_P port, uint8_t ntx, PORT_P pin, uint8_t nrx);
+
+
+/** Check if sonar is busy */
+bool sonar_busy();
+
+
+/** Get result of last measurement, in millimeters. Returns -1 if no obstacle detected */
+int16_t sonar_result();
+
+
+/**
+ * Start sonar measurement
+ * Interrupts must be enabled
+ * TIMER 1 will be used for the async measurement
+ */
+bool sonar_start(sonar_t* so);
+
+
+/** Handle TIMER1_OVF (returns true if consumed) */
+bool sonar_handle_t1ovf();
+
+
+/** Handle pin change interrupt (returns true if consumed) */
+bool sonar_handle_pci();

lib/uart_ansi.c

@@ -17,9 +17,9 @@ void vt_goto(uint8_t x, uint8_t y)
 {
 	uart_putc(27);
 	uart_putc('[');
-	uart_putu(x);
+	uart_putu(y+1); // one-based !
 	uart_putc(';');
-	uart_putu(y);
+	uart_putu(x+1);
 	uart_putc('H');
 }
 
@@ -28,7 +28,7 @@ void vt_goto_x(uint8_t x)
 {
 	uart_putc(27);
 	uart_putc('[');
-	uart_putu(x);
+	uart_putu(x+1);
 	uart_putc('`');
 }
 
@@ -37,7 +37,7 @@ void vt_goto_y(uint8_t y)
 {
 	uart_putc(27);
 	uart_putc('[');
-	uart_putu(y);
+	uart_putu(y+1);
 	uart_putc('d');
 }