   delayMicroseconds(10);
   digitalWrite(23, LOW);
 
-  duration = pulseIn(22,HIGH,1740);   // 1740 uS => 30 cm
-  distacne_cm = duration / 58;
+  long duration = pulseIn(22,HIGH,1740);   // 1740 uS => 30 cm
+  long distance_cm = duration / 58;
 
-  if (distacne_cm > 0 && distacne_cm < 15) {
-    return 0;
+  if (distance_cm > 0 && distance_cm < 15) {
+    return 0.0;
   }
-  else if (distacne_cm < 30) {
-    return (distacne_cm-15)/15;
+  else if (distance_cm > 0 && distance_cm < 30) {
+    return (float) (distance_cm-15)/15.0;
   } 
   else {
-    return 1;
+    return 1.0;
   }
 }
 

  int rot = (int)(twist_msg.angular.z * 255.0);
  lspd -= rot;
  rspd += rot;

  dist_factor = (int) ultrasonic();
  
  if (rspd > 255)
    rspd = dist_factor * 255;
  if (rspd < -255)
    rspd = dist_factor * -255;
  if (lspd > 255)
    lspd = dist_factor * 255;
  if (lspd < -255)
    lspd = dist_factor * -255;
  
  sprintf(msg, "l=%d, r=%d", lspd, rspd);
  str_msg.data = msg;
  debug.publish(&str_msg);
  
  if (lspd < 0) {
    analogWrite(6, 0);
    analogWrite(7, -lspd);
  } else {
    analogWrite(7, 0);
    analogWrite(6, lspd);
  }
  if (rspd < 0) {
    analogWrite(2, 0);
    analogWrite(3, -rspd);
  } else {
    analogWrite(3, 0);
    analogWrite(2, rspd);
  }
}


char msg[30];
int messagedelay = 0;


float ultrasonic(void) {
  digitalWrite(23, LOW);
  delayMicroseconds(2);
