//Auditory Navigator
//Garret Phillips, Matt Kinne, Nick Annetta
//ECE 4760
//Cornell University
//5/5/2010
//Implemented on the Atmel Mega32

#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include <stdio.h> // for sine
#include "uart.h"
#include <stdlib.h>
#include "lcd_lib.h"
#include <avr/pgmspace.h>
#include <util/delay.h>
#define countMS 62 //ticks/mSec

// ramp constants
#define RAMPUPEND 250 // = 4*62.5 or 4mSec * 62.5 samples/mSec NOTE:max=255
#define RAMPDOWNSTART 625 // = 10*62.5
#define RAMPDOWNEND 875 // = 14*62.5 NOTE: RAMPDOWNEND-RAMPDOWNSTART<255

#define RAMPUPEND2 (250 )// = 4*62.5 or 4mSec * 62.5 samples/mSec NOTE:max=255
#define RAMPDOWNSTART2 (625 ) // = 10*62.5
#define RAMPDOWNEND2 (875 )// = 14*62.5 NOTE: RAMPDOWNEND-RAMPDOWNSTART<255

#define MAXOFFSET 600

// The DDS variables
volatile unsigned long accumulator ;
volatile unsigned char highbyte ;
volatile unsigned long accumulator2 ;
volatile unsigned char highbyte2 ;
volatile unsigned long increment;

// tables for DDS
signed char sineTable[256] ;

char rampTable[256], left, rotateState ;

int delayIncrementTable[91];

double amplitudeIncrementTable[91] = {
254.3257,
254.1223,
253.5132,
252.5011,
251.0911,
249.2897,
247.1056,
244.5490,
241.6320,
238.3680,
234.7722,
230.8610,
226.6520,
222.1640,
217.4167,
212.4307,
207.2272,
201.8280,
196.2552,
190.5312,
184.6784,
178.7192,
172.6758,
166.5700,
160.4233,
154.2563,
148.0893,
141.9416,
135.8316,
129.7768,
123.7936,
117.8975,
112.1027,
106.4224,
100.8683,
95.4513,
90.1807,
85.0650,
80.1112,
75.3253,
70.7120,
66.2752,
62.0174,
57.9404,
54.0449,
50.3307,
46.7968,
43.4415,
40.2623,
37.2561,
34.4192,
31.7476,
29.2364,
26.8809,
24.6756,
22.6151,
20.6934,
18.9048,
17.2432,
15.7024,
14.2765,
12.9593,
11.7449,
10.6272,
9.6005,
8.6591,
7.7976,
7.0105,
6.2929,
5.6396,
5.0461,
4.5078,
4.0205,
3.5802,
3.1830,
2.8253,
2.5038,
2.2154,
1.9570,
1.7260,
1.5199,
1.3362,
1.1728,
1.0278,
0.8993,
0.7855,
0.6851,
0.5966,
0.5186,
0.4501,
0.3901};

//sound delay
int offsetL,offsetR;

// Time variables
// the volitile is needed because the time is only set in the ISR
// time counts mSec, sample counts DDS samples (62.5 KHz)
volatile unsigned int time, sample, sample2, rampCount, rampCount2;
volatile unsigned int count;
volatile unsigned char fadeCountR,fadeCountL ;
volatile int sound_dir;

unsigned char amplitudeR, amplitudeL;

int input = 0;

// index for sine table build
//int i;

#define start 1
#define end 0

volatile unsigned char comp_clk,comp_dout,comp_en,comp_index;
volatile unsigned int comp_din,comp_clock,get_data,gps_time;
volatile int comp_x_val,comp_y_val,comp_angle,comp_i, comp_time;

//the subroutines
//void initialize(void); //all the usual mcu stuff

unsigned int i,j,k; //counter

