Add Feature to Get Encoder Rate (Period)

[beardedone55]

This change addresses issue #39. It adds code to calculate the encoder period (time between encoder pulses) and sends the data from the XRP to the robot code on the PC running the simulation. This change will allow the robot code to determine the motor speeds on the XRP using the Encoder::getRate() function.

For this to work, a corresponding change is necessary in WPILIB. I've included the change below. I've made sure that both this change and the change I am proposing for WPILIB are backward compatible. If this change is accepted, I will submit my WPILIB change for inclusion in their repository.

diff --git a/simulation/halsim_xrp/README.md b/simulation/halsim_xrp/README.md
index 79d6d525c..525ad3ddf 100644
--- a/simulation/halsim_xrp/README.md
+++ b/simulation/halsim_xrp/README.md
@@ -111,10 +111,12 @@ IDs:
 
 #### Encoder
 
-| Order | Data Type | Description |
-|-------|-----------|-------------|
-| 0     | _uint8_t_ | ID          |
-| 1     | _int32_t_ | Value       |
+| Order | Data Type  |  Description   |
+|-------|------------|----------------|
+| 0     | _uint8_t_  | ID             |
+| 1     | _int32_t_  | Value          |
+| 2     | _uint32_t_ | Period         |
+| 3     | _uint32_t_ | Period Divisor |       |
 
 IDs:
 | ID | Description         |
diff --git a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
index ae78469e4..04bfc7177 100644
--- a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
+++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
@@ -345,6 +345,9 @@ void XRP::ReadEncoderTag(std::span<const uint8_t> packet) {
     return;  // size(1) + tag(1) + id(1) + value(4)
   }
 
+  // size(1) + tag(1) + id(1) + value(4) + period(4) + divisor(4)
+  bool containsPeriod = packet.size() >= 15; 
+
   uint8_t encoderId = packet[2];
 
   packet = packet.subspan(3);  // Skip past the size and tag and ID
@@ -363,6 +366,19 @@ void XRP::ReadEncoderTag(std::span<const uint8_t> packet) {
   encJson["device"] = std::to_string(wpilibEncoderChannel);
   encJson["data"] = {{">count", value}};
 
+
+  if(containsPeriod) {
+     uint32_t period_count = 
+         static_cast<uint32_t>(wpi::support::endian::read32be(&packet[4]));
+     uint32_t period_divisor = 
+         static_cast<uint32_t>(wpi::support::endian::read32be(&packet[8]));
+     double period = (double)(period_count >> 1) / period_divisor;
+     if(!(period_count & 1))
+         period = -period;
+
+     encJson["data"].push_back({wpi::json(">period"), wpi::json(period)});
+  }
+
   m_wpilib_update_func(encJson);
 }
```diff

[zhiquanyeo]

@beardedone55 could you rebase against main? I just upgraded the build tooling so this should get the CI build working again

[beardedone55]

@beardedone55 could you rebase against main? I just upgraded the build tooling so this should get the CI build working again

Done.

[zhiquanyeo]

Sorry for the delay in reviewing/merging this in @beardedone55 , but this looks good to go now!

[beardedone55]

Great! Thank you!

[zhiquanyeo]

@beardedone55 feel free to put in a PR to allwpilib with the necessary halsim_xrp changes. Thanks again for the help with the firmware :)

[beardedone55]

I've submitted pull request wpilibsuite/allwpilib#6795 with the necessary halsim_xrp changes.