   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: ");


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

void setup()
{

  pinMode(forceRPin, INPUT);

  pinMode(forceLVccPin, OUTPUT);
  digitalWrite(forceLVccPin, LOW);
  pinMode(forceLVccPin, OUTPUT);
  digitalWrite(forceRVccPin, LOW);
  
  Serial.begin(9600);
}

void loop()
{
  Serial.println();

  Serial.print("Force sensor values: [L: ");
  Serial.print( readForceSensor(forceLVccPin, forceLPin) );
  Serial.print("] [R: ");
  Serial.print( readForceSensor(forceRVccPin, forceRPin) );
  Serial.println("]");

  Serial.println();
  
  Serial.print("Servo #");
