File content as of revision 0:a955f7fabdf5:

#include "mbed.h"


Serial pc(USBTX, USBRX); // tx, rx for PC
Serial mag(p28, p27); // tx,rx for Magnetometer
Serial gps(p9, p10); //tx, rx for GPS
// GPS is 9,10; MAG is 28,27

DigitalOut on(LED1); // running lights
DigitalOut test_led(LED2); // extra for testing
DigitalOut front(p21);  /// Connect LED cathode to pin 21
DigitalOut frontR(p22);
DigitalOut R(p23);
DigitalOut back(p24);
DigitalOut L(p25);
DigitalOut frontL(p26);

float dir_m,dir_t,rel,decl,latitude,longitude,lock,degrees,minutes,uttime,old_lat,old_long;
float san_coop_lat,san_coop_long;
float lat_1,long_1,lat_2,long_2,lat_3,long_3;
int waypoint;
int wait_front,wait_front_cycle;
char ns,ew;
bool there,gps_lock;
float PI = 3.1415926535;

float course_to,gps_t,gps_m;
double meters_to;


/* Instructions:
1) f10 to download to mbed, save it.
2) TeraTerm, serial, port 6 (MBED)
3) Tera Term, setup>serialport>baudrate (921600), setup>terminal>newline (LF) */

