/*
** PID motor control code for Orangutan PWM/Motor driver. 
** Uses "WheelWatcher" from nubotics.com to measure rotational periods of wheels. 
** The Wheelwatcher tick outputs are wired to INT0/1 (shared by LCD display--LCD display pins 4,5), 
** while the wheelwatcher rotational direction outputs are wired to PORTC.5,4
** CPU clock speed is 8 MHz
**
** This is a differential PD algorithm:
** err=requested_speed-current_speed
** delta(PWM) = Kp*err + Kd*(err-err_last)
** The current speed is calculated using a low pass filter, so this is a "double differential" algorithm
** i.e. current_speed = 1/current_period 
** and where current_period = 0.25*measured_period + 0.75*last_current_period
**
** S. James Remington (sjames_remington _at_ yahoo.com  (2006)
*/

#define F_CPU 8000000UL  // CPU clock speed for delays in AVRLIB
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <stdlib.h>

#define 	sei()   __asm__ __volatile__ ("sei" ::)
#define 	cli()   __asm__ __volatile__ ("cli" ::)


// function prototypes

extern unsigned int get_timer_ticks(void); 	//fast ticks (0.128 ms)
extern unsigned int get_timer(void);  		//slow ticks (32.7 ms)
extern int uart_putchar(char c, FILE *stream);

/*
** Delays
*/

#define	Delay( x )	_delay_loop_2( x )
#define Wait2 {int iw; for(iw=1;iw<160;iw++) Delay(25000);} //2 sec
#define Wait5 {int iw; for(iw=1;iw<400;iw++) Delay(25000);} //5 s


//global variables

int tick_R,pwm_R,dir_R,enc_per_R,req_spd_R,spd_R;
int tick_L,pwm_L,dir_L,enc_per_L,req_spd_L,spd_L;
signed long enc_pos_R,enc_pos_L;
unsigned char TCNT0H,TCNT0T,tenth_sec_tick;

//constants, determined by trial and error

#define wheel_timeout 10	//timeout for stopped wheel, in ticks of TMR0 overflow (1 tick = 32.7 ms)
#define PWM_bump 15			//bump PWM by this amount if wheel is stuck
#define SPDSCALE 20800		//convert wheel tick period to rotational speed. Selected so that 
//							  SPDSCALE/(period of the *slower* wheel at max rpms) = 255
#define Kp 0.15				//Proportional scale factor, optimized by trial and error.
#define Kd 0.4				//Derivative scale factor, ditto
							//(floating point not used: now integerized in code)


// OUTPUT COMPARE 1A Interrupt
// This interrupt occurs on compare match of TCNT1 with OCR1A.
// set right motor controller to coast mode (outputs "off")

SIGNAL (SIG_OUTPUT_COMPARE1A)
{
	PORTB &= ~_BV(1);  //IN1=PB.1=0
	PORTD &= ~_BV(5);  //IN2=PD.5=0
}

// OUTPUT COMPARE 1B Interrupt
// This interrupt occurs on compare match of TCNT1 with OCR1B.
// set left motor controller to coast mode (outputs "off")

SIGNAL (SIG_OUTPUT_COMPARE1B)
{
	PORTB &= ~_BV(2); 	//IN3=PB.2=0
	PORTD &= ~_BV(6);	//IN4=PD.6=0
}

// TIMER 1 OVERFLOW Interrupt
// This interrupt occurs each time TCNT1 reaches 0xFF (TOP) in 8-bit mode (488 Hz/1 MHz CPU clock)
// check direction and set motor control lines for each motor, as appropriate
// This routine could be used for timekeeping as this is a regular interrupt 

SIGNAL (SIG_OVERFLOW1)
{
	if (dir_R>0) PORTB |= _BV(1);	//forward, PB.1=1 PD.5=0
	if (dir_R<0) PORTD |= _BV(5);	//reverse, PD.5=1 PB.1=0
// for dir_R/L == 0, do nothing as motors are off
	if (dir_L>0) PORTB |= _BV(2);	//forward, PB.2=1 PD.6=0
	if (dir_L<0) PORTD |= _BV(6);	//reverse, PD.6=1 PB.2=0
}