//RXC ISR variables
volatile unsigned char r_index, r_count, r1_count; //current string index
volatile unsigned char r_buffer[200],r2_buffer[200]; //input string
volatile unsigned char r_ready; //flag for receive done
volatile unsigned char r_char; //current character
volatile unsigned char string_ready; //received full packet flag
volatile unsigned char dir_ready;

volatile unsigned char target_dir_char[2];

volatile unsigned long lat1min,lon1min, lat1_min,lon1_min, lon1_min_dec, lat1_min_dec, lat1_deg, lon1_deg;
volatile float dlon, target_dir, lon2, lat2,lat1f,lon1f,lat1, lon1;
volatile unsigned long lat1a,lat1b,lon1a,lon1b,ld1;

volatile unsigned char gps_status, rx_transmission_flag, compass_done;

volatile unsigned char led; //for debugging

//state machine states
volatile unsigned char waitState;
#define beginWait 1
#define GWait 2
#define PWait 3
#define RWait 4
#define MWait 5
#define CWait 6
#define returnWait 7

volatile unsigned char commaState;
#define timeStamp 1
#define gpsStatus 2
#define latIn 3
#define latInDir 4
#define lonIn 5
#define lonInDir 6
#define speedIn 7
#define trackAngle 8
#define magVar 9
#define magVarDir 10
#define checkSum 11

//equal to (max wave amp)/(degrees in resolution)
//=255/90
#define AMPLITUDE_INCREMENT 2.833

//=MAXOFFSET/90
#define DELAY_INCREMENT 6.666

#define pi 3.14159

void compass(void); //get data from compass
void comp_wait(void); //delay
void comp_toggle_ena(void);
void GPS(void); //calculate bearing to target
void sound(void); //output sound over PWM
void initialize(void);

// LCD display buffer
const char loc1[] PROGMEM = "Eng Quad\0";
const char loc2[] PROGMEM = "BRB\0";
const char loc3[] PROGMEM = "Clock Tower\0";
const char loc4[] PROGMEM = "CTB\0";
const char loc5[] PROGMEM = "Apartment\0";
const char loc6[] PROGMEM = "\0";

char* locations[] PROGMEM = {loc1, loc2, loc3, loc4, loc5, loc6};

//track location table index
char locationIndex;

void degreesToSound(int degrees);

char calculateAmplitude(int degree);

int calculateDelay(int degree);

void init_lcd(void);

void checkButtons();

void printLocation();

FILE shell_uart = FDEV_SETUP_STREAM(shell_putchar, shell_getchar, _FDEV_SETUP_RW);

