 #define BATTERY_VOLTAGE_PUBLISH_RATE 1000 // ms (publish voltage every second)
 
 // Encoders
-#define ENCODER_LEFT_PIN_A 2
-#define ENCODER_LEFT_PIN_B 11
-
-#define ENCODER_RIGHT_PIN_A 3
-#define ENCODER_RIGHT_PIN_B 4
-
-// #define TICKS_PER_REV 380 // ticks for one wheel revolution
+#define ENCODER_LEFT_PIN_A 3
+#define ENCODER_LEFT_PIN_B 4
+
+#define ENCODER_RIGHT_PIN_A 2
+#define ENCODER_RIGHT_PIN_B 11
+
+
+#define TICKS_PER_REV 379 // ticks for one wheel revolution
 #define TICKS_PER_METER 3110 // 790 ticks for 10 inches, 790 ticks per .254m ~ 3110.23622 ticks/meter
-
-#define WHEEL_VEL_PUBLISH_RATE 50 // ms (20Hz)
+#define METERS_PER_REV 0.12186495176848874 // meters per full revolution
+#define REV_PER_METER 8.20580474934037 // revolutions per meter
+
+
+#define WHEEL_VEL_PUBLISH_RATE 100 // ms (10Hz)
+#define MOTOR_PID_RATE 20 // ms (50Hz) PID Motor update rate (Should be < WHEEL_VEL_PUBLISH_RATE)
+#define MOTOR_SPEED_CUTOFF 20 // lowest motor speed at which to zero it
 
 // Pan Tilt Servo pins
 #define SERVO_PAN_PIN 5 // Pan

        else {encoder_right_ticks++;}
    }
    encoder_right_pin_A_last = encoder_right_val;
    // updateWheelPositionVelocity();
}

void updateWheelPositionVelocity()
{

        else {encoder_left_ticks++;}
    }
    encoder_left_pin_A_last = encoder_left_val;
}

void measureRightEncoder()
