/*
    Title:    AVR-GCC program for receiver in EE281 project
    Author:   Ioan Tudosa
    Date:     12/2001
    Purpose:  Receives data through serial port, reads pressure sensor and adjust the depth in the water 
    			by controlling a digital linear actuator( stepper motor)
    needed
    Software: AVR-GCC to compile
    needed
    Hardware: ATmega163 on STK500 board
    Note:     To contact me, mail to
                  itudosa@stanford.edu
              You might find more AVR related stuff at:
                  http://www.
*/

#include <io.h>
#include <interrupt.h>
#include <math.h>
#include <sig-avr.h>
#include <string-avr.h>

typedef unsigned char  u08;
typedef          char  s08;
typedef unsigned short u16;
typedef          short s16;


u08 i,j,tmp,tmp1;
u08 buttons,pressure_l,pressure_h;
s16 pressure,pressure_offset=0,setpoint;
static volatile u08 start,end,counter;
char data[64];
float P=1,I=0,D=0;
u08 Control=0,pushout=0;


void delay(u08 delay)
{
	u08 i,j;
	
		for (i=0; i<delay; i++) 
  		{   					// delay loop  
    		j++;
  		}
}


void long_delay(u08 del)
{
	u08 i;
  
  		for (i=0; i<del; i++) 
  		{   					// delay loop  
   			delay(255);
  		}
}


void step_out(u08 rate)			// ~ 1"/5sec linear travel at 8MHz for a delay rate of 20 
{
		outp(0x0A, PORTB);			// B3-B0 is Q1-Q4 
		long_delay(rate);
		outp(0x09, PORTB);
		long_delay(rate);
		outp(0x05, PORTB);
		long_delay(rate);
		outp(0x06, PORTB);		
		long_delay(rate);
		outp(0x00, PORTB);
}


void step_in(u08 rate)		// generate a signal sequence for a step-in of 0.2 mm 
{
		outp(0x06, PORTB);
		long_delay(rate);
		outp(0x05, PORTB);
		long_delay(rate);
		outp(0x09, PORTB);
		long_delay(rate);
		outp(0x0A, PORTB);		
		long_delay(rate);
		outp(0x00, PORTB);
}


void pulse(u08 half_period)		// generate square wave of 40KHz frequency for ultrasonic transducer; implement later 
{
	u08 i,j;
	
		for(i=0;i<8;i++)	//begin loop 
		{
			sbi(PORTA,7);		//set bit in port B 
			for(j=0;j<half_period;j++);
			asm volatile("nop\n\t""nop\n\t"::);	// nop inserted to balance the 2 halves of the wave 
			cbi(PORTA,7);
			for(j=0;j<half_period;j++);
		};										//there is a test here that takes about 2-3 cycles, it is balanced above  
}

/*
void read_buttons(void)		// buttons is return global variable 
{
	u08 tmp,tmp1;
	
		tmp=inp(PINC);
    	delay(255);
    	tmp1=inp(PINC);
    	if	(tmp == tmp1) 
    		buttons=tmp;	//read debounced buttons at PortC 
}
*/


SIGNAL(SIG_ADC)				// interrupt routine reads A/D values when MCU wakes up at conversion complete interrupt 
{
		cli();					//disable interrupts 
		outp(0,ADCSR);			//disable A/D 
		outp(0x00,MCUCR);		//disable sleep mode 
		pressure_l=inp(ADCL);
    	pressure_h=inp(ADCH);	// Read converted pressure value  		
		
}

void sample_pressure(void)	//pressure_h and pressure_l are return global variables written by the interrupt routine 
{
		outp((1<<ADEN)|(0<<ADSC)|(0<<ADFR)|(1<<ADIE)|(1<<ADPS0)|(1<<ADPS1),ADCSR);	//enable ADC single conversion, enable interrupt 
		sei();													//enable interrupts 
		outp((1<<SE)|(0<<SM1)|(1<<SM0),MCUCR);					//enable sleep mode ADC noise reduction 
		asm volatile("sleep"::);	//Start A/D, in noise reduction mode conversion, for pressure sensor 
		    	
}

s16 read_pressure(void)		// do an average over 64 samples to get rid of the noise 
{
	u08 i;
	u16 sum_h,sum_l;
	
		sum_h=0;sum_l=0;
    	for(i=0;i<64;i++)
    	{
    		sample_pressure();
    		sum_h+=pressure_h;
    		sum_l+=pressure_l;
    	};
    	pressure_h=((u08)(sum_h/64))&((u08)0x03);
    	pressure_l=(u08)(sum_l/64);
    	pressure=pressure_h;						//store the 10 bit value in a variable of 16 bits "pressure"
    	pressure=(pressure<<8)+pressure_l;
    	pressure-=pressure_offset;					// IMPORTANT! extract the offset; 0.204V at 5.1V reference
    	if (pressure<0) pressure=0;					// implement only positive pressures from the offset (ground level);
    	return pressure;
}


