Add Encoder class (#172)

* Add Encoder class

Signed-off-by: Javier Balloffet <[email protected]>

* Address comments

Signed-off-by: Javier Balloffet <[email protected]>

---------

Signed-off-by: Javier Balloffet <[email protected]>

andino_firmware/src/commands.h

@@ -81,7 +81,5 @@
 #define UPDATE_PID     'u'
 #define DIGITAL_WRITE  'w'
 #define ANALOG_WRITE   'x'
-#define LEFT           0
-#define RIGHT          1
 
 #endif

andino_firmware/src/encoder_driver.h → andino_firmware/src/encoder.cpp

@@ -1,5 +1,5 @@
 // Code in this file is inspired by:
-// https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
+// https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
 //
 // ----------------------------------------------------------------------------
 // ros_arduino_bridge's license follows:
@@ -62,8 +62,61 @@
 // 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.
+#include "encoder.h"
 
-void initEncoders();
-long readEncoder(int i);
-void resetEncoder(int i);
-void resetEncoders();
+#include <stdint.h>
+
+#include "Arduino.h"
+#include "pcint.h"
+
+namespace andino {
+
+constexpr int8_t Encoder::kTicksDelta[];
+
+const PCInt::InterruptCallback Encoder::kCallbacks[kInstancesMax] = {callback_0, callback_1};
+
+void Encoder::callback_0() {
+  if (Encoder::instances_[0] != nullptr) {
+    Encoder::instances_[0]->callback();
+  }
+}
+
+void Encoder::callback_1() {
+  if (Encoder::instances_[1] != nullptr) {
+    Encoder::instances_[1]->callback();
+  }
+}
+
+Encoder* Encoder::instances_[kInstancesMax] = {nullptr, nullptr};
+int Encoder::instance_count_ = 0;
+
+void Encoder::init() {
+  // The current implementation only supports two instances of this class to be constructed. This
+  // prevents reaching a buffer overflow.
+  if (instance_count_ == kInstancesMax) {
+    return;
+  }
+
+  pinMode(a_gpio_pin_, INPUT_PULLUP);
+  pinMode(b_gpio_pin_, INPUT_PULLUP);
+  andino::PCInt::attach_interrupt(a_gpio_pin_, kCallbacks[instance_count_]);
+  andino::PCInt::attach_interrupt(b_gpio_pin_, kCallbacks[instance_count_]);
+
+  instances_[instance_count_] = this;
+  instance_count_++;
+}
+
+long Encoder::read() { return count_; }
+
+void Encoder::reset() { count_ = 0L; }
+
+void Encoder::callback() {
+  // Read the current channels state into the lowest 2 bits of the encoder state.
+  state_ <<= 2;
+  state_ |= (digitalRead(b_gpio_pin_) << 1) | digitalRead(a_gpio_pin_);
+
+  // Update the encoder count accordingly.
+  count_ += kTicksDelta[(state_ & 0x0F)];
+}
+
+}  // namespace andino

andino_firmware/src/encoder_driver.cpp → andino_firmware/src/encoder.h

@@ -1,5 +1,5 @@
 // Code in this file is inspired by:
-// https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
+// https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
 //
 // ----------------------------------------------------------------------------
 // ros_arduino_bridge's license follows:
@@ -62,74 +62,98 @@
 // 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.
-
-#include "encoder_driver.h"
+#pragma once
 
 #include <stdint.h>
 
-#include "Arduino.h"
-#include "commands.h"
-#include "hw.h"
 #include "pcint.h"
 
-volatile long left_enc_pos = 0L;
-volatile long right_enc_pos = 0L;
-static const int8_t ENC_STATES[] = {0, 1, -1, 0,  -1, 0,  0, 1,
-                                    1, 0, 0,  -1, 0,  -1, 1, 0};  // encoder lookup table
-
-/* Interrupt routine for LEFT encoder, taking care of actual counting */
-void left_encoder_cb() {
-  static uint8_t enc_last = 0;
-
-  enc_last <<= 2;                      // shift previous state two places
-  enc_last |= (PIND & (3 << 2)) >> 2;  // read the current state into lowest 2 bits
-
-  left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
-}
-
-/* Interrupt routine for RIGHT encoder, taking care of actual counting */
-void right_encoder_cb() {
-  static uint8_t enc_last = 0;
-
-  enc_last <<= 2;                      // shift previous state two places
-  enc_last |= (PINC & (3 << 4)) >> 4;  // read the current state into lowest 2 bits
-
-  right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
-}
-
-void initEncoders() {
-  pinMode(LEFT_ENCODER_A_GPIO_PIN, INPUT_PULLUP);
-  pinMode(LEFT_ENCODER_B_GPIO_PIN, INPUT_PULLUP);
-  pinMode(RIGHT_ENCODER_A_GPIO_PIN, INPUT_PULLUP);
-  pinMode(RIGHT_ENCODER_B_GPIO_PIN, INPUT_PULLUP);
-
-  andino::PCInt::attach_interrupt(LEFT_ENCODER_A_GPIO_PIN, left_encoder_cb);
-  andino::PCInt::attach_interrupt(LEFT_ENCODER_B_GPIO_PIN, left_encoder_cb);
-  andino::PCInt::attach_interrupt(RIGHT_ENCODER_A_GPIO_PIN, right_encoder_cb);
-  andino::PCInt::attach_interrupt(RIGHT_ENCODER_B_GPIO_PIN, right_encoder_cb);
-}
-
-/* Wrap the encoder reading function */
-long readEncoder(int i) {
-  if (i == LEFT)
-    return left_enc_pos;
-  else
-    return right_enc_pos;
-}
-
-/* Wrap the encoder reset function */
-void resetEncoder(int i) {
-  if (i == LEFT) {
-    left_enc_pos = 0L;
-    return;
-  } else {
-    right_enc_pos = 0L;
-    return;
-  }
-}
-
-/* Wrap the encoder reset function */
-void resetEncoders() {
-  resetEncoder(LEFT);
-  resetEncoder(RIGHT);
-}
+namespace andino {
+
+/// @brief This class allows to use a quadrature encoder by configuring it and then getting its
+/// ticks count value.
+/// @note The current implementation only supports two instances of this class to be constructed.
+class Encoder {
+ public:
+  /// @brief Constructs a new Encoder object.
+  ///
+  /// @param a_gpio_pin Encoder channel A GPIO pin.
+  /// @param b_gpio_pin Encoder channel B GPIO pin.
+  Encoder(int a_gpio_pin, int b_gpio_pin) : a_gpio_pin_(a_gpio_pin), b_gpio_pin_(b_gpio_pin) {}
+
+  /// @brief Initializes the encoder.
+  void init();
+
+  /// @brief Returns the ticks count value.
+  ///
+  /// @return Ticks count value.
+  long read();
+
+  /// @brief Sets the ticks count value to zero.
+  void reset();
+
+ private:
+  /// Ticks delta lookup table. Its content is defined as follows:
+  ///   +--------+-----+-----+--------+-----------+
+  ///   | Number | Old | New | Binary | Direction |
+  ///   |        | A B | A B |        |           |
+  ///   +--------+-----+-----+--------+-----------+
+  ///   |    0   | 0 0 | 0 0 |  0000  |     0     |
+  ///   |    1   | 0 0 | 0 1 |  0001  |    +1     |
+  ///   |    2   | 0 0 | 1 0 |  0010  |    -1     |
+  ///   |    3   | 0 0 | 1 1 |  0011  |     0     |
+  ///   |    4   | 0 1 | 0 0 |  0100  |    -1     |
+  ///   |    5   | 0 1 | 0 1 |  0101  |     0     |
+  ///   |    6   | 0 1 | 1 0 |  0110  |     0     |
+  ///   |    7   | 0 1 | 1 1 |  0111  |    +1     |
+  ///   |    8   | 1 0 | 0 0 |  1000  |    +1     |
+  ///   |    9   | 1 0 | 0 1 |  1001  |     0     |
+  ///   |   10   | 1 0 | 1 0 |  1010  |     0     |
+  ///   |   11   | 1 0 | 1 1 |  1011  |    -1     |
+  ///   |   12   | 1 1 | 0 0 |  1100  |     0     |
+  ///   |   13   | 1 1 | 0 1 |  1101  |    -1     |
+  ///   |   14   | 1 1 | 1 0 |  1110  |    +1     |
+  ///   |   15   | 1 1 | 1 1 |  1111  |     0     |
+  ///   +--------+-----+-----+--------+-----------+
+  static constexpr int8_t kTicksDelta[]{0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0};
+
+  /// Maximum number of instances of this class that can be instantiated.
+  static constexpr int kInstancesMax{2};
+
+  /// Static wrappers that redirect to instance callback methods.
+  static const PCInt::InterruptCallback kCallbacks[kInstancesMax];
+
+  /// Static wrapper that redirects to the first instance callback method.
+  static void callback_0();
+
+  /// Static wrapper that redirects to the second instance callback method.
+  static void callback_1();
+
+  /// Channels GPIO interrupt callback.
+  void callback();
+
+  /// Holds references to the constructed Encoder instances.
+  static Encoder* instances_[kInstancesMax];
+
+  /// Number of constructed Encoder instances.
+  static int instance_count_;
+
+  /// Channel A GPIO pin.
+  int a_gpio_pin_;
+
+  /// Channel B GPIO pin.
+  int b_gpio_pin_;
+
+  /// Encoder state. It contains both the current and previous channels state readings:
+  ///   +------+-----+-----+-----+-----+-----+-----+-----+-----+
+  ///   | Bits |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0  |
+  ///   +------+-----+-----+-----+-----+-----+-----+-----+-----+
+  ///   |      |  x  |  x  |  x  |  x  | PREVIOUS  |  CURRENT  |
+  ///   +------+-----+-----+-----+-----+-----+-----+-----+-----+
+  uint8_t state_{0x00};
+
+  /// Ticks count.
+  volatile long count_{0L};
+};
+
+}  // namespace andino

andino_firmware/src/main.cpp

@@ -78,7 +78,7 @@
 #include "motor.h"
 
 /* Encoder driver function definitions */
-#include "encoder_driver.h"
+#include "encoder.h"
 
 /* PID parameters and functions */
 #include "pid.h"
@@ -127,6 +127,10 @@ andino::Motor right_motor(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_
 andino::PID left_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
 andino::PID right_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
 
+// TODO(jballoffet): Make these objects local to the main function.
+andino::Encoder left_encoder(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN);
+andino::Encoder right_encoder(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN);
+
 /* Clear the current command parameters */
 void resetCommand() {
   cmd = 0;
@@ -176,14 +180,15 @@ int runCommand() {
       Serial.println("OK");
       break;
     case READ_ENCODERS:
-      Serial.print(readEncoder(LEFT));
+      Serial.print(left_encoder.read());
       Serial.print(" ");
-      Serial.println(readEncoder(RIGHT));
+      Serial.println(right_encoder.read());
       break;
     case RESET_ENCODERS:
-      resetEncoders();
-      left_pid_controller.reset(readEncoder(LEFT));
-      right_pid_controller.reset(readEncoder(RIGHT));
+      left_encoder.reset();
+      right_encoder.reset();
+      left_pid_controller.reset(left_encoder.read());
+      right_pid_controller.reset(right_encoder.read());
       Serial.println("OK");
       break;
     case MOTOR_SPEEDS:
@@ -192,8 +197,8 @@ int runCommand() {
       if (arg1 == 0 && arg2 == 0) {
         left_motor.set_speed(0);
         right_motor.set_speed(0);
-        left_pid_controller.reset(readEncoder(LEFT));
-        right_pid_controller.reset(readEncoder(RIGHT));
+        left_pid_controller.reset(left_encoder.read());
+        right_pid_controller.reset(right_encoder.read());
         left_pid_controller.enable(false);
         right_pid_controller.enable(false);
       } else {
@@ -209,8 +214,8 @@ int runCommand() {
     case MOTOR_RAW_PWM:
       /* Reset the auto stop timer */
       lastMotorCommand = millis();
-      left_pid_controller.reset(readEncoder(LEFT));
-      right_pid_controller.reset(readEncoder(RIGHT));
+      left_pid_controller.reset(left_encoder.read());
+      right_pid_controller.reset(right_encoder.read());
       // Sneaky way to temporarily disable the PID
       left_pid_controller.enable(false);
       right_pid_controller.enable(false);
@@ -246,14 +251,15 @@ int runCommand() {
 void setup() {
   Serial.begin(BAUDRATE);
 
-  initEncoders();
+  left_encoder.init();
+  right_encoder.init();
 
   // Enable motors.
   left_motor.set_state(true);
   right_motor.set_state(true);
 
-  left_pid_controller.reset(readEncoder(LEFT));
-  right_pid_controller.reset(readEncoder(RIGHT));
+  left_pid_controller.reset(left_encoder.read());
+  right_pid_controller.reset(right_encoder.read());
 }
 
 /* Enter the main loop.  Read and parse input from the serial port
@@ -304,8 +310,8 @@ void loop() {
   if (millis() > nextPID) {
     int left_motor_speed = 0;
     int right_motor_speed = 0;
-    left_pid_controller.compute(readEncoder(LEFT), left_motor_speed);
-    right_pid_controller.compute(readEncoder(RIGHT), right_motor_speed);
+    left_pid_controller.compute(left_encoder.read(), left_motor_speed);
+    right_pid_controller.compute(right_encoder.read(), right_motor_speed);
     left_motor.set_speed(left_motor_speed);
     right_motor.set_speed(right_motor_speed);
     nextPID += PID_INTERVAL;