//UART character-ready ISR
ISR (USART_RXC_vect)
{
r_char=UDR; //get a char

//state machine checks for RMC sentence
switch (waitState)
{
case beginWait:
if (r_char == '$'){
waitState=GWait;
r_buffer[r_index]=r_char;
r_index++;
rx_transmission_flag=1;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
case GWait:
if (r_char == 'G'){
waitState=PWait;
r_buffer[r_index]=r_char;
r_index++;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
case PWait:
if (r_char == 'P'){
waitState=RWait;
r_buffer[r_index]=r_char;
r_index++;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
case RWait:
if (r_char == 'R'){
waitState=MWait;
r_buffer[r_index]=r_char;
r_index++;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
case MWait:
if (r_char == 'M'){
waitState=CWait;
r_buffer[r_index]=r_char;
r_index++;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
case CWait:
if (r_char == 'C'){
waitState=returnWait;
r_buffer[r_index]=r_char;
r_index++;
}
else
{
waitState=beginWait;
r_index=0;
}
break;
//record data and wait for \n termination
case returnWait:
if (r_char == '\n'){
r_buffer[r_index]=r_char;
string_ready=1;
r1_count=r_index;
rx_transmission_flag=0;
r_index=0;
}
else
{
if(r_char=='.'){
//don't put it in the buffer
}
else{
waitState=returnWait;
r_buffer[r_index]=r_char;
r_index++;
}
}
break;
}
}

//clocked for compass
ISR(TIMER1_COMPA_vect)
{

if(comp_clock&&((comp_i%2)==0)){ //cut comp_clock in half
comp_clk = comp_clk^0x01; //Toggle comp_clock
}

if((comp_clk==1)&get_data){
if(comp_index%2==0){ //takes care of double comp_clock
comp_din|=(((PINB&(0b00000010))>>1)<<((comp_index)>>1)); //comp_index gets divided by two (comp_index>>1) because of mod
}
comp_index--;
}
comp_i++; //Master time keeper
}

//clocked for PWM 0
ISR (TIMER0_OVF_vect)
{
//the actual DDR
accumulator = accumulator + increment ; //sets frequency of sound pulse
highbyte = (char)((accumulator+offsetR) >> 24) ;

OCR0 = 128 + ((sineTable[highbyte] * rampTable[rampCount])>>7) ;

//right side
sample++ ;
if(sample < offsetR)
OCR0 = 128;
else{
//ramp up
if ((sample <= RAMPUPEND+offsetR)&&(rampCount<amplitudeR)){
rampCount++;
}
//middle of pulse
if (sample > RAMPUPEND+offsetR && sample <= RAMPDOWNSTART+offsetR ) {
rampCount = amplitudeR ;
}
//ramp down
if ((sample > RAMPDOWNSTART+offsetR) && (sample <= RAMPDOWNEND+offsetR)
&&(rampCount>0) )rampCount--;
//turn off
if (sample > RAMPDOWNEND+offsetR) rampCount = 0;
}

//left side
if(sample < offsetL)
OCR2 = 128;
else{
//ramp up
if ((sample <= RAMPUPEND+offsetL)&&(rampCount2<amplitudeL)){
rampCount2++;
}
//middle of pulse
if (sample > RAMPUPEND2+offsetL && sample <= RAMPDOWNSTART2+offsetL ) {
rampCount2 = amplitudeL ;

}
//ramp down
if ((sample > RAMPDOWNSTART2+offsetL) && (sample <= RAMPDOWNEND2+offsetL)
&& (rampCount2>0) )rampCount2--;
//turn off
if (sample > RAMPDOWNEND2+offsetL) rampCount2 = 0;
}
// generate time base for MAIN
// 62 counts is about 1 mSec
count--;
if (0 == count ){
count=countMS;
time++; //in mSec
}
}

//clocked for PWM 2
ISR (TIMER2_OVF_vect){
accumulator2 = accumulator2 + (long)(increment) ;
highbyte2 = (char)((accumulator-offsetL) >> 24) ;

// output the wavefrom sample
OCR2 = 128 + ((sineTable[highbyte2] * rampTable[rampCount2])>>7) ;
}

void comp_wait(void){ //Wait a bit ////////////

while (comp_i<(comp_time+2)){
PORTB = (comp_dout|comp_clk|comp_en);
}
comp_time=comp_i;
}

void comp_toggle_ena(void){ // Toggle Enable //////

while (comp_i<(comp_time+2)){
comp_en=(1<<2);
PORTB = (comp_dout|comp_clk|comp_en); //Send Data
}
comp_en=(0<<2);
PORTB = (comp_dout|comp_clk|comp_en); //Send Data
comp_time=comp_i;
}

//get compass data
void compass(void){

TIMSK=(1<<OCIE1A); //turn on timer 0 compare match ISR
OCR1A=128; //compate on 250
TCCR1B=2; //divide by 1024
TCCR1A|=(1<<WGM11); //clear on match, was TCCR0A|=(1<<WGM01);

DDRB=0xff; //all output
PORTB|=(1<<2); //enable high

comp_en=0;
comp_i=0; //Reset the overall comp_clock
comp_time=0; //Reset the overall comp_clock

comp_wait();

comp_toggle_ena();

comp_wait();

while(comp_i<(comp_time+16)){ // Send Reset ////
comp_clock=start;
PORTB = (comp_dout|comp_clk|comp_en) ;
}

comp_clock=end;
comp_time=comp_i;

comp_wait();

comp_toggle_ena();

comp_wait();

while(comp_i<(comp_time+16)){ // Send Start ////
comp_clock=start;
if(comp_i<(comp_time+4)){ //Send Start Bit 1000
comp_dout=(1<<1);
}
else{
comp_dout=(0<<1);
}

PORTB = (comp_dout|comp_clk|comp_en) ;
}

comp_clock=end;
comp_time=comp_i;

while(comp_din!=12){ //12 0b00001100 Wait for the compass data to be ready//////

comp_wait();

comp_toggle_ena();

comp_wait();

comp_index=7; //decrement (2N-1) Need to figure out why 6 doesnt work!!!
while(comp_i<(comp_time+16)){ // Send Measure ////
comp_clock=start;
if(comp_i<(comp_time+8)){ //Send Status Bit 1100
comp_dout=(1<<1);
}
else{
comp_dout=(0<<1);
}
PORTB = (comp_dout|comp_clk|comp_en) ;
}

comp_clock=end;
comp_time=comp_i;
comp_din=0; //Reset the Status value
comp_index=7; //decrement (2N-1)
comp_wait();

DDRB=0b11111101; //make C.1 input to get data
while(comp_i<(comp_time+16)){ // Get Status ////
comp_clock=start;
get_data=start;
PORTB = (comp_clk|comp_en) ;
}

DDRB=0b11111111; //make C.1 output to send data

comp_clock=end;
comp_time=comp_i;

get_data=end; //get_data might not be important (might save time thought)

}

comp_din=0; //clear status data, now use for x data
comp_index=21; //decrement (get 11 bits) 2N-1
comp_wait();

DDRB=0b11111101; //make C.1 input to get data

while(comp_i<(comp_time+44)){ // Get X Data
comp_clock=start;
get_data=start;
PORTB = (comp_clk|comp_en) ;
}

DDRB=0b11111111; //make C.1 output to send data (might not need, everything is done)

comp_x_val=comp_din<<5; //shift to get proper 2's comp
comp_clock=end;
comp_time=comp_i;

comp_wait();

comp_din=0; //clear x data, now use for y data
comp_index=21; //decrement (get 11 bits)
DDRB=0b11111101; //make C.1 input to get data

while(comp_i<(comp_time+44)){ // Get Y Data
comp_clock=start;
get_data=start;
PORTB = (comp_clk|comp_en) ;
}

DDRB=0b11111111; //make C.1 output to send data (might not need, everything is done)
comp_y_val=comp_din<<5; //need mask
comp_clock=end;
comp_time=comp_i;
comp_en=(1<<2);//Bring enable high to finish
PORTB = (comp_dout|comp_clk|comp_en) ;

comp_angle=(atan2(comp_y_val,comp_x_val))*57.32; //Compute comp_angle
comp_angle=comp_angle*(-1);

if (comp_angle<0){ //convert from (-180 to 180) to (0 to 360)
comp_angle=360+comp_angle;
}

comp_angle=comp_angle+90; //Rotate the compass

if (comp_angle>359){
comp_angle=comp_angle-360;
}

//compensate for magnetic variation 12W
if(comp_angle>11){
comp_angle = comp_angle-12;
}
else{
comp_angle=comp_angle+348;
}

TCNT0=0;

TCCR0 = 1 ;
//turn on timer 0 overflow ISR
TIMSK = (1<<TOIE0)|(1<<TOIE2) ;
// turn on PWM
// turn on fast PWM and OC0A output
// at full clock rate, toggle OC0A (pin B3)
// 16 microsec per PWM cycle sample time
TCCR0 |= (1<<COM00) | (1<<COM01) | (1<<WGM00) | (1<<WGM01) ;
TCCR2 = (1<<COM21) | (1<<COM20)| (1<<WGM20) | (1<<WGM21) |(1<<CS20) ;
OCR0 = 128 ; // set PWM to half full scale
OCR2 = 128 ;
}

int main(void){

initialize();
while(1){
if(time>40 && compass_done == 0){
PORTD = PORTD ^ 0b00000100;
compass();
PORTD = PORTD ^ 0b00000100;
compass_done=1;
}
if (time>41){
time=0;
compass_done=0;
sound();
gps_time++;
}

GPS();

if(gps_time==2){

TIMSK=0; //Turn off timer1,timer0, and timer2
time=0;
UCSRB = 0x98; //Turn on GPS
fprintf(&shell_uart, "\r\n %d, %f, %d", sound_dir,target_dir,comp_angle);
gps_time=0;
}
}
}

void GPS(void){

if (string_ready==1){
UCSRB = 0; //Turn off Serial Interupt
waitState=beginWait;

time=0;
//Turn on The PWM
TCCR0 = 1 ;
TIMSK = (1<<TOIE0)|(1<<TOIE2) ;
TCCR0 |= (1<<COM00) | (1<<COM01) | (1<<WGM00) | (1<<WGM01) ;
TCCR2 = (1<<COM21) | (1<<COM20) | (1<<WGM20) | (1<<WGM21) | (1<<CS20);
OCR2 = 128 ;
OCR0 = 128 ;

//get longitude and latitude from string buffer
sscanf(r_buffer,"%*6s,%*6ld,%*c,%2ld%ld,%*c,%3ld%ld,%*1c,%*5ld,%*3ld,%*6ld,%*1ld,%*c,%*s",&lat1a,&lat1min,&lon1a,&lon1min);

//convert from minutes to degrees
lat1 = lat1min/(60.0f*10000);
lon1 = lon1min/(60.0f*10000);
lat1 = lat1+(float)(lat1a);
lon1 = lon1+(float)(lon1a);

//convert to radians
lat1 = lat1*pi/180.0;
lon1 = lon1*pi/180.0;

//calculate direction to target
dlon = lon2-lon1;
target_dir = atan2(sin(dlon)*cos(lat2), (cos(lat1)*sin(lat2))-(sin(lat1)*cos(lat2)*cos(dlon)));
target_dir = target_dir*180.0/pi;

target_dir = target_dir*(-1.0);

if(target_dir<0){
target_dir = target_dir+360.0;
}

k=0;
dir_ready=1;
string_ready=0;
PORTD = PORTD ^ 0b00000100; //toggle LED
time=0;
}

}

void sound(void){
sound_dir = target_dir - comp_angle;
if(sound_dir < 0){
sound_dir = sound_dir+360;
}

degreesToSound((int)sound_dir);
// init ramp variables
sample = 0;
rampCount = 0;

increment = 68719000L ;

// phase lock the sine generator DDS
accumulator = 0 ;
}

 

void initialize(void){
count=countMS;
gps_time=0;

DDRB = (1<<PINB3) ;

PORTB = (1<<PIN6)|(1<<PIN7);
locationIndex = 0;

//delay table
int r;
for(r=0; r<91; r++){
delayIncrementTable[r] = DELAY_INCREMENT*r;
}

// make B.3 an output
DDRD = (1<<PIND7) | (1<<PIND6);

// init the sine table
for (i=0; i<256; i++){
sineTable[i] = (char)(127.0 * sin(6.283*((float)i)/256.0)) ;
// the following table needs
// rampTable[0]=0 and rampTable[255]=127
rampTable[i] = i>>1 ;
}

// init the time counter
time=0;

UCSRB = 0x98;

UBRRL = 207 ; //4800 baud rate with 16MHz clk

DDRD= DDRD ^ 0b00000100; // PORT D.1,2 is an ouput
PORTD = PORTD ^ 0b00000100; //toggle LED

r_ready=0;
r_index=0;
string_ready=0;

waitState = beginWait;

//initialize variables
dir_ready=0;
k=0;
lat1=0;
lon1=0;
lat1_deg=0;
lon1_deg=0;
dlon=0;
target_dir=0;
lat2 = 0;
lon2= 0;
compass_done=0;
sound_dir=0;
rx_transmission_flag=0;

//reference:
//ithaca = 42deg 26' 26"N, 76deg 29' 48"W = 42.4406N, 76.4967W

//setup LCD
init_lcd();
CopyStringtoLCD(locations[0], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[1], 0, 1);//start at char=0 line=0

while(PINB!=167){
checkButtons();
}
degreesToSound((int)input);

// turn on all ISRs
sei() ;
}

//sets parameters of PWMs
void degreesToSound(int degrees){
if(degrees>=270){
amplitudeL = calculateAmplitude(0);
amplitudeR = calculateAmplitude(degrees);
offsetL = calculateDelay(0);
offsetR = calculateDelay(degrees);
}
else if(degrees>=180){
amplitudeL = calculateAmplitude(0);
amplitudeR = calculateAmplitude(degrees);
offsetL = calculateDelay(0);
//this should be min
offsetR = calculateDelay(degrees);
}
else if(degrees>=90){
amplitudeL = calculateAmplitude(degrees);
amplitudeR = calculateAmplitude(0);
//this should be min
offsetL = calculateDelay(degrees);
offsetR = calculateDelay(0);
}
else{
amplitudeL = calculateAmplitude(degrees);
amplitudeR = calculateAmplitude(0);
offsetL = calculateDelay(degrees);
offsetR = calculateDelay(0);
}
}

char calculateAmplitude(int degree){
//min
if(degree < 70)
return amplitudeIncrementTable[degree];
else if(degree>290)
return amplitudeIncrementTable[360-degree];

//min amp values
else if(degree>=270)
return amplitudeIncrementTable[70];
else if(degree >= 70)
return amplitudeIncrementTable[70];
else
return 255;
}

int calculateDelay(int degree){

if(degree<=90)
return delayIncrementTable[degree];
else if(degree>=270)
return delayIncrementTable[360-degree];
else
return MAXOFFSET;
}

// LCD setup
void init_lcd(void)
{
LCDinit(); //initialize the display
LCDcursorOFF();
LCDclr(); //clear the display
LCDGotoXY(0,0);
// CopyStringtoLCD(LCD_initialize, 0, 0);
}

void checkButtons(){
if((PINB==151)&&locationIndex>0){
locationIndex--;
printLocation();
}
else if((PINB==247)&&locationIndex<4){
locationIndex++;
printLocation();
}
_delay_ms(60);
}

void printLocation(){
switch(locationIndex){
case 0://Eng Quad
LCDclr();
CopyStringtoLCD(locations[0], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[1], 0, 1);//start at char=0 line=0
lat2 = 42.444617;
lon2= 76.4834;
break;
case 1://Brb
LCDclr();
CopyStringtoLCD(locations[1], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[2], 0, 1);//start at char=0 line=0
lat2 = 42.448533;
lon2= 76.481217;
break;
case 2://Clock tower
LCDclr();
CopyStringtoLCD(locations[2], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[3], 0, 1);//start at char=0 line=0
lat2 = 42.44746;
lon2= 76.48505;
break;
case 3://CTB
LCDclr();
CopyStringtoLCD(locations[3], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[4], 0, 1);//start at char=0 line=0
lat2 = 42.442417;
lon2= 76.48527;
break;
case 4://Apartment
LCDclr();
CopyStringtoLCD(locations[4], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[5], 0, 1);//start at char=0 line=0
lat2 = 42.442417;
lon2= 76.48527;
break;
case 5:
LCDclr();
CopyStringtoLCD(locations[5], 0, 0);//start at char=0 line=0
LCDGotoXY(0, 1);
CopyStringtoLCD(locations[6], 0, 1);//start at char=0 line=0
break;
default:
break;
}
}

//Bruce is the man