/*
** pwm_init - Set up the pwm ports (PORT B1=IN1, B2=IN3, D5=IN2, D6=IN4)
*/
void	pwm_init( void ) 
{
	DDRB |= (1<<1) | (1<<2);		//make outputs as required
	DDRD |= (1<<5) | (1<<6);

// set outputs to off (coast) by using the following two lines:
	PORTB &= ~(_BV(1) | _BV(2));	//IN1=IN3=0 : coast
	PORTD &= ~(_BV(5) | _BV(6));	//IN2=IN4=0

// or, set outputs to brake both motors using the following two lines:
//	PORTB |= _BV(1)|_BV(2);		//IN1=IN3=1 : brake
//	PORTD |= _BV(5)|_BV(6);		//IN2=IN4=1

// set timer1 to fast PWM mode 5: WGM12+WGM10 
// and to increment at fosc/8

	TCCR1A = _BV (WGM10);   //0x01
	TCCR1B = (_BV(WGM12) | _BV (CS11));  //0x0A
	OCR1A = 0;
	OCR1B = 0;

	DDRD &= ~ (_BV(2)|_BV(3)); 	//INT0,1=input (PORTD.2,3 are shared with LCD display)

// enable interrupts for timer1 overflow and output compares

	TIMSK |= (_BV(OCIE1A) | _BV(OCIE1B) | _BV (TOIE1));
}

/*
** pwm_set - Set the two PWM speeds, pwm_L and pwm_R (globals).  Each ranges from 0 to 255
** Warning: DO NOT SET PWM TO EXCEED 255 or behavior will be unexpected!
*/

void	pwm_set(void)
{
	OCR1A = pwm_R;					//set compare registers
	OCR1B = pwm_L;
}

/*
** sat_u8
** Limit 16 bit integer input to 0 - 255 output
*/

int sat_u8(int value)
{	
   if (value > 255)	value = 255;
   else if (value < 0) value = 0;
   return value;
}

// return sign of integer ala BASIC (-1 for <0, 0 for 0, 1 for >0)

int Sign(int val)
{
	int s;
	if(val<0) s=-1; else if (val>0) s=1; else s=0;
	return s;
}

// Post requested speed and direction of rotation.
// Make sure requested speed is within limits (80-255). Speeds < 80 are set to zero

void set_speed(int l, int r)

{
	dir_L=Sign(l); dir_R=Sign(r);
	req_spd_L=abs(l); req_spd_R=abs(r);

	if(req_spd_L > 255) req_spd_L = 255;
	else if(req_spd_L < 80) req_spd_L = 0;

	if(req_spd_R > 255) req_spd_R = 255;
	else if(req_spd_R < 80) req_spd_R = 0;

	pwm_R = req_spd_R;
	pwm_L = req_spd_L;
	pwm_set();
}

/* TIMER0 interrupt handler

Timer0 clock is set to 7.8 KHz, increment every 0.128 ms, 8 bit overflow every 32.7 ms (30.6 per second)

This routine:
1. accumulates time in 16 bits, max time = 2150 seconds
2. checks for stuck wheel by counting down tick_R and tick_L and if so, bump PWM for that wheel
3. sets flag (tenth_sec_tick) for approximate 1/10 second intervals--used for debug print output, etc.
*/

SIGNAL(SIG_OVERFLOW0)  //happens every 32.7 ms
{
	TCNT0H++;
	if (TCNT0H==0) TCNT0T++; 	//increments every 8.37 sec

	if(tenth_sec_tick==0) tenth_sec_tick=3;  //3 ticks per (1 tenth second)
	tenth_sec_tick--;

//	right wheel check	
	tick_R--;					//countdown timeout for stuck right wheel
	if(tick_R==0) 				// timeout for right wheel ticks (reset in INT0 routine)
	{ 
	spd_R=0;					//inform world wheel is not rotating
	tick_R=wheel_timeout; 
	if (req_spd_R>0)	 		// if wheel stuck, bump up pwm setting.
		{
		pwm_R = sat_u8(pwm_R+PWM_bump);
		pwm_set();
		}
	}

//	left wheel check
	tick_L--;
	if(tick_L==0)				// likewise for left wheel (reset in INT1 routine)
	{ 
	spd_L=0; 					// tell everyone
	tick_L=wheel_timeout;
	if (req_spd_L>0) 
		{
		pwm_L = sat_u8(pwm_L+PWM_bump);  // if left wheel stuck, bump up pwm setting
		pwm_set();
		}
	}
}

// get 16 bit time stamp, two forms with differing resolution

unsigned int get_timer_ticks(void)	//0.128 ms ticks
{
   return (TCNT0H<<8) + TCNT0;
}

unsigned int get_timer(void)  		//32.7 ms ticks
{
   return (TCNT0T<<8) + TCNT0H;
}

/*
 INTERRUPT0 ISR
This interrupt service routine is called on each pulse of the
right WW-01 CLK line.

This routine:
1. gets the current time stamp, 
2. adjusts the current position based on the RDIR pin (PC5), 
3. calculates a new encoder clock period using the new time stamp. 
4. updates running average encoder period using low pass filter
5. from period, calculates speed and compares to requested speed.
6. applies PID correction.
*/

