//ECE 476 Final Project
//Cooler-Bot Car Code
//Dan Lee and Robert Zimmerman

#include <Mega16.h>



char received = 0; //signal received flag
int transleft = 0; //transmit timer

int ainavg = 0;  //used to average the A/D conversions
signed char Ain[8];  //stores last 8 A/D conversions
char Ainptr = 0;  //pointer to current location in array

int dist = 0, dist1 = 0, dist2 = 0;   //distance measurements
int distupdate1 = 0, distupdate2 = 0;  //number of missed receives

char RX = 0;  //current receiver

unsigned int time1 = 5000;  //timer variable

unsigned char ADReady = 0;  //A/D conversion ready flag

unsigned char motor = 0;  //motor state


#define t1 5000  //timer setting





//Timer0 Compare match ISR
//This is the main decision making routine
interrupt [TIM0_COMP] void timer0_compare(void)
{                                       
    int a; //loop variable
    int angle, range; //angle and range of user
    unsigned char turn; //turn direction
    

    if (received < 10)   //if a steady signal has not been received
        dist++;          //increment the distance counter
    
    if (transleft)       //if transmitting, decrement the transmit counter
        --transleft;
    else                 //otherwise, turn off the OC2 (transmit) output of Timer2
        TCCR2=0b00001010;
    
    PORTC = time1 % 2 ? motor : motor & 0xfb;   //50% duty cycle for motors
    
    if (--time1 == 0)  //if a new pulse cycle is starting
    {
        
        transleft = 20;       //transmit pulse of length 20
        TCCR2 = 0b00011010;   //turn on OC2
        
        time1 = t1;           //reset the timer

        //clear the A/D history array
        for (a = 0; a < 8; a++)
            Ain[a] = 0;   

        //switch receivers
        RX = !RX;
        
        
        if (RX)  //if receiver 1
        {
            if (dist < 1000) //check for valid reading, ignore false triggers and non-triggers
            {
                dist1 = dist;     //update the distance for receiver 1
                distupdate1 = 0;  //reset miss counter
            } 
            else   
                distupdate1++;    //increase miss timer
        } else {
            if (dist < 1000)
            {   
                dist2 = dist;     //update the distance for receiver 2
                distupdate2 = 0;  //reset miss counter
            } 
            else   
                distupdate2++;    //increase miss timer
        }         
        
        //calculate rough angle and range
        angle = dist1 - dist2;  //this is a rough approximation
        range = dist1 + dist2;  //this is an average of the two
        
        if (angle > 10)         //turn right if angle is large enough
            turn = 0x01; 
        else if (angle < -10)   //turn left if angle is large enough
            turn = 0x02;
        else                    //otherwise don't turn
            turn = 0x00;  
        
        //turn away from the blind receiver if only one reciever can hear
        if (distupdate1 > 4)
        {
            if (distupdate2 > 4)
                turn = 0;
            else
                turn = 0x01;
        } else {
            if (distupdate2 > 4)
                turn = 0x02;
        }     
        
        
        //turn on the motor if the range is large is large enough
        if (range > 600)
            motor = 0x04 | turn;
        else
            motor = turn;
        

        //reset counters
        dist = 0;
        received = 0;
        ainavg = 0;
        
        //start A/D conversions
        ADCSRA = 0b11001100;
        
    } 
    else if (time1 == 2500)  //halfway through cycle
    {
        if (RX)  //switch the A/D MUX, this gives time for the ADC to settle
            ADMUX = 0b01101001; 
        else
            ADMUX = 0b01101101;
    }
}

//ADC Ready Interrupt
//This flags the code when a conversion is ready
interrupt [ADC_INT] void adcready()
{
    ADReady = 1; 
}


//Main routine
void main(void)
{
    signed char read, dummy;
    
    DDRD = 0xc0; //pin 6 and 7 are outputs, the rest are inputs
                 //PORTD6 is no longer used, PORTD7 = OC2
                 //PORTD2 = 23.8kHz square wave from OC0
    DDRB = 0xff; //PORTB3 = OC0, so all pins are set to output
    DDRC = 0xff; //PORTC0,1 = front motor control
                 //PORTC2 = rear motor control

    PORTB = 0x00; //initialize all to low
    PORTC = 0x00; //motors off
    
    //set up timer 0 and 2
    //timer 0 is fed back to PORTD2
    TIMSK=2;		//turn on cmp match ISR 
    OCR0 = 41;  	//creates 23.8kHz output
    TCCR0=0b00011010;  //turns on OC0
    OCR2 = 41;	    //creates 23.8kHz output
    TCCR2=0b00001010; //leave OC2 off for now
    
    //no longer used
    PORTD.6 = 1;
    
    //init the A to D converter 
    ADMUX = 0b01101101;      
    //enable ADC and set prescaler to 1/16

    //crank up the ISRs
    #asm
        sei
    #endasm  
        

    //main loop
        while (1)
        {
            
            while (!ADReady)  //wait for ADC conversion
                ;
            
           
            read = ADCH; //read the result
                        
            //update position in history array
            if (++Ainptr == 8)
            {
                Ainptr = 0;
            }

            //this saves the time of computing the average by
            //looping through the array each time
            //simply subtract the oldest value and add the newest
            ainavg -= Ain[Ainptr];
            
            //this "multiplies" the input signal by the reference 23.8kHz signal
            if (PIND.2) dummy = read;  else dummy = -read;
            
            //save the new result
            Ain[Ainptr] = dummy;
            
            //update the average
            ainavg += dummy;
            
            //correlation of 10 required for detection
            //use positive and negative penalties to avoid false triggering
            if (ainavg > 10 || ainavg < -10)
                received += 3;
            else if (received)
                received--; 
            
            //if pulse not yet detected, keep convertin'
            if (received < 10)
                ADCSRA = 0b11001100;
            
            //wait for conversion
            ADReady = 0;
            
        }
}