 int available_bytes = 0;
 
 // Target and previous velocity arrays
-static float target_vel[] = {0.f , 0.f};
+static int target_vel[] = {0 , 0};
 
 //Motor Control
 FRCmotor leftMotor; //DECLARE LEFT MOTOR CONTROLLER

//Sonar
#define SONAR_NUM     9 // Number of sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
unsigned int sonars[SONAR_NUM];
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(22, 22, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(23, 23, MAX_DISTANCE),
  NewPing(24, 24, MAX_DISTANCE),
  NewPing(25, 25, MAX_DISTANCE),
  NewPing(26, 26, MAX_DISTANCE),
  NewPing(27, 27, MAX_DISTANCE),
  NewPing(28, 28, MAX_DISTANCE),
  NewPing(29, 29, MAX_DISTANCE),
  NewPing(30, 30, MAX_DISTANCE)
};

//SerialRead/Write
#define BUFSIZE 256
#define SPEED_LIMIT 100.f
#define RAMP_CONST 1 // Higher is faster

const int safesize = BUFSIZE / 2;

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