/*********************************************************************/
/* Project: Eric                                                                                             
/* Group members: Karthik Kesavan,Hieu Nguyen, & Mark Riegert                       
/* Course: CENG 4636
/* Date: May 12, 2003
/* Assignment: Software Module to control Eric
/* Environment:MPLab using PIC C Compiler
/* Files Included: EricSpring2003.c
/* Purpose: Calculate direction and distance to control
/* Eric using realtime GPS, and Analog
/* Compass singnal.
/* Input: Serial ASCII character from GPS at 9600 bps
/* 2 Analog signal from analog compass
/* Preconditions: none
/* OutPut: 4 TTL signal to control Eric movement
/* Serial ASCII character to LCD
/* Postconditions: none
/* Algorithm:
/* - set up interupt and analog to digital condition
/* - while waypoint is not the last waypoint
/* - get current waypoint
/* - calculating distance and direction
/* - if distance is less than 3 meter get next
/* waypoint
/* - while current direction is not within 3 degree
/* of target direction
/* - if current direction is greater than target
/* direction -> turn right
/* - if current direction is less than target
/* direction -> turn left
/* - end direction while loop
/* - go straight
/* - end waypoint while loop
/* - end program
/* Error: none
/* Example: none
/* History: none
/********************************************************************/






/********************************************************************/
#include <16f877.h> // pin definitions and internal variable definition
//Device Specification. *=16 defines 16 bit pointers
#device PIC16f877 *=16 ADC=10

#include<STDLIB.H> // Definitions for printf(), putc(), getc(), etc.
#include<math.h> // Definitions for arctan2
// Define the fuses
#fuses XT, NOWDT,NOPROTECT,NOBROWNOUT,NOPUT
// Define the clock speed being used
#use delay(clock=4000000)
//Define the rs232 connection variables
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7,ERRORS)


#byte PORTB = 6 //Define the location on Port B
#define NUMOFCHAR 39 //Nu. of character to received
/********************************************************************/

int gpsdataerror; //Error ocurred variable
int STARTCOLLECTING; //variable to check the incoming statement
int ARRAYINDEX; //Index variable for the array
int i = 0; //counter

float currentlatitude; //variable to hold current latitude
float currentlongitude; //variable to hold current longitude/*29.577970033.8132931937*/
float currentangle; //variable to hold current angle/*117.9010931937/95.1033067*/
float Difangle; //variable to hold different angle between current angle and target angle
float GPSAngle; //variable to hold GPS angle

//Array to hold future latitudes
float const futurelongitude[4] =
{95.10365833,95.10360667,117.9010931937,117.9012530541};
float const futurelatitude[4] = {29.57809833, 29.57804, 33.8132132635,
33.8132132635};

byte character; //variable to hold incoming character
byte DATAREC[NUMOFCHAR]; //array to hold incoming characters
void stop();
/*********************************************************************
*PURPOSE: To service the RS232 interrupt
*INPUT: None
*PRECONDITIONS: Start connection must be called into order for
the interrupts to be serviced thru this routine
*********************************************************************/
//RS232 receive data available
#INT_RDA
serial_isr()
{
character = getc(); //store the received character in variable 'character'
if(character == '$' && STARTCOLLECTING == 0) //check for $ and 0 on STARTCOLLECTING
STARTCOLLECTING = 1;
if(character == 'G' && STARTCOLLECTING == 1) //check for G and 1 on STARTCOLLECTING
STARTCOLLECTING = 2;


if(STARTCOLLECTING == 2) //if STARTCOLLECTING is 2 store character
{
DATAREC[ARRAYINDEX] = character; //Store Character @ position ARRAYINDEX in DATAREC
ARRAYINDEX++; //Increment ARRAYINDEX

if (ARRAYINDEX == 14 && DATAREC[13] != 'A') //Check the validity of the data received
{
STARTCOLLECTING = 0; //if data is not valid then clear the STARTCOLLECTING variable
ARRAYINDEX = 0; //reset ARRAYINDEX to 0
}
//else if(DATAREC[13] == 'V')
// stop();
}

if(ARRAYINDEX == NUMOFCHAR && (DATAREC[13] == 'A')) //check if all elements have been received if true
disable_interrupts(INT_RDA); //disable the RS232 Interrupt
}

