   duration = pulseIn(sonarEchoPin, HIGH);
   return 0.146 * duration - 23.843;  // returns in mm
 }
+
+double readForceSensor(int forceSensorVccPin, int forceSensorPin)
+{
+  digitalWrite(forceSensorVccPin, HIGH);
+  delay(5);
+  double sensorValue = analogReadAverage(forceSensorPin, 10);
+  digitalWrite(forceSensorVccPin, LOW);
+  return sensorValue;
+}
+
+double analogReadAverage(int pin, int count)
+{
+  double sum = 0;
+  for (int i=0; i<count; i++)
+    sum += analogRead(pin);
+  return sum / count;
+}
+


Servo servo[4];  // servo control objects
int angles[4] = { 90, 65, 90, 60 };   // initial angles

void setup()
{

  Serial.print(" set to ");
  Serial.print( angles[servo_num] );
  Serial.println(" degrees.");

  Serial.print("Measured angle (raw): ");
  Serial.println( analogReadAverage( servoPosPin[servo_num], 10 ) );

  Serial.println();
  Serial.print("Sonar reading: ");
