   // Set PID controllers command range
   pidLeft.setCmdRange(-motorsMaxCommand, motorsMaxCommand);
   pidRight.setCmdRange(-motorsMaxCommand, motorsMaxCommand);
+
+  if (btnA.isPresses())
+  {
+    // Center servos
+    maestro.setTarget(panServoCh, 6000);
+    maestro.setTarget(tiltServoCh, 6000);
+    maestro.setTarget(mastServoCh, 6000);
+    delay(500);
+    // Release servos
+    maestro.setTarget(panServoCh, 0);
+    maestro.setTarget(tiltServoCh, 0);
+    maestro.setTarget(mastServoCh, 0);
+  }
 }
 
 void loop() {

int lastSpeedCmdRight = 0;

// Define maximum speed command change per time step
const int accelMax = 20;

// Define different objects from RasPiBot202V2 library
// Encoders

const float motorsMaxCommand = 400;

// Define motor trims in 1/40
const int leftTrim = 40;
const int rightTrim = 40;

// Define speed variables for acceleration control
