/*
** Test code for Orangutan PWM/Motor driver function. Uses "WheelWatcher" to measure rotational
** period of right wheel only. The Wheelwatcher tick is wired to INT0 (shared by LCD display--LCD display pin 4), 
** while the wheelwatcher rotational direction output is wired to PORTC.5
**
** Output on LCD Display: 
** Line 1: Measured tick period in units of 0.256 ms
** Line 2: Corresponding PWM setting/255 on right wheel
** copyright (c) 2006 by S. James Remington (sjames_remington _at_ yahoo.com
*/

#define F_CPU 1000000UL  // 1 MHz  for delays in AVRLIB
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>  //itoa, abs
#define 	sei()   __asm__ __volatile__ ("sei" ::)
#define 	cli()   __asm__ __volatile__ ("cli" ::)

/*
** Delays
*/

#define	Delay( x )	_delay_loop_2( x )
#define Wait2 for(i=1;i<20;i++) Delay(25000); //2 sec
#define Wait5 for(i=1;i<50;i++) Delay(25000); //5 s

//globals

int r_speed,l_speed,tick_R,tmr_R_prev,enc_per_R;
signed long enc_pos_R,enc_per_R_sum;
unsigned char TCNT0H,TCNT0T;

/* TIMER0 interrupt handler
Timer0 clock is set to 3.9 KHz, tick every 0.256 ms  (1 MHz CPU clock)
This routine accumulates time in 16 bits, so the high order byte increments every 16.7 sec
*/

SIGNAL(SIG_OVERFLOW0) /* handler for tcnt0 overflow interrupt */
{
	TCNT0H++; //increments every 65 ms
	if (TCNT0H==0) TCNT0T++; //increments every 16.7 sec
}

/*
 INTERRUPT0 ISR
This interrupt service routine is called on each pulse of the
right WW-01 CLK line.  We grab the current time stamp, adjust
the current position based on the RDIR pin, then calculate a
new encoder clock period using the new time stamp. 
*/
SIGNAL (SIG_INTERRUPT0)
{
	int tmr;

	tmr = (TCNT0H<<8) + TCNT0;
	if (PINC & _BV(5)) enc_pos_R++;
	else enc_pos_R--;
	tick_R++;
	enc_per_R = (tmr - tmr_R_prev);
	enc_per_R_sum += enc_per_R;
	tmr_R_prev = tmr;
}

#include "lcd_or.c"
#include "pwm_brake.c"

/*
** MAIN
*/
int main(void)
{
	unsigned char buf[9];  //display buffer
	int i,j;

// Timer/Counter 0 initialization
// Clock source: System Clock/256
// Clock value: 3.9 kHz. Each tick=0.256 ms (1 MHz CPU clock)
	TCCR0=0x04;
	TIMSK |= 0x01;  //timer overflow interrupt TOIE0 enable

// 	enable external interrupts 0 and 1 for falling edge triggering
//	MCUCR = _BV(ISC11) | _BV(ISC01);
//	GICR = _BV(INT1) | _BV(INT0);
//	now use only INT0

	MCUCR |= _BV(ISC01);
	GICR |= _BV(INT0);
	DDRD &= ~_BV(2); //INT0=input
	PORTD &=~_BV(2);

	pwm_init();

// loop through a range of values and measure wheel rotational period

	for (j=35;j<256;j+=20)
	{

// Timer(s)/Counter(s) Interrupt(s) initialization
	TCNT0=TCNT0H=0;
	enc_per_R = 0;
	enc_per_R_sum = 0;
   	tmr_R_prev = 0;
	tick_R = 0;
	DDRD &= ~_BV(2); //INT0=input (PORTD.2 is shared as output to LCD display)
	PORTD &=~_BV(2);

	sei();  //enable interrupts

// let wheel run for 5 seconds and accumulate average period
	l_speed=0;
	r_speed=j;
	pwm_set();
	Wait5;
	r_speed=l_speed=0;  //stop
	pwm_set();
	cli();  //disable interrupts so that INT0 doesn't mess up LCD

//encode current wheel rotational period and display on LCD

	LCDInit();  	//initialize LCD
	enc_per_R=enc_per_R_sum/tick_R;
	itoa(enc_per_R,buf,10);  //use integer to ASCII function from AVRlib to encode for display
	LCDString(buf);

//encode PWM setting and display on line 2
	itoa(j,buf,10);
	LCDCommand(LCD_Line2);
	LCDString(buf);
	Wait5;  Wait5; //wait for me to write down result
	}
	while(1);
	
	return 0;
}