#include "main.h"
  #include <stdlib.h>
  #include <math.h>
  #include <string.h>
  #include "globals.c"
  #include "waypoints.c"
  #include "prototypes.c"
  #include "gps.c"
  #include "interrupts.c"
  #include "query_base.c"
  #include "roboteq.c"
  #include "TCMcompass.c"
  #include "report_base.c"
  #include "calc_heading.c"
  #include "calc_distance.c"
  #include "check_threshold.c"
  #include "control_heading.c"
  #include "control_velocity.c"

  


/**
  * This function just says hello during initial boot up
  *
**/
void test_ports()
{
fprintf(BASESTREAM, "HELLO BASESTREAM\r\n");
fprintf(COMPSTREAM, "HELLO COMPSTREAM\r\n");
fprintf(GPSSTREAM,  "HELLO GPS\r\n");
fprintf(ROBOSTREAM, "HELLO ROBOSTREAM\r\n");
}
/**
  * This is the main entry point where the program begins
  *
**/
void main()
{
   //wait for capacitors to charge / ics to start etc...
   delay_ms(500);
   //Setup watch dog timer
   setup_wdt(WDT_ON);
   //test ports for hardware debugging
   test_ports();
   //enable interrupts
   enable_interrupts(INT_RDA);
   enable_interrupts(GLOBAL);
   //make sure we're stopped at startup! - Kill Mode
   PARAMS.mode = 'K';
   //Set Current waypoint
   WAYPOINTS.current=0;
   // Main Loop
   WHILE(1)
   {
      
         query_base();
         update_compass();
         parse_gps();
         calc_distance();
         calc_heading();
         check_threshold();
         control_heading();    
         control_velocity();
         update_robo();
         query_robo();
         report_base();

   }
   
}