Encoders Return Count But Not Rate

[beardedone55]

The XRP returns the motor encoder count to the robot program, but the robot program cannot get the rate (speed) from the encoder.

I've written some code that will allow the XRP to return the most recent period between encoder pulses, so that the robot code could determine the encoder rate. I've made the change I made available on my GitHub (https://github.com/beardedone55/xrp-wpilib-firmware/tree/lepageb). This change does require a change WPILIB for it to be able to use the encoder rate returned by the XRP; however, I did verify that with my change to the XRP firmware, the current version of WPILIB still works as it does now; it just does not make use of the encoder period (and cannot calculate speed).

I would appreciate it if my update could be pulled into the official XRP firmware repo so that others can use it. If this change is accepted, I will submit the corresponding change for WPILIB to use the encoder period data for inclusion in the official WPILIB code base.

Thanks!

[zhiquanyeo]

Hi! Your branch looks pretty good, although I'm wondering if it might make more sense (since we have to create a new message type anyway) to have a new "extended" Encoder message that would transmit both position and rate (we can pre-calculate rate on the XRP itself), instead of just sending the period and leaving it up to robot code to calculate the rate.

[beardedone55]

I had the XRP calculate the period instead of the rate because that's what WPILIB is expecting, so that the required change to WPILIB would be minimal. Also, by calculating the period, I was able to just count clock ticks and deal only integers, as it is my understanding that floating point arithmetic is computationally expensive on the PICO.

My change did not require a new message type. I just added the period to the packet after the count in the Encoder message. The current version of WPILIB will pull the count out of the message and ignore any extra data that happens to come along with it.

I have a change to WPILIB that will extract the period information from the Encoder message, if and only if the message contains that data. I've included that change below:

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);
 }

[zhiquanyeo]

Cool, thanks! Could you put a PR together and I (plus maybe a couple of others) will review it. Thanks again!