/*********************************************************************
*PURPOSE: To convert the ASCII character array into float values
for latitude and longitude
*INPUT: None
*PRECONDITIONS: Character Array must be initialized before executing
this function
*********************************************************************/
//#SEPARATE
void getlatitude()
{
int a;
int degree, latlong = 2;
float minute;

do
{
degree = 0; //clear the degree variable
if(latlong == 2) //check if latlong is 2 if true
a = 15; //set the array index to 15
else //if latlong is not 2 then
{
a = 28; //set the array index to start @ 28
degree = ((DATAREC[27]) - 48)*100; //convert the 28th character into integer
//and Multiply by 100
}
// sequence to conver degree and minute into degree
degree = degree + ((DATAREC[a]) - 48)*10;
degree = degree + ((DATAREC[a+1]) - 48);

minute = (((DATAREC[a+2]) - 48) * 10.0);
minute = minute + ((DATAREC[a+3]) - 48);
minute = minute + (((DATAREC[a+5]) - 48) / 10.0);
minute = minute + (((DATAREC[a+6]) - 48) / 100.0);
minute = minute + (((DATAREC[a+7]) - 48) / 1000.0);
minute = minute + (((DATAREC[a+8]) - 48) / 10000.0);
minute = minute / 60;

if(latlong == 2) //latlong is 2 then set the collected degree + minute to latitude
{
currentlatitude = (float)degree + minute;
}
else //if latlong is not 2 then set the collected degree + minute to longitude
currentlongitude = (float)degree + minute;

latlong--; //in any case decrement the latlong variable
//so that next time around the current longitude is chosen
}while(latlong >= 1);
}

/*********************************************************************
*PURPOSE: To convert the analog signal at Pin A1 and A2 into
digital values
*INPUT: analog signal from analog compass
*PRECONDITIONS: None
*OUTPUT: set variable currentangle equal to current angle direction
*********************************************************************/
//#SEPARATE
void AnalogToDigital()
{
float port1,port2; // hold variable for analog value
float var1,var2,var3;

set_adc_channel( 0 ); // set port A0 for analog input
delay_us(10); // delay 10 micro second for port setup
port1 = (float)Read_ADC(); // move analog value to port1 variable
set_adc_channel( 1 ); // set port A1 for analog input
delay_us(10); // delay 10 micro second for port setup
port2 = (float)Read_ADC(); // move analog value to port2 variable

// var1, var2, var3 hold invert sine and cosine conversion value for case calculation
// for var1 the value (1680-(port1-458)*10)/168.0) is to make up for the inconsistenced
var1 = asin((port1-545+(1680-(port1-458)*10)/168.0)/110.3);
var2 = PI - asin((port1-545)/110.3);
var3 = acos((port2-545)/110.3);

if(port1 <= 626 && port2 >= 626) // case angle between 315-45 degree
{
currentangle = var1;
if(currentangle < 0)
{
currentangle += 2 * PI;
}
}
else if(port1 >= 458 && port2 <= 458) // case angle between 135-225 degree
{
currentangle = var2;
}
else if(port2 <= 626 && port1 >= 626) // case angle between 45-135 degree
{
currentangle = var3;
}
else if(port2 >= 458 && port1 <= 458) // case angle between 225-315 degree
{
currentangle = (2 * PI) - var3;
}
else if(port2 > 545) // make up for extreme data of port1
{
currentangle = var1;
if(currentangle < 0)
{
currentangle += 2 * PI;
}
}
else if(port2 < 545) // make up for extreme data of port2
{
currentangle = var2;
}
// convert from radian to degree
currentangle = 360 - (currentangle * (180.0/PI));
}

/*********************************************************************
*PURPOSE: To set up the interrupts for RS232 connection
*INPUT: None
*PRECONDITIONS: Global interrupt must be enabled
*********************************************************************/
//#SEPARATE
void startconnection()
{
STARTCOLLECTING = 0;
ARRAYINDEX = 0;

enable_interrupts(INT_RDA); // enable serial RS232 input

while (ARRAYINDEX<NUMOFCHAR) // wait for data
{ }

}

/*********************************************************************
*PURPOSE: tell Eric to go forward
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set portB pin 0-3 as '1010' print forward in LCD
*********************************************************************/
//#SEPARATE
void forward()
{
output_high (PIN_B0);
output_low (PIN_B1);
output_high (PIN_B2);
output_low (PIN_B3);
printf(" forward");
}

/*********************************************************************
*PURPOSE: tell Eric to stop
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set portB pin 0-3 as '0000' print step in LCD
*********************************************************************/
//#SEPARATE
void stop()
{
output_low (PIN_B0);
output_low (PIN_B1);
output_low (PIN_B2);
output_low (PIN_B3);
printf(" stop");
}

/*********************************************************************
*PURPOSE: tell Eric to backward
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set portB pin 0-3 as '0101' print backward in Lcd
*********************************************************************/
//#SEPARATE
/*void backward()
{
output_low (PIN_B0);
output_high (PIN_B1);
output_low (PIN_B2);
output_high (PIN_B3);
printf(" backward");
}*/

/*********************************************************************
*PURPOSE: tell Eric to turn right
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set portB pin 0-3 as '0110' print right in LCD
*********************************************************************/
//#SEPARATE
void right()
{
output_low (PIN_B0);
output_high (PIN_B1);
output_high (PIN_B2);
output_low (PIN_B3);
printf(" right");
}

