servo_measure.c

#include <util/delay.h>
#include <avr/sleep.h>
#include <avr/power.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdio.h>
#include "port_macros.h"
#include "teensy_20.h"
#include "cpu_clock.h"
#include "usb_config.h"
#include "usb_serial.h"

volatile int count = 0;

ISR (TIMER3_CAPT_vect) {
	if (READ_REG(TCCR3B, ICES3)) {

		// captured rising edge
		// reset counter to measure
		TCNT3 = 0;
		// set to capture falling edge
		CLEAR_REG(TCCR3B, ICES3);
	} else {
		// captured falling edge
		// value is units of 1 microsecond
		count = ICR3;
		// set to capture rising edge
		SET_REG(TCCR3B, ICES3);
	}
}

int main() {
	CLEAR(LED);
	CPU_PRESCALE(CLK_DIV2); // 8Mhz
	usb_init();
	for (;;) {
		if (usb_configured()) {
			break;
		}
		cli();
		sleep_enable();
		sei();
		sleep_cpu();
		sleep_disable();
	}
	SET(LED);

	// Timer/Counter 3B:
	// enable input capture noise canceler
	// waveform generation mode: Normal, count up, top = 0xFFFF
	// capture on rising edge
	// clock select: clk/8
	TCCR3B = _BV(ICES3) | _BV(ICNC3) | _BV(CS31);

	// Enable input capture interrupts
	SET_REG(TIMSK3, ICIE3);

	set_sleep_mode(SLEEP_MODE_IDLE);
	char buffer[80];
	for (;;) {
		// sleep until count > 0
		cli();
		if (count == 0) {
			sleep_enable();
			sei();
			sleep_cpu();
			sleep_disable();
			continue;
		}
		snprintf(buffer, sizeof(buffer), "%-4d\r", count);
		usb_serial_write((uint8_t *)&buffer, strlen(buffer));
		count = 0;
	}
}