//this program reads data from 3 IR sensors and performs right wall following
//need to bring the /ir input low for 70ms, then clock it,
//read the input, and shift the bit in.

#include <delay.h>
#include <mega16.h>   
#include <stdio.h>      

#define clockfreq 14745600 //clock freq          

//#define DEBUG      
#define IR_CLOCK_HIGH 10  //time in usec, got these number from a code example
#define IR_CLOCK_LOW 100  //time in usec                

     		      
//FROM PWM CODE		
//clock is 14.7456 MHz, and timer is 14.7456MHz/98
//this is one timer inc every 5.425e-7 set counter
//to 256-4=252 and count 46 of these to get a 10Khz 
//clock which we use for the PWM

#define ISR_LOAD 252        
#define IR_TOT_COUNT 200 *46      //IR period is .1 sec
#define	IR_LOW_COUNT 140 *46		//stay low for 70 ms   
#define PWM_COUNT 27
#ifdef DEBUG
//we want want to write to the UART every 1 sec.  Can't count that 
//many interrupts so we increment every PWM perios, .5 ms

#define baud 9600 //UART BAUD
unsigned int uart_counts=20000;   //can't count enough interrupts
//so count IR routines instead
#endif

#define KPRIGHT 1
#define KPFRONT 2

//every 100ms we want to check the IR data   
unsigned int IRstart_counts=IR_TOT_COUNT; 
//need to hold IR vin low for more than 70ms
unsigned int IRlow_counts=IR_LOW_COUNT;    
unsigned int PWM_counts=PWM_COUNT;     
unsigned int max_front_wall = 120; 
unsigned int max_right_wall = 100 ;     
unsigned int speed=2;
int steer=0,right_wall_error = 0,front_wall_error = 0;


unsigned int IR_raw_read_1=0,  IR_good_read_1=0,IR_raw_read_2=0,  IR_good_read_2=0,
		IR_raw_read_3=0,  IR_good_read_3=0;    

int pwr_left = 5, pwr_right=5; // pulse width ratio, smaller ration means go forward!
int pwc = 0; //pulse width counter   

//bit bang to get IR data
void IR_get_bits(int bitnum)             
{		      
                 
	//I don't think these delay functions halt interrupts
 	PORTA =  0x15;	//bring bits high again       	
    delay_us(IR_CLOCK_HIGH);   //sleep .2 msec
    PORTA =  0x00;	//bring bits low again   
    delay_us(IR_CLOCK_LOW);   //sleep .2 msec        
    IR_raw_read_1 += ((PINA&0x02)!=0)<<bitnum;     
    IR_raw_read_2 += ((PINA&0x08)!=0)<<bitnum;   
//    IR_raw_read_3 += ((PINA&0x20)!=0)<<bitnum;      

	
      		
}
		
