 {
   tilt_deg = constrain(tilt_deg, SERVO_TILT_MIN, SERVO_TILT_MAX);
   servo_tilt.write(map(tilt_deg, 0, 180, ST2_MIN, ST2_MAX));
+}
+
+
+#define PID_OUTMIN -400
+#define PID_OUTMAX 400
+
+#define KP 20
+#define KI 10 // 500 * 20ms = 10
+#define KD 0
+
+// Had to do PIDs unclassed manually since we're running out of memory.
+double left_ITerm = 0;
+double left_lastInput = 0;
+void doLeftPID()
+{
+    /* Compute all the working error variables */
+    double error = pid_left_setpoint - pid_left_input;
+    left_ITerm += (KI * error);
+    if(left_ITerm > PID_OUTMAX) left_ITerm= PID_OUTMAX;
+    else if(left_ITerm < PID_OUTMIN) left_ITerm= PID_OUTMIN;
+    double dInput = (pid_left_input - left_lastInput);
+
+    /* Compute PID Output */
+    pid_left_output = KP * error + left_ITerm- KD * dInput;
+      
+    if(pid_left_output > PID_OUTMAX) pid_left_output = PID_OUTMAX;
+    else if(pid_left_output < PID_OUTMIN) pid_left_output = PID_OUTMIN;
+  
+    /* Remember some variables for next time */
+    left_lastInput = pid_left_input;
+}
+
+double right_ITerm = 0;
+double right_lastInput = 0;
+void doRightPID()
+{
+    /* Compute all the working error variables */
+    double error = pid_right_setpoint - pid_right_input;
+    right_ITerm += (KI * error);
+    if(right_ITerm > PID_OUTMAX) right_ITerm= PID_OUTMAX;
+    else if(right_ITerm < PID_OUTMIN) right_ITerm= PID_OUTMIN;
+    double dInput = (pid_right_input - right_lastInput);
+
+    /* Compute PID Output */
+    pid_right_output = KP * error + right_ITerm- KD * dInput;
+      
+    if(pid_right_output > PID_OUTMAX) pid_right_output = PID_OUTMAX;
+    else if(pid_right_output < PID_OUTMIN) pid_right_output = PID_OUTMIN;
+  
+    /* Remember some variables for next time */
+    right_lastInput = pid_right_input;
 }
    nh.subscribe(pan_tilt_sub); // Pan/Tilt Subscriber
  }

  
  last_battery_published = millis();
}


  } 

  // Attach Encoder Interrupts
  attachInterrupt(1, measureLeftEncoder, CHANGE); // 1 = pin 3 interrupt
  attachInterrupt(0, measureRightEncoder, CHANGE); // 0 = pin 2 interrupt

  // Left motor power wiring is reversed.
  motors.flipLeftMotor(true);
  
  // Initial speed is zero
  drive(0, 0);

  // Initialize PIDs
  initPIDs();

  // Init pan/tilt to center
  setPanTiltZero();

  nh.getHardware()->setBaud(ROS_SERIAL_BAUD_RATE);
  nh.initNode();
  nh.advertise(battery_pub); // Battery Voltage Publisher

