///////////////////////////////////////////////////////////////////////
//Summer 2002 - Senior Project
//CODE for the ERIC 3.0
//File Name: ERIC4.c
//Group Members:
//////////////////////////////////////////////////////////////////////


// Preprocessor Directives
#pragma chip PIC16F877
// n Bits of PortD are assigned as variable "dn"

bit d0 @ PORTD.0;
bit d1 @ PORTD.1;
bit d2 @ PORTD.2;
bit d3 @ PORTD.3;
bit d4 @ PORTD.4;
bit d5 @ PORTD.5;
bit d6 @ PORTD.6;
bit d7 @ PORTD.7;


// n Bits of PortA are assigned as variable "an"

bit a0 @ PORTA.0;
bit a1 @ PORTA.1;
bit a2 @ PORTA.2;
bit a3 @ PORTA.3;
bit a4 @ PORTA.4;
bit a5 @ PORTA.5;

// n bits of portB are assigned as variable "bn".

bit b0 @ PORTB.0;
bit b1 @ PORTB.1;
bit b2 @ PORTB.2;
bit b3 @ PORTB.3;
bit b4 @ PORTB.4;
bit b5 @ PORTB.5;
bit b6 @ PORTB.6;
bit b7 @ PORTB.7;

//n bits of portC are assigned as variable "cn"
bit c0 @ PORTC.0;
bit c1 @ PORTC.1;
bit c2 @ PORTC.2;
bit c3 @ PORTC.3;
bit c4 @ PORTC.4;
bit c5 @ PORTC.5;
bit c6 @ PORTC.6;
bit c7 @ PORTC.7;


//variable declaration
int present_latitude ;
int present_longitude;
int goal_latitude ;
int goal_longitude ;


//////////////////////////////////////////////////////////////////////////////////////
//Function for delay_ms(n);
//Purpose: To set up delays as a multiple of 1 milliseconds at 4 MHz
// using the TMR0 timer.
//Input: Integers in millisec
//Output: Delays set in millisec
/////////////////////////////////////////////////////////////////////////////
void delay_ms( uns16 millisec)
{
char next = 0;

OPTION = 2; // prescaler divide TMR0 rate by 8
TMR0 = 2; // deduct 2*8 fixed instruction cycles delay
do {
next += 125;
clrwdt(); // needed only if watchdog is enabled
while (TMR0 != next) // 125 * 8 = 1000 (= 1 ms)
;
} while ( -- millisec != 0);
}
////////////////////
//
//
//
//
//////////////////
int digital_compass(void)
{
int heading;
int positionA ;
int positionB ;
int countd0;
int countd1 ;
int countd2 ;
int countd3;
int countd4;
int countd5;
int countd6;
int countd7;

int countc0;
int countc1 ;
int countc2 ;
int countc3;
int countc4;
int countc5;
int countc6;
int countc7;

positionA = 0;
positionB = 0;

if (d0 == 0) // read portd
countd0 = 0;
else
countd0 = 1;
if (d1 == 0)
countd1 = 0;
else
countd1 = 2;
if (d2 == 0)
countd2 = 0;
else
countd2 = 4;
if (d3 == 0)
countd3 = 0;
else
countd3 = 8;
if (d4 == 0)
countd4 = 0;
else
countd4 = 16;
if (d5 == 0)
countd5 = 0;
else
countd5 = 32;
if (d6 == 0)
countd6 = 0;
else
countd6 = 64;
if (d7 == 0)
countd7 = 0;
else
countd7 = 128;


if (c0 == 0) //read portc
countc0 = 0;
else
countc0 = 1;
if (c1 == 0)
countc1 = 0;
else
countc1 = 2;
if (c2 == 0)
countc2 = 0;
else
countc2 = 4;
if (c3 == 0)
countc3 = 0;
else
countc3 = 8;
if (c4 == 0)
countc4 = 0;
else
countc4 = 16;
if (c5 == 0)
countc5 = 0;
else
countc5 = 32;
if (c6 == 0)
countc6 = 0;
else
countc6 = 64;
if (c7 == 0)
countc7 = 0;
else
countc7 = 128;

positionA = countd0 + countd1 + countd2 + countd3 + countd4 +
countd5 + countd6 + countd7;

positionB = countc0 + countc1 + countc2 + countc3 + countc4 +
countc5 + countc6 + countc7;

heading = 0;

if (positionB > 143)
heading = 12 + 143 - positionA;
if (positionA < 119)
heading = 36 + 143 - positionB;
if (positionB < 119)
heading = 60 + positionA -119 ;
if (positionA > 143)
heading = 84 + positionB - 119;

return heading;

}

/*
int determine_angle(float dir)
{
int a;

return a;

} */

/////////////////////////////////////////////////////////////////////////////////////
//Function left();
//Purpose: To make the car turn left.
//Input: No parameter is set.
//Output: ////////////////////////////////////////////////////////////////////////////////////
void left(void)
{

b0 = 1;// reverse left wheel
b1 = 0;
b4 = 0;// forward right wheel
b5 = 1;


delay_ms(26);

}

/////////////////////////////////////////////////////////////////////////////////////
//Function right();
//Purpose: To turn the wheel to the right.
//Input: No parameter is set.
//Output: ////////////////////////////////////////////////////////////////////////////////////
void right(void)
{
b0 = 0;// forward left wheel
b1 = 1;
b4 = 1;// reverse right wheel
b5 = 0;

delay_ms(26);

}
//////////////////////////////////////////////
//
//
//
////////////////////////////////////////////////
void forward(void)
{
b0 = 0;
b1 = 1;
b4 = 0;
b5 = 1;

delay_ms(26);
}