int main() {
    //Declarations
    gps.baud(4800);
    pc.baud(9600);
    mag.baud(19200);
    
    pc.printf("Starting...\n");
    
    lat_1=38.983148;
    long_1= -76.484680;
    lat_2=38.984525;
    long_2=-76.483457;
    lat_3=38.984045;
    long_3=-76.484192;
    waypoint=1;
    
    there = 0;
    gps_lock = 0;
    wait_front_cycle=3;
    wait_front=wait_front_cycle;
    
    decl = -11.11; //11.11 WEST
    
    while(1) {
// Read Serial
 //pc.printf("^1");       
        if(mag.readable()) {
            while (mag.getc() != '$'); {                
            //mag.scanf("HCHDT,%f,T*%c%c",&dir,&cs1,&cs2);
            //pc.printf("MAG dir = %f, cs = %c%c\n", dir, cs1,cs2);
            mag.scanf("HCHDT,%f,",&dir_m);
            //pc.printf("_read_mag");
            dir_t=dir_m+decl;
                }
            }                   
 //pc.printf("^2");       
 
        if(gps.readable()){
                gps.getc();}
                
        //pc.printf("_buffer");
                
        if(gps.readable()){
            while (gps.getc()!='$');{
                if(gps.scanf("GPGGA,%f,%f,%c,%f,%c",&uttime, &latitude, &ns, &longitude, &ew))
                {
                     //pc.printf("_read_gps");
                     gps_lock=1;                     
                }       
            }
        }
                            
                
                //pc.printf("_%c",ew);
                                
                degrees = floor(latitude / 100.0);
                minutes = latitude - (degrees * 100.0);
                latitude = degrees + minutes / 60.0;    
                degrees = floor(longitude / 100.0);
                minutes = longitude - (degrees * 100.0);
                longitude = degrees + minutes / 60.0;   
                
                //pc.printf("_b_%f",longitude);    
                if(ns == 'S') {    latitude  *= -1.0;}
                if(ew == 'W') {    longitude *= -1.0;}
                //pc.printf("_a_%f",longitude);            
                
                if(gps_lock==0)
                {
                    longitude=old_long;
                    latitude=old_lat;
                }                         
                        
                old_lat=latitude;
                old_long=longitude;   
                gps_lock=0;  
                
  //pc.printf("^3");      
  
    if(waypoint==1){
        san_coop_lat=lat_1;
        san_coop_long=long_1;
        } else if (waypoint ==2){
            san_coop_lat=lat_2;
            san_coop_long=long_2;
            } else if (waypoint ==3){
                san_coop_lat=lat_3;
                san_coop_long=long_3;
                } 
  
    const double d2r = PI / 180.0;
    const double r2d = 180.0 / PI;
    
       
    double dlat = abs(san_coop_lat - latitude) * d2r;
    double dlong = abs(san_coop_long - longitude) * d2r;
    double x_sign = san_coop_long-longitude;
    double y = sin(dlong) * cos(san_coop_lat * d2r);
    double x = cos(latitude*d2r)*sin(san_coop_lat*d2r) - sin(latitude*d2r)*cos(san_coop_lat*d2r)*cos(dlong);
    
    course_to = atan2(y,x)*r2d; 
   
    if (x_sign<0){
        course_to=-course_to+360;}
    
// calculate range/bearing
/*float deltalat = abs(san_coop_lat - latitude);
float deltalon = abs(san_coop_long - longitude);

float deltalatr = deltalat *(PI/180);
float deltalonr = deltalon *(PI/180);

//float d = sin(deltalatr/2.0);

//float x = pow(d,2.0);
//float y = cos(latitude*(PI/180))*cos(san_coop_lat*(PI/180))*(sin(deltalonr/2.0f)^2.0f); 
*/
float a = pow(sin(dlat/2.0),2.0) + cos(latitude*(PI/180)) * cos(san_coop_lat*(PI/180)) * pow(sin(dlong/2.0),2.0);
float c = 2*atan2(sqrt(a),sqrt(1-a));

float distft = (3956*c)*5280;
//pc.printf("^4");
//course_to = (atan2(sin(-1*deltalonr)*cos(san_coop_lat*(PI/180)),cos(latitude*(PI/180))*sin(san_coop_lat*(PI/180))-sin(latitude*(PI/180))*cos(san_coop_lat*(PI/180))*cos(-1*deltalonr)))*(180/PI);


//course_to = gps.calc_course_to(san_coop_lat,san_coop_long);
//meters_to = gps.calc_dist_to_m(san_coop_lat,san_coop_long);     
rel = course_to-dir_t;

if (rel<0) {
    rel=rel+360;}

if (distft<30){
    there=1;}
//pc.printf("^5");
pc.printf("mag_%f,lat_%f,long_%f,c2_%f,d2_%f,rel_%f,urhere_%d\n",dir_t,latitude,longitude,course_to,distft,rel,there);
//pc.printf("^6");
// LED or Motor
if (rel<10||rel>350) {
        if (wait_front>=wait_front_cycle){
            front = 1;
            wait_front=0;}
        wait_front++;
    }
if (rel>10&&rel<35){
    front = 1;
    frontR =1;}
if (rel>35&&rel<55) {
    frontR=1;}
if (rel>55&&rel<80) {
    frontR=1;
    R=1;}
if (rel>80&&rel<100){
    R=1;}
if (rel>100&&rel<150){
    R=1;
    back=1;}
if (rel>150&&rel<210){
    back=1;}
if (rel>210&&rel<260){
    back=1;
    L=1;}
if(rel>260&&rel<280){
    L=1;}
if(rel>280&&rel<305){
    L=1;
    frontL=1;}
if(rel>305&&rel<325){
    frontL=1;}
if(rel>325&&rel<350){
    frontL=1;
    front=1;}

if(there){
    front = 1;
    frontR = 1;
    R=1;
    back=1;
    L=1;
    frontL=1;  
    wait(0.5);
    
    front = 0;
    frontR = 0;
    R=0;
    back=0;
    L=0;
    frontL=0;
    wait(0.5);
    
    front = 1;
    frontR = 1;
    R=1;
    back=1;
    L=1;
    frontL=1;  
    wait(0.5); 
    
    front = 0;
    frontR = 0;
    R=0;
    back=0;
    L=0;
    frontL=0;
    wait(0.5);
    
    front = 1;
    frontR = 1;
    R=1;
    back=1;
    L=1;
    frontL=1;  
    wait(0.5);     
    
    there = 0;   
    waypoint=waypoint+1;
} 

//pc.printf("^7");
wait(.5);    
front = 0;
frontR = 0;
R=0;
back=0;
L=0;
frontL=0;
//pc.printf("^8");
    }
}