/*********************************************************************
*PURPOSE: tell Eric to turn left
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set portB pin 0-3 as '1001' print left in LCD
*********************************************************************/
//#SEPARATE
void left()
{
output_high (PIN_B0);
output_low (PIN_B1);
output_low (PIN_B2);
output_high (PIN_B3);
printf(" left");
}

/*********************************************************************
*PURPOSE: get target latitude and longitude and calculate
* GPS angle, and distance
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: set GPSAngle as the angle of target direction
*********************************************************************/
//#SEPARATE
void GetAngle(int ind)
{
// get distance between current and future latitude
currentlatitude = (futurelatitude[ind] - currentlatitude);
// get distance between current and future longitude
currentlongitude = (currentlongitude - futurelongitude[ind]);
// get GPS angle
GPSAngle = atan2(currentlatitude,currentlongitude) * 57.2958;
if(GPSAngle < 0)
{
GPSAngle += 360;
}
}

/*********************************************************************
*PURPOSE: check to see if Eric is within 3 meter from target
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: return 1 if within 3 meter from target, else return 0
*********************************************************************/
//#SEPARATE
int checklocation()
{
if((abs(currentlatitude) <= .00005) && (abs(currentlongitude) <= .00005))
{
stop();
i++;
printf("\fI AM HERE!!!\r\n");
// process to get next waypoint if more than 1 waypoint is present
// startconnection(); // enable interupt
// getlatitude();
// printf("L %f\r\n",currentlatitude);
// printf("LO %f\r\n",currentlongitude);
// GetAngle(i);
// printf("GA %f\r\n", GPSAngle);

return 1;
}
else
return 0;
}

/*********************************************************************
*PURPOSE: control turn and movement of Eric
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: align Eric to target direction and go forward
*********************************************************************/
//#SEPARATE
void aligntogpsangle()
{
do
{
AnalogToDigital(); // get current direction of Eric

// out put to LCD current latitude and longitude
// GPS angle
// current angle
printf("\f");
printf("LA: %f\n\r",currentlatitude);
printf("LO: %f\n\r",currentlongitude);
printf("GA: %5.2f\n\r", GPSAngle);
printf("CA: %5.2f", currentangle);

// get the different between GPS angle and current angle
Difangle = GPSAngle - currentangle;
// sequence to select the shortest path to turn
if(Difangle < -180)
{
Difangle += 360;
}
else if(Difangle > 180)
{
Difangle -= 360;
}
if(Difangle > 3) // if different angle is > 3 turn left to align
{
left(); // call turn left
}
else if(Difangle < -3) // if different angle is < -3 turn right to align
{
right(); // call turn right
}

} while(Difangle > 3 || Difangle < -3);

// out put to LCD current latitude and longitude
// GPS angle
// current angle
printf("\f");
printf("LA: %f\n\r",currentlatitude);
printf("LO: %f\n\r",currentlongitude);
printf("GA: %5.2f\n\r", GPSAngle);
printf("CA: %5.2f", currentangle);
delay_ms(100);
forward();// call go straight

}

/*********************************************************************
*PURPOSE: main control and check up for Eric
*INPUT: none
*PRECONDITIONS: none
*OUTPUT: none
*********************************************************************/
//#SEPARATE
main()
{

set_tris_b(0xF0); //B3,B2,B1,B0 are outputs & B4 & B5 are inputs
output_low (PIN_B0); // reset B3,B2,B1,B0
output_low (PIN_B1);
output_low (PIN_B2);
output_low (PIN_B3);

printf("\f"); // set control to clear LCD
printf("start"); // print start to LCD
delay_ms(5000); // wait 5000 micro second

enable_interrupts(GLOBAL); // enable RS232 interupt

// set up analog to digital as A0,A1 and refferent in A2,A3
setup_port_a(RA0_RA1_ANALOG_RA3_RA2_REF);
// set up analog clock
setup_adc(ADC_CLOCK_INTERNAL);

// in case of 2 waypoint set i<2
// if 3 waypoint set i<3 and for 4 waypoint set i<4
while(i<2)
{
startconnection(); // enable interupt
getlatitude(); // convert GPS ASCII charater to float value

// out put to LCD current latitude and longitude
// GPS angle
// current angle
printf("\f");
printf("LA: %f\n\r",currentlatitude);
printf("LO: %f\n\r",currentlongitude);
GetAngle(i);
printf("GA: %5.2f\n\r", GPSAngle);
printf("CA: %5.2f", currentangle);
printf("\f");

if(checklocation() == 0) // check location of Eric
{
aligntogpsangle(); // align Eric to target direction
delay_ms(5000); // go forward for 5000 micro second
}

}
// Eric reached all waypoint then stop
printf(" ****** ");
}