Skip to content

Commit

Permalink
Add digital in interface (#212)
Browse files Browse the repository at this point in the history
Signed-off-by: Javier Balloffet <[email protected]>
  • Loading branch information
jballoffet authored Feb 2, 2024
1 parent dfc2cc9 commit 387916f
Show file tree
Hide file tree
Showing 6 changed files with 153 additions and 38 deletions.
58 changes: 58 additions & 0 deletions andino_firmware/src/digital_in.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
// BSD 3-Clause License
//
// Copyright (c) 2024, Ekumen Inc.
// All rights reserved.
//
// 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. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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

namespace andino {

/// @brief This class defines an interface for digital inputs.
class DigitalIn {
public:
/// @brief Constructs a DigitalIn using the specified GPIO pin.
///
/// @param gpio_pin GPIO pin.
explicit DigitalIn(const int gpio_pin) : gpio_pin_(gpio_pin) {}

/// @brief Destructs the digital input.
virtual ~DigitalIn() = default;

/// @brief Initializes the digital input.
virtual void begin() const = 0;

/// @brief Gets the digital input value (0 or 1).
///
/// @return Digital input value.
virtual int read() const = 0;

protected:
/// GPIO pin.
const int gpio_pin_;
};

} // namespace andino
16 changes: 7 additions & 9 deletions andino_firmware/src/encoder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,13 @@

#include <stdint.h>

#include <Arduino.h>

#include "pcint.h"
#include "interrupt_in_arduino.h"

namespace andino {

constexpr int8_t Encoder::kTicksDelta[];

const PCInt::InterruptCallback Encoder::kCallbacks[kInstancesMax] = {callback_0, callback_1};
const InterruptIn::InterruptCallback Encoder::kCallbacks[kInstancesMax] = {callback_0, callback_1};

void Encoder::callback_0() {
if (Encoder::instances_[0] != nullptr) {
Expand All @@ -98,10 +96,10 @@ void Encoder::init() {
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_]);
channel_a_interrupt_in_.begin();
channel_a_interrupt_in_.attach(kCallbacks[instance_count_]);
channel_b_interrupt_in_.begin();
channel_b_interrupt_in_.attach(kCallbacks[instance_count_]);

instances_[instance_count_] = this;
instance_count_++;
Expand All @@ -114,7 +112,7 @@ 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_);
state_ |= (channel_b_interrupt_in_.read() << 1) | channel_a_interrupt_in_.read();

// Update the encoder count accordingly.
count_ += kTicksDelta[(state_ & 0x0F)];
Expand Down
15 changes: 8 additions & 7 deletions andino_firmware/src/encoder.h
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@

#include <stdint.h>

#include "pcint.h"
#include "interrupt_in_arduino.h"

namespace andino {

Expand All @@ -79,7 +79,8 @@ class Encoder {
///
/// @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) {}
Encoder(int a_gpio_pin, int b_gpio_pin)
: channel_a_interrupt_in_(a_gpio_pin), channel_b_interrupt_in_(b_gpio_pin) {}

/// @brief Initializes the encoder.
void init();
Expand Down Expand Up @@ -121,7 +122,7 @@ class Encoder {
static constexpr int kInstancesMax{2};

/// Static wrappers that redirect to instance callback methods.
static const PCInt::InterruptCallback kCallbacks[kInstancesMax];
static const InterruptIn::InterruptCallback kCallbacks[kInstancesMax];

/// Static wrapper that redirects to the first instance callback method.
static void callback_0();
Expand All @@ -138,11 +139,11 @@ class Encoder {
/// Number of constructed Encoder instances.
static int instance_count_;

/// Channel A GPIO pin.
int a_gpio_pin_;
/// Interrupt input connected to encoder channel A pin.
InterruptInArduino channel_a_interrupt_in_;

/// Channel B GPIO pin.
int b_gpio_pin_;
/// Interrupt input connected to encoder channel B pin.
InterruptInArduino channel_b_interrupt_in_;

/// Encoder state. It contains both the current and previous channels state readings:
/// +------+-----+-----+-----+-----+-----+-----+-----+-----+
Expand Down
28 changes: 13 additions & 15 deletions andino_firmware/src/pcint.h → andino_firmware/src/interrupt_in.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
// BSD 3-Clause License
//
// Copyright (c) 2023, Ekumen Inc.
// Copyright (c) 2024, Ekumen Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -29,30 +29,28 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once

#include <stdint.h>

#include <Arduino.h>
#include "digital_in.h"

namespace andino {

/// @brief This class provides a simple way to set and use Pin Change Interrupts.
class PCInt {
/// @brief This class defines an interface for digital interrupt inputs.
class InterruptIn : public DigitalIn {
public:
/// @brief Interrupt callback type.
typedef void (*InterruptCallback)();

/// @brief Attaches an interrupt callback.
/// @note Only one callback per port is supported.
/// @brief Constructs a InterruptIn using the specified GPIO pin.
///
/// @param pin Pin of interest.
/// @param callback Callback function.
static void attach_interrupt(uint8_t pin, InterruptCallback callback);
/// @param gpio_pin GPIO pin.
explicit InterruptIn(const int gpio_pin) : DigitalIn(gpio_pin) {}

private:
PCInt() = delete;
/// @brief Destructs the digital interrupt input.
virtual ~InterruptIn() = default;

/// Map between ports and Pin Change Mask registers.
static constexpr volatile uint8_t* kPortToPCMask[]{&PCMSK0, &PCMSK1, &PCMSK2};
/// @brief Attaches an interrupt callback.
///
/// @param callback Callback function.
virtual void attach(InterruptCallback callback) const = 0;
};

} // namespace andino
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
// BSD 3-Clause License
//
// Copyright (c) 2023, Ekumen Inc.
// Copyright (c) 2024, Ekumen Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
Expand All @@ -27,24 +27,28 @@
// 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 "pcint.h"
#include "interrupt_in_arduino.h"

#include <Arduino.h>

static andino::PCInt::InterruptCallback g_callbacks[3] = {nullptr};
/// Holds the attached callbacks.
static andino::InterruptIn::InterruptCallback g_callbacks[3] = {nullptr};

/// Pin change interrupt request 0 interrupt service routine.
ISR(PCINT0_vect) {
if (g_callbacks[0] != nullptr) {
g_callbacks[0]();
}
}

/// Pin change interrupt request 1 interrupt service routine.
ISR(PCINT1_vect) {
if (g_callbacks[1] != nullptr) {
g_callbacks[1]();
}
}

/// Pin change interrupt request 2 interrupt service routine.
ISR(PCINT2_vect) {
if (g_callbacks[2] != nullptr) {
g_callbacks[2]();
Expand All @@ -53,11 +57,16 @@ ISR(PCINT2_vect) {

namespace andino {

constexpr volatile uint8_t* PCInt::kPortToPCMask[];
/// Map between ports and Pin Change Mask registers.
static constexpr volatile uint8_t* kPortToPCMask[]{&PCMSK0, &PCMSK1, &PCMSK2};

void PCInt::attach_interrupt(uint8_t pin, InterruptCallback callback) {
uint8_t bit_mask = digitalPinToBitMask(pin);
uint8_t port = digitalPinToPort(pin);
void InterruptInArduino::begin() const { pinMode(gpio_pin_, INPUT_PULLUP); }

int InterruptInArduino::read() const { return digitalRead(gpio_pin_); }

void InterruptInArduino::attach(InterruptCallback callback) const {
uint8_t bit_mask = digitalPinToBitMask(gpio_pin_);
uint8_t port = digitalPinToPort(gpio_pin_);

if (port == NOT_A_PORT) {
return;
Expand Down
51 changes: 51 additions & 0 deletions andino_firmware/src/interrupt_in_arduino.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// BSD 3-Clause License
//
// Copyright (c) 2024, Ekumen Inc.
// All rights reserved.
//
// 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. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 "interrupt_in.h"

namespace andino {

/// @brief This class provides an Arduino implementation of the digital interrupt input interface.
class InterruptInArduino : public InterruptIn {
public:
/// @brief Constructs a InterruptInArduino using the specified GPIO pin.
///
/// @param gpio_pin GPIO pin.
explicit InterruptInArduino(const int gpio_pin) : InterruptIn(gpio_pin) {}

void begin() const override;

int read() const override;

void attach(InterruptCallback callback) const override;
};

} // namespace andino

0 comments on commit 387916f

Please sign in to comment.