Add Feature to Get Encoder Rate (Period)

include/encoder.h

@@ -0,0 +1,148 @@
+/* The Encoder class reads encoder periods calculated by the
+   encoder2 pio program, stores the calculated encoder periods
+   and keeps track of encoder tick count.
+
+ Copyright (C) 2024 Brian LePage
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+ 1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+ 3. The name of the author may not be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+#include <Arduino.h>
+#include <limits>
+#include <vector>
+#include "encoder2.pio.h"
+
+namespace xrp {
+
+class Encoder {
+public:
+
+  Encoder() : period_queue(samples_to_average()) {}
+
+/****************************************************************
+*
+*  Encoder::init()
+*     Initialize PIO program that measures encoder period.
+*
+*  Returns true on sucess.
+*  
+*****************************************************************/
+  bool init(const int pin);
+
+/****************************************************************
+*
+*  Encoder::enable()
+*     Enable the encoder.
+*
+*  
+*****************************************************************/
+  void enable();
+
+/****************************************************************
+*
+*  Encoder::disable()
+*     Disable the encoder.
+*
+*  
+*****************************************************************/
+  void disable();
+
+/****************************************************************
+*
+*  Encoder::update()
+*     Get the latest encoder period(s) from the PIO if available
+*     store the latest, and updated the tick count.
+*  
+*  Returns number of samples that were retrieved.
+*  
+*****************************************************************/
+  int update();
+
+/****************************************************************
+*
+*  Encoder::setSamplesToAverage()
+*     Set the number of Encoder period samples to average together to
+*     calculate period.
+*
+*****************************************************************/
+
+void setSamplesToAverage(const int n);
+
+/****************************************************************
+*
+*  Encoder::getPeriod()
+*     Return the period calculated by the PIO in the following format:
+*     31                           1   0
+*    |  Period in 16-cycle ticks    | dir |
+*  
+*    This is the same format return by the PIO.
+*  
+*****************************************************************/
+  uint getPeriod();
+
+/****************************************************************
+*
+*  Encoder::getCount()
+*     Return the number of encoder ticks since the robot started.
+*
+*****************************************************************/
+  int getCount() const;
+
+/****************************************************************
+*
+*  Encoder::getDivisor()
+*     Get divisor for encoder period in ticks per second.
+*  
+*****************************************************************/
+
+ static constexpr uint getDivisor() {
+    return F_CPU / encoder2_CYCLES_PER_COUNT;
+ }
+
+private:
+  uint period = 0;
+  int period_fraction = 0;
+  uint saved_period = UINT_MAX;
+  bool direction = true;
+  bool saved_direction = true;
+  int samples_to_average_shift = 3;
+  int sample_count = 0;
+  int sample_index = 0;
+  std::vector<uint> period_queue;
+  uint count = 0;
+  int StateMachineIdx = -1;
+  unsigned long last_sample_time = 0;
+  PIO PioInstance = nullptr;
+  int pin = 0;
+  int offset = -1;
+  bool enabled = false;
+  void clearPeriodQueue();
+  uint getFraction(const uint count) const;
+  uint getWholeNumber(const uint count) const;
+  constexpr uint samples_to_average() const {
+    return 1 << samples_to_average_shift;
+  }
+}; 
+
+}

include/encoder2.pio.h

@@ -0,0 +1,82 @@
+// -------------------------------------------------- //
+// This file is autogenerated by pioasm; do not edit! //
+// -------------------------------------------------- //
+
+#pragma once
+
+#if !PICO_NO_HARDWARE
+#include "hardware/pio.h"
+#endif
+
+// -------- //
+// encoder2 //
+// -------- //
+
+#define encoder2_wrap_target 16
+#define encoder2_wrap 31
+
+#define encoder2_VERSION 102
+#define encoder2_CYCLES_PER_COUNT 16
+
+static const uint16_t encoder2_program_instructions[] = {
+    0x0b10, //  0: jmp    16                     [11]
+    0x0315, //  1: jmp    21                     [3] 
+    0x0318, //  2: jmp    24                     [3] 
+    0x0b11, //  3: jmp    17                     [11]
+    0x0318, //  4: jmp    24                     [3] 
+    0x0b10, //  5: jmp    16                     [11]
+    0x0b11, //  6: jmp    17                     [11]
+    0x0315, //  7: jmp    21                     [3] 
+    0x0315, //  8: jmp    21                     [3] 
+    0x0b11, //  9: jmp    17                     [11]
+    0x0b10, // 10: jmp    16                     [11]
+    0x0318, // 11: jmp    24                     [3] 
+    0x0b11, // 12: jmp    17                     [11]
+    0x0318, // 13: jmp    24                     [3] 
+    0x0315, // 14: jmp    21                     [3] 
+    0x0b10, // 15: jmp    16                     [11]
+            //     .wrap_target
+    0x0052, // 16: jmp    x--, 18                    
+    0xe020, // 17: set    x, 0                       
+    0x4002, // 18: in     pins, 2                    
+    0xa0e6, // 19: mov    osr, isr                   
+    0x60a4, // 20: out    pc, 4                      
+    0x0079, // 21: jmp    !y, 25                     
+    0xa04a, // 22: mov    y, !y                      
+    0x0019, // 23: jmp    25                         
+    0x0076, // 24: jmp    !y, 22                     
+    0xa0e6, // 25: mov    osr, isr                   
+    0xa0c9, // 26: mov    isr, !x                    
+    0x4041, // 27: in     y, 1                       
+    0x8000, // 28: push   noblock                    
+    0x60c2, // 29: out    isr, 2                     
+    0xa0eb, // 30: mov    osr, !null                 
+    0x603f, // 31: out    x, 31                      
+            //     .wrap
+};
+
+#if !PICO_NO_HARDWARE
+static const struct pio_program encoder2_program = {
+    .instructions = encoder2_program_instructions,
+    .length = 32,
+    .origin = 0,
+};
+
+static inline pio_sm_config encoder2_program_get_default_config(uint offset) {
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + encoder2_wrap_target, offset + encoder2_wrap);
+    return c;
+}
+
+static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) {
+    pio_sm_config c = encoder2_program_get_default_config(offset);
+    sm_config_set_in_pins(&c, base_pin);
+    sm_config_set_in_shift(&c, false, false, 32);
+    sm_config_set_out_shift(&c, true, false, 32);
+    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX);
+    pio_sm_init(pio, sm, offset, &c);
+    pio_sm_set_enabled(pio, sm, true);
+}
+
+#endif
+

include/robot.h

@@ -62,10 +62,8 @@ void robotSetEnabled(bool enabled);
 
 // Encoder Related
 void configureEncoder(int deviceId, int chA, int chB);
-int readEncoder(int deviceId);
 int readEncoderRaw(int rawDeviceId);
-void resetEncoder(int deviceId);
-std::vector<std::pair<int,int> > getActiveEncoderValues();
+uint readEncoderPeriod(int rawDeviceId);
 
 // PWM Related
 void setPwmValue(int wpilibChannel, double value);

include/wpilibudp.h

@@ -15,7 +15,7 @@ bool dsWatchdogActive();
 bool processPacket(char* buffer, int size);
 void resetState();
 
-int writeEncoderData(int deviceId, int count, char* buffer, int offset = 0);
+int writeEncoderData(int deviceId, int count, unsigned period, unsigned divisor, char* buffer, int offset = 0);
 int writeDIOData(int deviceId, bool value, char* buffer, int offset = 0);
 int writeGyroData(float rates[3], float angles[3], char* buffer, int offset = 0);
 int writeAccelData(float accels[3], char* buffer, int offset = 0);