SIGNAL (SIG_INTERRUPT0)
{
	unsigned int tmr;
	static unsigned int tmr_last=0;
//	float err;
//	static float err_last=0.0;
	int err;
	static int err_last=0;
	tmr = (TCNT0H<<8) + TCNT0;  			//get time stamp

	if (PINC & _BV(5)) enc_pos_R++;		//increment current position
	else enc_pos_R--;
	tick_R=wheel_timeout;					//wheel is rotating, so reset timeout

	if (tmr>tmr_last)	  					//skip this round if timer overflowed last round
	{
	enc_per_R = (enc_per_R * 3 + (tmr - tmr_last)) >> 2;	//low pass filter
	spd_R=sat_u8(SPDSCALE/enc_per_R);				//new wheel speed
	if(req_spd_R>0)									//do PID correction if expected
		{
		err=(req_spd_R - spd_R);					//setpoint-actual
//		pwm_R += Kp*err+Kd*(err-err_last);
		pwm_R += (15*err+40*(err-err_last))/100;	//integerize with current optimized constants
		err_last=err;
		pwm_R = sat_u8(pwm_R);						//limit to [0,255]
		pwm_set();
		}
	}
	tmr_last = tmr;
}
/*
 INTERRUPT1 ISR
This interrupt service routine is called on each pulse of the
left wheel WW-01 CLK line. See INTERRUPT0 ISR description for actions.

*/

SIGNAL (SIG_INTERRUPT1)
{
	unsigned int tmr;
	static unsigned int tmr_last=0;
//	float err;
//	static float err_last=0.0;
	int err;
	static int err_last=0;
	
	tmr = (TCNT0H<<8) + TCNT0;

	if (PINC & _BV(4)) enc_pos_L--;	//left wheel rotates in opposite direction to right
	else enc_pos_L++;
	tick_L=wheel_timeout;				//reset wheel timeout

	if (tmr>tmr_last)					//see comments for INT0 above
	{
	enc_per_L = (enc_per_L * 3 + (tmr - tmr_last)) >> 2;
	spd_L=sat_u8(SPDSCALE/enc_per_L);
	if(req_spd_L>0)
		{
		err= (req_spd_L-spd_L);					//setpoint-actual
//		pwm_L += Kp*err+Kd*(err-err_last);
		pwm_L += (15*err+40*(err-err_last))/100;
		err_last=err;
		pwm_L = sat_u8(pwm_L);
		pwm_set();
		}
	}
	tmr_last = tmr;
}

// RS232 Character Output
// Send character c to UART, after TX holding register clears
//


int uart_putchar(char c, FILE *stream)
{
   if (c == '\n') uart_putchar('\r', stream);
   loop_until_bit_is_set(UCSRA, UDRE);
   UDR = c;
   return 0;
}

// enable serial port for output. TX=PORTD.1

void init_uart(unsigned long baudrate)
{
	unsigned int baud;
	
	UCSRB = _BV(TXEN);							// tx enable
	baud = (F_CPU / (16 * baudrate)) - 1;
	UBRRH = (unsigned char)(baud >> 8);
	UBRRL = (unsigned char)baud;
	UCSRC = (1<<URSEL)|(3<<UCSZ0);				//8 bits
//	set up serial printing
	fdevopen(uart_putchar,NULL);				// associate uart output function with stdio
}


/*
** MAIN
*/
int main(void)
{

// Timer/Counter 0 initialization
// Clock source: System Clock/1024
// Clock value: 7.8 kHz. Each tick=0.128 ms

	TCCR0=0x05;
	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);

	pwm_init();

// Timer/Counter initialization for interrupt routines

	TCNT0=TCNT0H=TCNT0T=0;
	tenth_sec_tick=3;					//there are roughly 3 ticks/second in TMR0 overflow interrupt
	enc_per_R = enc_per_L = 0;			//these need not be global, but...
	tick_R = tick_L = wheel_timeout;  	//constant in pid_kd.c

// enable serial port TX=PORTD.1 for monitor output

	init_uart(38400UL);

	sei(); //enable interrupts

	set_speed(150,150);
	while(1)

	{
	if (tenth_sec_tick==0)
// print algorithm performance for tuning
		{ 
		printf(" %u, %d, %d\n", get_timer(), spd_R, pwm_R);
		while(tenth_sec_tick==0);  //wait until tick has passed
		}	
	}
	return 0;
}