   memset(write_buffer,'\0',BUFSIZE);
   for(int x = 0; x<SONAR_NUM;x++)
   {
-    sprintf(ftos,"%f",sonars[x]);
+    dtostrf(sonar_value[x] / 50.0 ,20,10,ftos);
     strcat(write_buffer, ftos);
     strcat(write_buffer, ",");
   }
-  sprintf(ftos,"%f",latitude);
+  dtostrf(latitude,20,10,ftos);
   strcat(write_buffer, ftos);
   strcat(write_buffer, ",");
-  sprintf(ftos,"%f",longitude);
+  dtostrf(longitude,20,10,ftos);
   strcat(write_buffer, ftos);
   strcat(write_buffer, ",");
-  sprintf(ftos,"%f",compass_theta);
+  dtostrf(heading,20,10,ftos);
   strcat(write_buffer, ftos);
   strcat(write_buffer, "\n");
   Serial.write(write_buffer);

  
  ss.begin(GPSBaud);

  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop()
{
  readSerial();
  moveMotor();
  getGPS();
  getSonars();
  getCompass();
  writeSerial();
}

void readSerial()


void moveMotor()
{
  leftMotor.Set(-1*target_vel[0]);
  rightMotor.Set(target_vel[1]);
  //Serial.print(target_vel[0]);
  //Serial.println(target_vel[1]);
}

void getGPS(){
