/*--------------------
LEV V. 5 Firmware

Chris King 2009

Target: ATmega168
Fosc:   14,745,600 hz
--------------------*/

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

volatile int n; //PWM settings to be clipped
volatile int e;
volatile int s;
volatile int w;

volatile int x; //sensors with offsets
volatile int y;
volatile int z;

volatile int calx; //user offsets
volatile int caly;
volatile int calz;

volatile char power;
volatile char here;

volatile int xs;
volatile int ys;
volatile int zs;

char incamt = 20;

int limitrange = 150; //user power limit
int lowerlimit;
int upperlimit;


//------------------------------------SUBS---------------------------------------
int sample(ch){ //Function to sample ADC on a given channel
ADMUX &= 0xF8; // clear bottom 3 bits
ADMUX |= ch; // set bottom 3 bits to channel "ch"
ADCSRA |= (1 << ADSC); //start conversion
while (bit_is_set(ADCSRA, ADSC));//wait for conversion to finish
return ADC - 512; //ref is 0
} //End function
//-------------------------------------------------------------------------------
void adcpre(p){ //Change ADC prescale
ADCSRA &= 0xF8; // clear bottom 3 bits
ADCSRA |= p;   // set bottom 3 bits to channel "ch"
} //End function
//-------------------------------------------------------------------------------
void cal(){ //calibrate to current sensor inputs
while (bit_is_set(ADCSRA, ADSC));
calx = -1 * sample(0);
caly = -1 * sample(1);
calz = -1 * sample(2);
}//end cal
//-------------------------------------------------------------------------------
ISR(INT0_vect) //INT0 went low     
{
PORTD |= (1<<7);  //LED on
cal(); //zero sensors
PORTD &= ~(1<<7); //LED off
} //end INT0
//-------------------------------------------------------------------------------
ISR(INT1_vect) //INT1 went low     
{
if (power == 1){
power = 0;
}else{
power = 1;
} 
} //end INT1
//-------------------------------------------------------------------------------
ISR(USART_RX_vect)  //Serial RX complete interrupt
{
//PORTD |= (1<<7); //LED on
unsigned char ReceivedByte;
ReceivedByte = UDR0;//Read Received Byte
switch (ReceivedByte) {
case 0:
UDR0 = OCR1A;    //N
break;
case 1:
UDR0 = OCR1B;    //S
break;
case 2:
UDR0 = OCR0A;    //E
break;
case 3:
UDR0 = OCR0B;    //W
break;
case 4:
UDR0 = xs + 127;    
break;
case 5:
UDR0 = ys + 127;    
break;
case 6:
UDR0 = zs;    
break;
case 7:
calx = calx + incamt;    //increment cal value
break;
case 8:
calx = calx - incamt;
break;
case 9:
caly = caly + incamt;
break;
case 10:
caly = caly - incamt;
break;
case 11:
calz = calz + incamt;
break;
case 12:
calz = calz - incamt;
break;
case 13:                 //Power states
power = 1;               
break;
case 14:
power = 0;
break;                   
case 15:                 //Speeds
clock_prescale_set(0);   //57600hz PWM
UBRR0 = 15;              //Set baud rate
adcpre(4);               //Set prescale
break;
case 16:
clock_prescale_set(1);   //28800hz PWM
UBRR0 = 7;               //Set baud rate
adcpre(3);               //Set prescale
break;
case 17:
clock_prescale_set(2);   //14400hz PWM
UBRR0 = 3;               //Set baud rate
adcpre(2);               //Set prescale
break;
case 18:
clock_prescale_set(3);   //7200hz PWM
UBRR0 = 1;               //Set baud rate
adcpre(1);               //Set prescale
break;
}
unsigned char dummy;
while ( UCSR0A & (1<<RXC0) ) dummy = UDR0; //Dump the receive buffer!
//PORTD &= ~(1<<7); //LED off
} //end Serial RX complete interrupt
//-------------------------------END SUBS----------------------------------------

int main(){

//Set CPU clock prescale
clock_prescale_set(3);//0=1,1=2,2=4,3=8 Start at 7200hz

lowerlimit = 127-(limitrange/2);
upperlimit = 127+(limitrange/2);

DDRD = 0b11100000;  //pins 5,6,7 on port D out, D7 is the LED
DDRB = 0b00000111;  //pins 0,1,2 on port B out 

//PWM config for timer 0
TCCR0B |= (1<<CS00);            // prescale of 1
TCCR0A |= (1<<WGM00)|(1<<WGM01)|(1<<COM0A1)|(1<<COM0A0)|(1<<COM0B1)|(1<<COM0B0); //set on match, clear at bottom
OCR0A = 127; //Start at 50%, Out pair 3
OCR0B = 127; //Start at 50%, Out pair 4 

//PWM config for timer 1
TCCR1B |= (1<<CS10) | (1<<WGM12); // prescale of 1, 8 bit fast PWM
TCCR1A |= (1<<WGM10)|(1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0); //set on match, clear at bottom, 8 bit fast PWM
OCR1A = 127; //Start at 50%, Out pair 1
OCR1B = 127; //Start at 50%, Out pair 2 

//ADC config:
adcpre(2); //Set prescale
ADCSRA |= (1 << ADEN) | (1 << ADSC); // Enable ADC

//UART config
UBRR0 = 1;               //Set baud rate
UCSR0B = (1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0);//Enable receiver, transmitter, and received inturrupt
UCSR0C = (1<<UCSZ00)|(1<<UCSZ01);//Set format: 8 data bits, 1 stop bit

//INT0, INT1 config
EIMSK |= (1 << INT0) | (1 << INT1);  //enable INT0, INT1
EICRA |= (1 << ISC01) | (1 << ISC11);//falling edge 

cal();//zero sensors

PORTB |= (1 << 0); //!Send enable to chips!

sei(); //global interrupt enable

for (;;){ //Main Loop

PORTD ^= (1<<7);

//Sample, Offset, Mix, Clip, Set
x = -(sample(0) + calx); //sample ch 0: X
y = (sample(1) + caly);  //sample ch 1: Y
z = (sample(2) + calz);  //sample ch 2: Z

here = 1;//See if object is present by checking z

if(x < -127) xs = -127; else if(x > 127) xs = 127; else xs = x;
if(y < -127) ys = -127; else if(y > 127) ys = 127; else ys = y;
if(z < -127) zs = -127; else if(z > 127) zs = 127; else zs = z;

if ((power & here) == 1) { 

n = z + 127 + y; //Out pair 1: OCR1A ,N
e = z + 127 + x; //Out pair 2: OCR1B ,E
s = z + 127 - y; //Out pair 3: OCR0A ,S
w = z + 127 - x; //Out pair 4: OCR0B ,W 

if(n < lowerlimit) n = lowerlimit; //Clip 
if(n > upperlimit) n = upperlimit;
if(e < lowerlimit) e = lowerlimit;    
if(e > upperlimit) e = upperlimit;
if(s < lowerlimit) s = lowerlimit;    
if(s > upperlimit) s = upperlimit;
if(w < lowerlimit) w = lowerlimit;     
if(w > upperlimit) w = upperlimit;

OCR1A = n; //Set
OCR1B = e;
OCR0A = s;
OCR0B = w;

PORTB |= (1 << 0); //!Send enable to chips!

}else{

PORTB &= ~(1 << 0); //power off chips
OCR1A = 127; //Set
OCR1B = 127;
OCR0A = 127;
OCR0B = 127;

} //end if
} //End Main Loop
} //End program