diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index 038a8560..bacd2027 100644 --- a/andino_firmware/src/commands.h +++ b/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 diff --git a/andino_firmware/src/encoder_driver.h b/andino_firmware/src/encoder.cpp similarity index 68% rename from andino_firmware/src/encoder_driver.h rename to andino_firmware/src/encoder.cpp index cad57b0c..313548c3 100644 --- a/andino_firmware/src/encoder_driver.h +++ b/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 + +#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 diff --git a/andino_firmware/src/encoder_driver.cpp b/andino_firmware/src/encoder.h similarity index 50% rename from andino_firmware/src/encoder_driver.cpp rename to andino_firmware/src/encoder.h index 7e10f075..f2d3f0b3 100644 --- a/andino_firmware/src/encoder_driver.cpp +++ b/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 -#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 diff --git a/andino_firmware/src/main.cpp b/andino_firmware/src/main.cpp index 75c80bae..d60ed3e3 100644 --- a/andino_firmware/src/main.cpp +++ b/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;