 //gps 3,4
 //mag A4 A5
 //Heading Adjust A2
+//Speed Adjust A1
 
 #include <LiquidCrystal.h>
 #include <SoftwareSerial.h>

 void forward(){
   //Send data
    dataStruct.tar = 20;
    dataStruct.val = SPEED;
    etData.sendData();
    dataStruct.tar = 21;
    dataStruct.val = SPEED;
    etData.sendData();
   ST.motor(RIGHT, SPEED);
   ST.motor(LEFT, SPEED);
 }
 
 void left(){
      //Send data
    dataStruct.tar = 20;
    dataStruct.val = SPEED;
    etData.sendData();
    dataStruct.tar = 21;
    dataStruct.val = TURN_SPEED;
    etData.sendData();
   ST.motor(RIGHT, SPEED);
   ST.motor(LEFT, TURN_SPEED);
 }
 
 void right(){
      //Send data
    dataStruct.tar = 20;
    dataStruct.val = TURN_SPEED;
    etData.sendData();
    dataStruct.tar = 21;
    dataStruct.val = SPEED;
    etData.sendData();
   ST.motor(RIGHT, TURN_SPEED);
   ST.motor(LEFT, SPEED);
 }
 
 float formatDistance(int meters){

float compassHeading;
int mHeadingOffset = 0;
#define HEADING_ADJUST_PIN A2

//Speed control
#define SPEED_ADJUST_PIN A1
int mSpeedCap = 0;

// 2x16 LCD stuff
LiquidCrystal lcd(8, 9, 4, 5, 6, 7); 
