geocachingGame

#include "mbed.h"
#include "SDFileSystem.h"
#include "wave_player.h"
#include "uLCD_4DGL.h"
#include "rtos.h"
#include <algorithm>

uLCD_4DGL uLCD(p28, p27, p29);

Serial gps(p9,p10);

DigitalOut led(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

SDFileSystem sd(p11, p12, p13, p8, "sd");
AnalogOut DACout(p18);
wave_player waver(&DACout);

char byteGPS;
char linea[300] = "";
char comandoGPR[7] = "$GPRMC";
int countB=0;
int good=0;
int countA=0;
int indices[13];

char latStr[9] = "";
float latFloat = 0.0f;
char lonStr[9] = "";
float lonFloat = 0.0f;

float lat[3];       //Can add more locations if desired
float lon[3];      //Can add more locations if desired

int locCount = 0;

float latFloatT = 8423.4753;
float lonFloatT = 3346.2227;

float diffLon = 10.0f;
float diffLat = 10.0f;

float margin = 0.0020f;

int locCircX = 0;
int locCircY = 0;


void lcd_createMap(){               //Initialize/Reset Screen

    uLCD.circle(63,63,10,RED);
    uLCD.line(93,63,33,63,RED);
    uLCD.line(63,93,63,33,RED);
    
    FILE *wave_file;
    wave_file=fopen("/sd/Loading.wav","r");
    waver.play(wave_file);
    fclose(wave_file);
    
    wait(.5);
    
    uLCD.circle(63,63,10,BLACK);
    uLCD.line(93,63,33,63,BLACK);
    uLCD.line(63,93,63,33,BLACK);
    
    uLCD.circle(63,63,4,RED);
    uLCD.line(73,63,53,63,RED);
    uLCD.line(63,73,63,53,RED);
    uLCD.color(BLUE);
    uLCD.locate(9,0);
    uLCD.putc('N');
    uLCD.locate(9,15);
    uLCD.putc('S');
    uLCD.locate(0,7);
    uLCD.putc('W');
    uLCD.locate(17,7);
    uLCD.putc('E');
       
    wave_file=fopen("/sd/Proceed.wav","r");
    waver.play(wave_file);
    fclose(wave_file);
}

void reachedPoint(){            //Called when location is reached
    if(locCount<2){           //Change if using a different number of coordinates
        locCount++;
    }else{
        locCount=0;
    }
    
    uLCD.cls();
    uLCD.circle(63,63,10,RED);
    uLCD.line(93,63,33,63,RED);
    uLCD.line(63,93,63,33,RED);    
    
    FILE *wave_file;
    wave_file=fopen("/sd/Success.wav","r");
    waver.play(wave_file);
    fclose(wave_file);
    
    wait(.5);
    
    lcd_createMap();
} 

void checkPoint(){                      //Checks to see if location is reached
    if(abs(diffLon)<=margin && abs(diffLat)<=margin){
        reachedPoint();
    }else{
    }
}

void lcd_thread() {                     //Updates navigation screen
    
    uLCD.filled_circle(locCircX,locCircY,2,BLACK);
    uLCD.line(63,12,locCircX,12,BLACK);
    uLCD.line(12,63,12,locCircY,BLACK);
    uLCD.circle(63,63,4,RED);
    uLCD.line(73,63,53,63,RED);
    uLCD.line(63,73,63,53,RED);
    diffLon = lon[locCount]-lonFloat;
    diffLat = lat[locCount]-latFloat;
    
    
    
    locCircX = 63-480.0f*diffLon;
    locCircY = 63-480.0f*diffLat;
    
    if(locCircX<20){
        locCircX=20;
    }else if(locCircX>100){
        locCircX=100;
    }
    if(locCircY<20){
        locCircY=20;
    }else if(locCircY>100){
        locCircY=100;
    }        

    uLCD.line(63,12,locCircX,12,GREEN);
    uLCD.filled_circle(locCircX,locCircY,2,GREEN);
    uLCD.line(12,63,12,locCircY,GREEN);
    
    checkPoint();
}

void gps_thread(void const *args) {             //Gets GPS Data
    
    gps.baud(4800);
    //pc.baud(115200);
    
    for (int i=0;i<300;i++){       // Initialize a buffer for received data
        linea[i]=' ';
    }
    for (int i=0;i<9;i++){       // Initialize a buffer for received data
        latStr[i]=' ';
    }
    for (int i=0;i<9;i++){       // Initialize a buffer for received data
        lonStr[i]=' ';
    }
    
    
    while(1){        
        
        //Get GPS Data
        
        byteGPS = gps.getc();         // Read a byte of the serial port            
        linea[countA]=byteGPS;        // If there is serial port data, it is put in the buffer
        countA++;             
            
        if (byteGPS=='\r'){         // If the received byte is = to 13, end of transmission
                                    // note: the actual end of transmission is <CR><LF> (i.e. 0x13 0x10)
            countB=0;
            good=0;
            // First byte is the <LF> (0x10) from the previous transmission.
            for (int i=1;i<7;i++){     // Verifies if the received command starts with $GPR  
                if (linea[i]==comandoGPR[i-1]){
                    good++;
                }
            }
                
            if(good==6){               // If yes, continue and process the data
                for (int i=0;i<300;i++){
                    if (linea[i]==','){
                        indices[countB]=i;
                        countB++;
                    }
                    if (linea[i]=='*'){    // ... and the "*"
                        indices[12]=i;
                        countB++;
                    }
                }
                    
                //Get latitude value
                int i=2;
                for (int j=indices[i];j<(indices[i+1]-1);j++){
                    latStr[j-indices[i]] = linea[j+1];
                }
                latFloat = atof(latStr);                   
                        
                //Get longitude value
                i=4;
                for (int j=indices[i];j<(indices[i+1]-1);j++){
                    lonStr[j-indices[i]] = linea[j+1];
                }
                lonFloat = atof(lonStr);

                lcd_thread();                    
            }
            countA=0;                    // Reset the buffer
            for (int i=0;i<300;i++){
                linea[i]=' ';             
            }               
        }
    }
}


int main() {
    // EDIT COORDINATES HERE
    lat[0] = 3346.2283;
    lon[0] = 8423.5186;
    lat[1] = 3346.2340;
    lon[1] = 8423.4885;
    lat[2] = 3346.2173;
    lon[2] = 8423.4818;
    // END EDIT COORDINATES
    
    led = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    
    uLCD.baudrate(BAUD_3000000);
    
    lcd_createMap();
    
    Thread tGPS(gps_thread, (void *)"Thread 1");        //GPS thread started
    
    while(1)
    {
        led=!led;
        Thread::wait(500);
        
    }
}


int main() {
    // EDIT COORDINATES HERE
    lat[0] = 3346.2283;
    lon[0] = 8423.5186;
    lat[1] = 3346.2340;
    lon[1] = 8423.4885;
    lat[2] = 3346.2173;
    lon[2] = 8423.4818;
    // END EDIT COORDINATES
    
    led = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    
    uLCD.baudrate(BAUD_3000000);
    
    lcd_createMap();
    
    Thread tGPS(gps_thread, (void *)"Thread 1");        //GPS thread started
    
    while(1)
    {
        led=!led;
        Thread::wait(500);
        
    }
}