From bf841ef63d798467822d7c348e357b1e2ba2dfb2 Mon Sep 17 00:00:00 2001
From: Zhiquan Yeo <zhiquan.yeo@gmail.com>
Date: Mon, 16 Oct 2023 11:54:57 -0400
Subject: [PATCH] Make drive motor encoders report positive count when moving
 forward

---
 src/main.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 2893def..d012be5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -106,11 +106,18 @@ void sendData() {
 
   // Encoders
   for (int i = 0; i < 4; i++) {
-    ptr += wpilibudp::writeEncoderData(i, xrp::readEncoderRaw(i), buffer, ptr);
+    int encoderValue = xrp::readEncoderRaw(i);
+
+    // We want to flip the encoder 0 value (left motor encoder) so that this returns
+    // positive values when moving forward.
+    if (i == 0) {
+      encoderValue = -encoderValue;
+    }
+
+    ptr += wpilibudp::writeEncoderData(i, encoderValue, buffer, ptr);
   } // 4x 7 bytes
 
   // DIO (currently just the button)
-  // TODO: Line sensors too if they are configured
   ptr += wpilibudp::writeDIOData(0, xrp::isUserButtonPressed(), buffer, ptr);
   // 1x 4 bytes