////////////////////////////////////////////////////////////////////
//
//
//
//
//
////////////////////////////////////////////////////////////////////


int calculate_position(void)
{
//using port D for input of lat and long

int position ;
int count1 ;
int count2 ;
int count3;
int count4;
int count5;
int count6;
int count7;

int count0;

if (d0 == 0)
count0 = 0;
else
count0 = 1;
if (d1 == 0)
count1 = 0;
else
count1 = 2;
if (d2 == 0)
count2 = 0;
else
count2 = 4;
if (d3 == 0)
count3 = 0;
else
count3 = 8;
if (d4 == 0)
count4 = 0;
else
count4 = 16;
if (d5 == 0)
count5 = 0;
else
count5 = 32;
if (d6 == 0)
count6 = 0;
else
count6 = 64;
if (d7 == 0)
count7 = 0;
else
count7 = 128;

position = count0 + count1 + count2 + count3 + count4 + count5 + count6 + count7;

return position;

}

//////////////////////////////////////
//
//
//
//
//
//
///////////////////////////////////////////

/*

void Input_position(void)
{

int set1 = 0;

while(set1 != 1)
{
set1 = b2;
}

present_latitude = calculate_position();

set1 = 0;

while(set1 != 1) //remember to flip b2 back to 0 then flip b3 to 1 after input set up
{
set1 = b3;
}

goal_latitude = calculate_position();

set1 = 0;

while(set1 != 1)//remember to flip b3 back to 0 then b2 to 1 after input set up
{
set1 = b2;
}

set1 = 0;

goal_longitude = calculate_position();

while(set1 != 1)
{
set1 = b3;
}
present_longitude = calculate_position();

} */

//////////////////////////////////////////////
//
//
//
//
//
//////////////////////////////////////////////////////////////

/* int Calculate_Heading(void)
{
int delta_latitude;
int delta_longitude;
int heading;
int angle;
float direction;
delta_latitude = goal_latitude - present_latitude;
delta_longitude = present_longitude - goal_longitude;

if (delta_latitude > 0 && delta_longitude > 0)
{
direction = (uns8)delta_latitude / delta_longitude;
angle = determine_angle(direction);
heading = 90 - angle;
}
if (delta_latitude > 0 && delta_longitude < 0)
{
direction = (uns8)delta_latitude / delta_longitude;
angle = determine_angle(direction);
heading = 270 + angle;
}
if (delta_latitude < 0 && delta_longitude > 0)
{
direction = (uns8)delta_latitude / delta_longitude;
angle = determine_angle(direction);
heading = 90 + angle;
}
if (delta_latitude < 0 && delta_longitude < 0)
{
direction = (uns8)delta_latitude / delta_longitude;
angle = determine_angle(direction);
heading = 270 - angle;
}
return heading;

} */

//////////////////////////////////////////////////////////////////////////////////
//
//
//
//
//
/////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
//
//
//
//
//
/////////////////////////////////////////////////////////////////////////////////

void update_heading( int heading)
{
int present_heading;
int heading1;
int heading2;
int t;

t = 1;

heading1 = heading + 2;
heading2 = heading - 2;

while(t == 1)
{
present_heading = digital_compass();

if (present_heading > heading2 && present_heading < heading1)
break;

else
right();
}
}
/////////////////////////////////////////////////////////////
//
//
//
/////////////////////////////////////////////////////////////

/*
void Test_Compass2(void)
{
int16 heading;
int test;
test = 1;
while( test == 1)
{
heading = digital_compass();
if ( heading == 0 || (heading > 0 && heading < 12))
right();
if (heading == 12 || (heading > 12 && heading < 24))
left();
if (heading == 24 || (heading > 24 && heading < 36))
forward();
if (heading == 36 || (heading > 36 && heading < 48))
right();
if (heading == 48 || (heading > 48 && heading < 60))
left();
if (heading == 60 || (heading > 60 && heading < 72))
forward();
if (heading == 72 || (heading > 72 && heading < 84))
right();
if (heading == 84 || (heading > 84 && heading < 96))
left();


}
}*/

////////////////////////////////////////////////////////////////
//
//
//
//
//
/////////////////////////////////////////////////////////////////

void continue_to_goal(int heading)
{


int d = 1;
int present_heading;
int heading1;
int heading2;


heading1 = heading + 2;
heading2 = heading - 2;

while( d == 1)
{
present_heading = digital_compass();

if (present_heading > heading2 && present_heading < heading1)
forward();
if (heading > present_heading)
right();
if (heading < present_heading)
left();

}

}
///////////////////////////////////////////////////////////////////
//
//
//
//
//

/////////////////////////////////////////////////////////////////////////////
//Function: main()
////////////////////////////////////////////////////////////////////////////
void main(void)
{

int heading;
int set1;

//ADCON1 = 0b0110;

//Bits of PortA are set to input.
PORTA = 0b.0000.0000;
TRISA = 0b.1111.1111;

//Bits of PortD are set as the inputs.
PORTD = 0b0000.0000;
TRISD = 0b1111.1111;

//Bits of PortB except b2 7 b3 are set to output
PORTB = 0b0000.0000;
TRISB = 0b0000.1100;

//Bits of PortC are set to input
PORTC = 0b0000.0000;
TRISC = 0b1111.1111;

//Input_position();

//heading = Calculate_Heading();
heading = 55;// Calculate_Heading_Test();
// Test_Compass2();
update_heading(heading);
continue_to_goal(heading);
} // ending main()