//timer 0 ISR, handles IR timing and PWM generation
interrupt [TIM0_OVF] void tim0_isr()
{     
   TCNT0=ISR_LOAD;  //need to reset it after every interrupt, duh  
   
      //~~~~~~~~~~~~~~~~~~~PWM FUNCTIONS~~~~~~~~~~~~~~~~~~~~~~~
  
   
	  
	       
    if (0==PWM_counts--)  
    {         
      PWM_counts=PWM_COUNT;
	  if (pwr_left==pwc ) //if count equals our ratio 
  		PORTC = PORTC ^ 0x80;  //toggle bit 7   
 	 if (pwr_right==pwc++ ) //if count equals our ratio 
  		PORTC = PORTC ^ 0x40;  //toggle bit 6 		
  
	  //end of period   
	  //pwm clock is much higher than opther routine clocks.
	  //so put their stuff in here, which runs at end of every period
	  //which is .5ms                                                        
  	if (pwc==100)
  	{     
  		PORTC = 0xc0;
 	 	pwc=0;    
 	 }
     }                             
	    
	 //~~~~~~~~~~~~~~~~~~~IR read FUNCTIONS~~~~~~~~~~~~~~~~~~~~~~~       
 	  if (0==IRstart_counts-- ) //bring IR vin low
 	  { 
 		   	PORTA =  0x00;		//bring bits 2 low for 70ms
 	    	IRstart_counts=IR_TOT_COUNT;     	//reload IR start counter
 	        IRlow_counts=IR_LOW_COUNT;                    
 		       //load the IR low counter for 70 ms or until PINA bit 0 is high   
 	 }          
   	
 	 if (IRlow_counts<10 ) //after a couple ms check if PINA_0 is 1, then IR is ready
 	 {  
 	    if  (1==(PINA&0x01))
   	    IRlow_counts=0;     
 	}
 
  	 if (0==IRlow_counts-- ) //after 70ms, clock IRs to read data
 	{	
 
  		IR_raw_read_1=0;  //zero to start with, we add each binary digit at a time 
 		IR_raw_read_2=0;
 //  		IR_raw_read_3=0;      
        	
 	  	IR_get_bits(7);
 		IR_get_bits(6);
 		IR_get_bits(5);
 		IR_get_bits(4);
 		IR_get_bits(3);
 		IR_get_bits(2);
 		IR_get_bits(1);
 		IR_get_bits(0);                   
            
 		PORTA =  0x15;	//bring bits high again      
       
   		IRlow_counts=10000000;   //load something stupid high, it will get set to IR_LOW_COUNT by IRstart
       
       //change reading to be 0 means something is very close                
 	   IR_good_read_1= 255-IR_raw_read_1;    	     
 	   IR_good_read_2= 255-IR_raw_read_2;    	   
 	   //saturate in a somewhat linear region
	   if( IR_good_read_1>130)	 IR_good_read_1=130;    
	   if( IR_good_read_2>130)	 IR_good_read_2=130;  
	   front_wall_error = max_front_wall-IR_good_read_1;
   	   right_wall_error = -max_right_wall+IR_good_read_2;
   	   
   	   steer=0;
   	   if (front_wall_error>=0)
   	   {
			steer = KPFRONT*(front_wall_error/12);       	
   	   }
   	   
   	   steer -= KPRIGHT*(right_wall_error/10) ;       
   	   
			//bit 7 is left, and small ratios take you forwards!   
	//comment this out to tune	
	  pwr_left = speed+steer;
	  pwr_right = speed-steer;  
	  
   	   if (pwr_left<1)
   	   	pwr_left=1;
   	   if (pwr_left>10)
   	     pwr_left=10;   
   	     
   	    if (pwr_right<1)
   	   	pwr_right=1;
   	   if (pwr_right>10)
   	     pwr_right=10;    
   	     
 //leave this if you need to tune pots  	     
//   	  pwr_right=3;
//   	  pwr_left=3;
   	     
 	}  	
       
  	  //~~~~~~~~~~~UART FUNCTION~~~~~~~~~~~~~~
 	#ifdef DEBUG
 	 if (0==uart_counts-- )
 	{                                                     
  		uart_counts=500000;     	//reload UART counter
  		printf("IRA1:%d  IRA2:%d\r", IR_good_read_1,IR_good_read_2);  
  		printf("pwm_left:%d  pwm_right:%d\r", pwr_left, pwr_right);
		printf("front_wall:%d steer:%d  \r", front_wall_error,steer);
  		
 	}
 	#endif
}
  

void main ()
{
  
   //port C bits 6 and 7 control the LEDS, and I am also 
   //putting the PWMs on them.  That way LED's should get dimmer or 
   //brighter depending on PWM rate	
   DDRC = 0xff;	//all bits are outputs
   PORTC = 0xc0;	//set bits low      
   
   //port A is IR_Sensors
   DDRA = 0x15; 	//bits 0,2,4, are output, must be high
   PORTA = 0x15;	//all outputs high for now                 
      
   TCCR0=0x2;	//set timer 0 for internal clock/8
   TCNT0=ISR_LOAD;	//timer is loaded with this number every time, counts to 256 to 
   		//generate interrupt.
   TIMSK=0x01;	//unmask timer 0 interrupt
   #asm("sei")	//global interrupt enable      
  
            
   #ifdef DEBUG         //set up the UART, turn this off in real build  
	// USART initialization
	// Communication Parameters: 8 Data, 1 Stop, No Parity   
	UCSRA=0x00;      // USART Receiver: Off
	UCSRB=0x08;	// USART Transmitter: On
	UCSRC=0x86;	// USART Mode: Asynchronous
	UBRRH=0x00;	// USART Baud rate: 9600
	UBRRL=0x5F;		       
   #endif	         
   
   while(1);
}