SIGNAL(SIG_UART_RECV)			// character A is the start of data string and character Z is the end of data string 
{
	char tmp;
	
		tmp=inp(UDR);			// read uart buffer
		switch (tmp)
		{
			case 'A':								// set start flag
				if(start<3) start++;				// count 3 A
				end=0;								// not the end yet
				data[counter++]=tmp;				// record in string
				outp(~counter,PORTC);
				break;
			case 'Z':								// set end flag 
				if (start==(u08)3) 					// has it started?
				{
					if(end<3) end++;				// count 3 Z
					data[counter++]=tmp;			// record in string
					outp(~counter,PORTC);
				}
				else {start=0;counter=0;}			// if not started reset
				break;
			default:
				if 	((start==(u08)3)&&(end<3))
				{	
					data[counter++]=tmp;				// record in string "data"
					end=0;								// not the end yet
					//outp(~data[counter-1],PORTC);
				}
				else {start=0;counter=0;}			// if not started reset
				break;
		}; 
		
}

void read_uart(void)
{

		start=0; end=0; counter=0;		// reset start and end flag, reset counter for number of data bytes
		sbi(UCSRB,RXCIE);				// enable receive uart interrupt
		sei();							// enable interrupts
		while (!( (start==(u08)3) && (end==(u08)3) )) {/*long_delay(25);*/};	// loop until it reads all the data string
		cli();							// disable interrupts
		cbi(UCSRB,RXCIE);				// disable receive uart interrupt
		//outp(~data[7],PORTB);
}



u08 PID(void)
{
	u16 process_value;
	static float pi=0,err0=0;
	float pp=0,pd=0,err;
	u08 output=0;
		
		P=0.01;I=0;D=0;					// FOR TEST try only proportional term
		setpoint=150;
		
		process_value = pressure;
		if (setpoint > process_value)
			{
				err = setpoint - process_value;
				pushout=1;
			}
		else 
			{
				err = process_value - setpoint;
				pushout=0;
			};
			
		pi = pi + err * I;
		//if (pi > 100) pi = 100;		// implement some limitation to integral term
		//if (pi < 0) pi = 0;

		pd = (err - err0) * D;
		err0 = err;

		pp = err * P;

		output = pp+pi+pd;
		//if (output>100) output = 100;
		//if (output < 0) output = 0;
	
		return output;
}

void ActuateMotor(u08 Control)
{
	u08 i;
	u08 rate=200;
	
		if (pushout==1) for(i=0;i<Control;i++) step_out(rate);
		if (pushout==0) for(i=0;i<Control;i++) step_in(rate);
}

u16 u16from2u08(u08 value1,u08 value2)
{
	u16 tmp;
		tmp=value1;
		tmp=(tmp<<8)+value2;
		return tmp;
}

void InterpretData(void)
{
	const float eps=0.00000001;										// add an insignificant constant to solve some issues with pow
		setpoint=u16from2u08(data[3],data[4]);
		P=u16from2u08(data[5],data[6])*pow(10+eps,data[7]+eps);
		I=u16from2u08(data[8],data[9])*pow(10+eps,data[10]+eps);
		D=u16from2u08(data[11],data[12])*pow(10+eps,data[13]+eps);
		//outp(pow(10+eps,0+eps),PORTB);
}

int main(void)
{
	outp(0xFF,DDRB);		// use all pins on PortB for stepper control  
	outp(0xFF,DDRC);		// use PortC for output
	outp(0xFF,PORTC);
	outp(0x00,DDRD);		// use all pins on PortD for input  
	
	outp(0x80,DDRA);		// use pin 7 of port A for generating ultrasonics pulse signal; implement it later 	
	
	outp(207,UBRR);					// set baud to 2400 for 8MHz
	outp((1<<RXEN),UCSRB);			// enable receive uart
	
	outp((1<<REFS0)|(0<<ADLAR),ADMUX);	// right adjusted; use pin A0 (pin 40, ADC0) for input voltage from pressure sensor 0.2 -4.7V  
	outp((1<<ADEN)|(1<<ADSC)|(0<<ADFR)|(1<<ADPS0)|(1<<ADPS1),ADCSR);	// start first conversion; discard it; scale ADC clock by 64  
	
	pressure_offset=read_pressure();     																// don't enable interrupt for A/D, use it in the routine sample_pressure
    for(;;)
    { 
    	//read_buttons();
    	read_pressure();
		//pulse(5);				// function call takes 3 cycles, then the pulse starts  
		
		read_uart(); 
		InterpretData();
		Control=PID();
		ActuateMotor(Control);
		//step_out(20);
		//outp(~Control,PORTB);  		
    	/*
    	i=200;	
		if	((buttons & (u08)0x01)==(u08)0x00)
			step_out(i);
		if	((buttons & (u08)0x02)==(u08)0x00)
			step_in(i);
		*/	
    }
}