//Distributed under the GPL v 2.0
//by fenn 03/10/07
//industrial servo control with PID algorithm demo
//this code was written for an attiny26 and is still buggy

#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>


#define SETBIT(PORT, BIT)  PORT |= (1<<BIT)
#define CLRBIT(PORT, BIT)  PORT &= ~(1<<BIT)

//==0 is a quick instruction that returns a logical result
//otherwise isset/isnset return a bitmask which can cause probs
#define ISNSET(PORT, BIT)  (PORT & (1<<BIT)) == 0 
#define ISSET(PORT, BIT)  !ISNSET(PORT, BIT) 
//if (ISSET(PIND, 2)) ...

/* tweak here to sacrifice some pwm resolution to get the exact frequency */
#define PWM_MAX_COUNT 128  


/* pinout */
#define LED PA0
#define ENC0_A PB4
#define ENC0_B PB5
#define ENC0_PORT PINB

// needs to be on ocr1x; this macro is mostly useless now
#define MOTOR_A PB0
#define MOTOR_B PB2

volatile int enc0_pos=0;

/*
unsigned int adc_sample (void){
	ADCSR |= _BV(ADSC);			// start conversion
	while (ADCSR & _BV(ADSC) ) {}	// wait until completed
	return (ADC);					
}

unsigned long int ADCaverage (int n){
	unsigned long int currentvalue = 0;             // incremental value
	unsigned int count;                        // amount of samples to take
    for(count=0;count<n;count++) {         // initialise array
        currentvalue +=adc_sample(); //running total
    }
	//return (currentvalue/n);           //Divide by n for average
	return (currentvalue);           //Divide by n for average
}*/

SIGNAL(SIG_INTERRUPT0)
{ //update quadrature state
	if((ISSET(ENC0_PORT, ENC0_A) == (ISSET(ENC0_PORT, ENC0_B))))
	{ enc0_pos++; }
	else
	{ enc0_pos--; }
	//PORTA ^= 1<<LED; //toggle the LED pin
}

long int boundscheck(long int n, long int bound)
{
	//int bound = 0xFF;
	if (n > bound ) { n = bound; }
	else if(n < -bound ) { n = -bound; }
	return n;
}
	

//for a delay of 1 second, n = about 23000
void delay(unsigned long int n) 
{
	unsigned long int a;
	for(a=0;a<n;a++)
	{ }
}

void pwm(int n)
{
	//change <  to > to reverse pwm polarity/direction
	if(n<0){
		OCR1A=(uint8_t) -n;
		OCR1B=0; }
	else
	{	
		OCR1A=0;
		OCR1B=(uint8_t) n; }
}


int main(void)
{
	//set port i/o direction
	DDRB=	(0<<ENC0_A) |
			(0<<ENC0_B) |
			(1<<MOTOR_A) |
			(1<<MOTOR_B) ;

	DDRA=	(1<<LED) |
			(0<<PA1) |
	//		(1<<MOTOR_A) |
	//		(1<<MOTOR_B) |
			(1<<PA4) |
			(1<<PA5) |
			(1<<PA6) |
			(1<<PA7); 
	
	//start the PLL clock and use it for the PWM timer
	PLLCSR =(1<<PCKE) |
			(1<<PLLE);
	//setup counter for pwm mode; see attiny26 datasheet page 75? (table 35)
	TCCR1A =(0<<COM1A1) |
			(1<<COM1A0) |
			(0<<COM1B1) |
			(1<<COM1B0) |
			(1<<PWM1A)  |
			(1<<PWM1B)  ;

	OCR1C  = PWM_MAX_COUNT; //reset timer when reached
	TCCR1B =(1<<CTC1) | //use PWM_MAX_COUNT
			(0<<CS13) | //0100 for around 37kHz
			(1<<CS12) | //0101 for around 17kHz
			(0<<CS11) | //0011 for around 65kHz
			(1<<CS10) ; //see table 37 in attiny26 datasheet
	
	//setup analog to digital converter
	ADCSR =(1<<ADEN) |
		//	(1<<ADLAR)| // left adjust for 8 bit resolution
			(1<<ADFR) | //sample continuously
			(0<<ADPS0)| // not sure what frequency this is
			(0<<ADPS1)| // not sure what frequency this is
			(0<<ADPS2) ;// not sure what frequency this is

	sei(); //global interrupt enable
	GIMSK |= (1<<INT0); // turn on interrupt 0
	MCUCR |= (0<<ISC01) | (1<<ISC00); //interrupt on any change of int0 (pb6)
	//MCUCR |= (1<<ISC01) | (1<<ISC00); //interrupt on rising edge of int0 (pb6); gives half the resolution
	ADMUX=1 & 7; //pick adc channel; there's got to be a better way to do this
	ADCSR |= _BV(ADSC); // start adc conversion; ADCSR is ADCSRA/B/C on other chips
	uint8_t old_pos=0, delta=0;
	uint8_t count=0;
	int setpoint=0, error=0, prev_error=0, differential=0, prev_differential=0;
	long proportional=0, integral=0;
	int32_t pid_total=0;
	//PORTB = MOTOR_A;
	while (1)
	{

		//setpoint = ADCaverage(1);
		//setpoint=-ADC; 
		//forward(count);
		setpoint = count;
		/* PID STUFF */
		prev_error = error;
		error = setpoint - enc0_pos;

		proportional =  (long) error * 1000;  //Kp 
		proportional = boundscheck(proportional, 0x7FFF);

		count+=1; 
		//integral = (long) integral + (long) (error * 1); //Ki
		integral = boundscheck(integral, 0x7FFF);


		prev_differential = differential;
		differential = ( (long) (error - prev_error) * 1000 ); //Kd
		differential = boundscheck(differential, 0x7FFF);
		
		//the final sum, with an overflow check, in fixed point
		pid_total = (proportional + integral + differential)>>8; //Ko
		pid_total = boundscheck(pid_total, 0xFF);

		/*//set max pwm value at ~95% of 2^8 (change this to whatever you like)
		if ( temp_pid_sum > 0xFC ) pid_total = 0xFC;
		else
			if ( temp_pid_sum < 0 ) pid_total = 0;
			else
				pid_total = temp_pid_sum;
			*/

		pwm(pid_total);
		/* end PID section */
		//PORTB=~PORTB;
		old_pos = enc0_pos;
		delay(100);
		delta = enc0_pos - old_pos;

//		PORTA=(enc0_pos<<4 | (PORTA & 0x0F)); //show position in binary on PA4-PA7
		PORTA=((uint8_t)enc0_pos<<4 | (PORTA & 0x0F)); //show setpoint in binary on PA4-PA7
		PORTA ^=_BV(LED); //toggle the led

	}
}