diff --git a/src/ultrasonic.h b/src/ultrasonic.h
index a4cdce7..82e6c2c 100644
--- a/src/ultrasonic.h
+++ b/src/ultrasonic.h
@@ -11,7 +11,7 @@ RgbUltrasonic ultrasonic(6, 7);  //(signal, RGB)
 long colors[] = { RGB_RED, RGB_PURPLE, RGB_GREEN, RGB_BLUE, RGB_YELLOW, RGB_WHITE };
 long ultraTimer;
 int ultraInterval = 1000;
-float distance;
+int distance;
 #ifdef BITTLE
 int feedbackDirection = -1;
 #elif defined NYBBLE
@@ -27,6 +27,20 @@ void readRGBultrasonic() {
       return;
     }
 
+    //Add up multiple distance readings to reduce sensor noise
+    static unsigned int totalDistances = 0;
+    static byte readings = 0;
+    totalDistances += distance;
+    readings += 1;	
+    if (readings < 10) {
+      return;
+    }
+    //Take the average distance of the readings
+    distance = totalDistances / readings;
+    //Reset tracking variables
+    totalDistances = distance;
+    readings = 1;
+
     if (distance > 90) {
       if (!manualEyeColorQ)
         ultrasonic.SetRgbEffect(E_RGB_ALL, RGB_WHITE, E_EFFECT_BREATHING);
@@ -106,14 +120,24 @@ void readRGBultrasonic() {
         -30,
       };
 #endif
-      int allParameter[] = { mid[0] - distance / 2, -10 + distance / 2, distance * (random() % 50 < 1 ? int(random() % 2 - 1) : 1), 0,
-                             0, 0, 0, 0,
-                             mid[8] - 15 + distance / 2, mid[9] - 15 + distance / 2, mid[10] - 30 + distance * feedbackDirection, mid[11] - 30 + distance * feedbackDirection,
-                             mid[12] + 35 - distance, mid[13] + 35 - distance, mid[14] + 40 - distance * feedbackDirection, mid[15] + 40 - distance * feedbackDirection };
-      //      printList(allParameter);
+      int servoAngle[] = { 
+		mid[0] - distance / 2,			//Head pan
+	-10 + distance / 2,				//Head tilt (Nybble only)
+	distance * (random() % 50 < 1 ? int(random() % 2 - 1) : 1),	//Tail (Nybble only)
+	0, 0, 0, 0, 0,
+	mid[8] - 15 + distance / 2, 			//Front upper legs
+	mid[9] - 15 + distance / 2, 
+	mid[10] - 30 + distance * feedbackDirection, 	//Back upper legs
+	mid[11] - 30 + distance * feedbackDirection,
+	mid[12] + 35 - distance, 			//Front lower legs
+	mid[13] + 35 - distance, 
+	mid[14] + 40 - distance * feedbackDirection, 	//Back lower legs
+	mid[15] + 40 - distance * feedbackDirection 
+      };
+      //      printList(servoAngle);
       cmdLen = 16;
       for (byte i = 0; i < cmdLen; i++)
-        newCmd[i] = (int8_t)min(max(allParameter[i], -128), 127);
+        newCmd[i] = (int8_t)min(max(servoAngle[i], -128), 127);
       newCmd[cmdLen] = '~';
       randomInterval = 5000;
       tQueue->addTask('L', newCmd);
@@ -140,4 +164,4 @@ float readUltrasonic(int trigger, int echo = -1) {  //give two parameters for th
   long duration = pulseIn(echo, HIGH, farTime);
   // Calculating the distance
   return duration * 0.034 / 2;  // 10^-6 * 34000 cm/s
-}
\ No newline at end of file
+}