mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-09-09 05:40:46 +02:00
Fixed a lot of bugs in pin details. Basically, pins are now re-used if needed (such as 'undefined'), and construction doesn't do any hardware pin initialization. RAII is enforced. Grbl starts.
This commit is contained in:
@@ -22,6 +22,7 @@
|
|||||||
# include "src/Grbl.h"
|
# include "src/Grbl.h"
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
|
Serial.begin(115200);
|
||||||
grbl_init();
|
grbl_init();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@@ -393,14 +393,14 @@ void init_motors() {
|
|||||||
for (int gang_index = 0; gang_index < 2; gang_index++) {
|
for (int gang_index = 0; gang_index < 2; gang_index++) {
|
||||||
Pin pin = ms3_pins[axis][gang_index];
|
Pin pin = ms3_pins[axis][gang_index];
|
||||||
if (pin != Pin::UNDEFINED) {
|
if (pin != Pin::UNDEFINED) {
|
||||||
pin.setAttr(Pin::Attr::Output | Pin::Attr::InitialHigh);
|
pin.setAttr(Pin::Attr::Output | Pin::Attr::InitialOn);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (StepperResetPin->get() != Pin::UNDEFINED) {
|
if (StepperResetPin->get() != Pin::UNDEFINED) {
|
||||||
// !RESET pin on steppers (MISO On Schematic)
|
// !RESET pin on steppers (MISO On Schematic)
|
||||||
StepperResetPin->get().setAttr(Pin::Attr::Output | Pin::Attr::InitialHigh);
|
StepperResetPin->get().setAttr(Pin::Attr::Output | Pin::Attr::InitialOn);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@@ -73,7 +73,7 @@ namespace Motors {
|
|||||||
_has_errors = false;
|
_has_errors = false;
|
||||||
init_step_dir_pins(); // from StandardStepper
|
init_step_dir_pins(); // from StandardStepper
|
||||||
|
|
||||||
_cs_pin.setAttr(Pin::Attr::Output | Pin::Attr::InitialHigh);
|
_cs_pin.setAttr(Pin::Attr::Output | Pin::Attr::InitialOn);
|
||||||
|
|
||||||
// use slower speed if I2S
|
// use slower speed if I2S
|
||||||
if (_cs_pin.capabilities().has(Pin::Capabilities::I2S)) {
|
if (_cs_pin.capabilities().has(Pin::Capabilities::I2S)) {
|
||||||
|
@@ -1,22 +1,24 @@
|
|||||||
#include "Pin.h"
|
#include "Pin.h"
|
||||||
#include "Grbl.h"
|
|
||||||
|
|
||||||
// Pins:
|
// Pins:
|
||||||
#include "Pins/PinOptionsParser.h"
|
#include "Pins/PinOptionsParser.h"
|
||||||
#include "Pins/VoidPinDetail.h"
|
#include "Pins/VoidPinDetail.h"
|
||||||
#include "Pins/GPIOPinDetail.h"
|
#include "Pins/GPIOPinDetail.h"
|
||||||
#include "Pins/I2SPinDetail.h"
|
#include "Pins/I2SPinDetail.h"
|
||||||
#ifdef PIN_DEBUG
|
|
||||||
|
#if defined PIN_DEBUG && defined ESP32
|
||||||
# include "Pins/DebugPinDetail.h"
|
# include "Pins/DebugPinDetail.h"
|
||||||
|
# include "Grbl.h" // grbl_sendf
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
bool Pin::parse(String str, Pins::PinDetail*& pinImplementation, int& pinNumber) {
|
bool Pin::parse(String str, Pins::PinDetail*& pinImplementation) {
|
||||||
// Initialize pinImplementation first! Callers might want to delete it, and we don't want a random pointer.
|
// Initialize pinImplementation first! Callers might want to delete it, and we don't want a random pointer.
|
||||||
pinImplementation = nullptr;
|
pinImplementation = nullptr;
|
||||||
|
|
||||||
if (str == "") {
|
if (str == "") {
|
||||||
|
// Re-use undefined pins happens in 'create':
|
||||||
pinImplementation = new Pins::VoidPinDetail();
|
pinImplementation = new Pins::VoidPinDetail();
|
||||||
return pinImplementation;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Parse the definition: [GPIO].[pinNumber]:[attributes]
|
// Parse the definition: [GPIO].[pinNumber]:[attributes]
|
||||||
@@ -31,13 +33,13 @@ bool Pin::parse(String str, Pins::PinDetail*& pinImplementation, int& pinNumber)
|
|||||||
++idx;
|
++idx;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int pinNumber = 0;
|
||||||
if (prefix != "") {
|
if (prefix != "") {
|
||||||
if (idx == str.end()) {
|
if (idx == str.end()) {
|
||||||
// Incorrect pin definition.
|
// Incorrect pin definition.
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
pinNumber = 0;
|
|
||||||
for (; idx != str.end() && *idx >= '0' && *idx <= '9'; ++idx) {
|
for (; idx != str.end() && *idx >= '0' && *idx <= '9'; ++idx) {
|
||||||
pinNumber = pinNumber * 10 + int(*idx - '0');
|
pinNumber = pinNumber * 10 + int(*idx - '0');
|
||||||
}
|
}
|
||||||
@@ -67,46 +69,86 @@ bool Pin::parse(String str, Pins::PinDetail*& pinImplementation, int& pinNumber)
|
|||||||
Pins::PinOptionsParser parser(options.begin(), options.end());
|
Pins::PinOptionsParser parser(options.begin(), options.end());
|
||||||
|
|
||||||
// Build this pin:
|
// Build this pin:
|
||||||
|
if (prefix == "gpio") {
|
||||||
try {
|
pinImplementation = new Pins::GPIOPinDetail(uint8_t(pinNumber), parser);
|
||||||
if (prefix == "gpio") {
|
} else if (prefix == "i2s") {
|
||||||
pinImplementation = new Pins::GPIOPinDetail(uint8_t(pinNumber), parser);
|
|
||||||
} else if (prefix == "i2s") {
|
|
||||||
#ifdef ESP_32
|
#ifdef ESP_32
|
||||||
pinImplementation = new Pins::I2SPinDetail(uint8_t(pinNumber), parser);
|
pinImplementation = new Pins::I2SPinDetail(uint8_t(pinNumber), parser);
|
||||||
|
#else
|
||||||
|
return false; // not supported
|
||||||
#endif
|
#endif
|
||||||
} else {
|
} else if (prefix == "void") {
|
||||||
#ifdef PIN_DEBUG
|
// Note: having multiple void pins has its uses for debugging. Note that using
|
||||||
pinImplementation = new Pins::DebugPinDetail(pinImplementation);
|
// when doing 'x == Pin::UNDEFINED' will evaluate to 'false' if the pin number
|
||||||
#endif
|
// is not 0.
|
||||||
}
|
pinImplementation = new Pins::VoidPinDetail(uint8_t(pinNumber));
|
||||||
} catch (const std::exception& e) {
|
|
||||||
grbl_sendf(CLIENT_SERIAL, "%s\r\n", e.what());
|
|
||||||
pinImplementation = new Pins::VoidPinDetail();
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
return pinImplementation;
|
|
||||||
|
#if defined PIN_DEBUG && defined ESP32
|
||||||
|
pinImplementation = new Pins::DebugPinDetail(pinImplementation);
|
||||||
|
#endif
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
Pin Pin::create(const String& str) {
|
Pin Pin::create(const String& str) {
|
||||||
Pins::PinDetail* pinImplementation;
|
Pins::PinDetail* pinImplementation = nullptr;
|
||||||
int pinNumber;
|
try {
|
||||||
|
#ifdef PIN_DEBUG
|
||||||
|
# ifdef ESP32
|
||||||
|
grbl_sendf(CLIENT_ALL, "Setting up pin: [%s]\r\n", str.c_str());
|
||||||
|
# endif
|
||||||
|
#endif
|
||||||
|
if (!parse(str, pinImplementation)) {
|
||||||
|
#ifdef ESP32
|
||||||
|
grbl_sendf(CLIENT_ALL, "Setting up pin: '%s' failed.", str.c_str());
|
||||||
|
#endif
|
||||||
|
return Pin::UNDEFINED;
|
||||||
|
} else {
|
||||||
|
// Check if we already have this pin:
|
||||||
|
auto existingPin = Pins::PinLookup::_instance.FindExisting(pinImplementation);
|
||||||
|
|
||||||
Assert(parse(str, pinImplementation, pinNumber), "Pin definition is invalid.");
|
// If we already had it, and we didn't find itself, remove the new instance:
|
||||||
|
if (existingPin >= 0) {
|
||||||
|
#ifdef ESP32
|
||||||
|
grbl_sendf(CLIENT_ALL, "Reusing previous pin initialization.");
|
||||||
|
#endif
|
||||||
|
if (pinImplementation) {
|
||||||
|
delete pinImplementation;
|
||||||
|
}
|
||||||
|
|
||||||
// Register:
|
return Pin(uint8_t(existingPin));
|
||||||
Pins::PinLookup::_instance.SetPin(uint8_t(pinNumber), pinImplementation);
|
} else {
|
||||||
|
// This is a new pin. So, register, and return the pin object that refers to it.
|
||||||
|
auto pinNumber = pinImplementation->number();
|
||||||
|
auto realIndex = Pins::PinLookup::_instance.SetPin(uint8_t(pinNumber), pinImplementation);
|
||||||
|
return Pin(realIndex);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
return Pin(uint8_t(pinNumber));
|
} catch (AssertionFailed& ex) { // We shouldn't get here under normal circumstances.
|
||||||
|
#ifdef PIN_DEBUG
|
||||||
|
# ifdef ESP32
|
||||||
|
grbl_sendf(CLIENT_ALL, "Failed. Details: %s\r\n", ex.stackTrace.c_str());
|
||||||
|
# endif
|
||||||
|
#endif
|
||||||
|
// RAII safety guard.
|
||||||
|
if (pinImplementation) {
|
||||||
|
delete pinImplementation;
|
||||||
|
}
|
||||||
|
|
||||||
|
return Pin::UNDEFINED;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Pin::validate(const String& str) {
|
bool Pin::validate(const String& str) {
|
||||||
Pins::PinDetail* pinImplementation;
|
Pins::PinDetail* pinImplementation;
|
||||||
int pinNumber;
|
int pinNumber;
|
||||||
|
|
||||||
auto valid = parse(str, pinImplementation, pinNumber);
|
auto valid = parse(str, pinImplementation);
|
||||||
|
if (pinImplementation) {
|
||||||
|
delete pinImplementation;
|
||||||
|
}
|
||||||
|
|
||||||
delete pinImplementation;
|
|
||||||
return valid;
|
return valid;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@@ -9,7 +9,7 @@
|
|||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
|
|
||||||
// #define PIN_DEBUG // Pin debugging. WILL spam you with a lot of data!
|
#define PIN_DEBUG // Pin debugging. WILL spam you with a lot of data!
|
||||||
|
|
||||||
// Forward declarations:
|
// Forward declarations:
|
||||||
class String;
|
class String;
|
||||||
@@ -37,7 +37,7 @@ class Pin {
|
|||||||
|
|
||||||
inline Pin(uint8_t index) : _index(index) {}
|
inline Pin(uint8_t index) : _index(index) {}
|
||||||
|
|
||||||
static bool parse(String str, Pins::PinDetail*& detail, int& pinNumber);
|
static bool parse(String str, Pins::PinDetail*& detail);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
using Capabilities = Pins::PinCapabilities;
|
using Capabilities = Pins::PinCapabilities;
|
||||||
@@ -61,8 +61,8 @@ public:
|
|||||||
inline Pin& operator=(Pin&& o) = default;
|
inline Pin& operator=(Pin&& o) = default;
|
||||||
|
|
||||||
// Some convenience operators:
|
// Some convenience operators:
|
||||||
inline bool operator==(Pin o) const { return _index == _index; }
|
inline bool operator==(const Pin& o) const { return _index == o._index; }
|
||||||
inline bool operator!=(Pin o) const { return _index != _index; }
|
inline bool operator!=(const Pin& o) const { return _index != o._index; }
|
||||||
|
|
||||||
inline uint8_t getNative(Capabilities expectedBehavior) const {
|
inline uint8_t getNative(Capabilities expectedBehavior) const {
|
||||||
auto detail = Pins::PinLookup::_instance.GetPin(_index);
|
auto detail = Pins::PinLookup::_instance.GetPin(_index);
|
||||||
|
@@ -36,7 +36,7 @@ namespace Pins {
|
|||||||
if (value.has(PinAttributes::Exclusive)) {
|
if (value.has(PinAttributes::Exclusive)) {
|
||||||
buf[n++] = 'X';
|
buf[n++] = 'X';
|
||||||
}
|
}
|
||||||
if (value.has(PinAttributes::InitialHigh)) {
|
if (value.has(PinAttributes::InitialOn)) {
|
||||||
buf[n++] = '+';
|
buf[n++] = '+';
|
||||||
}
|
}
|
||||||
buf[n++] = 0;
|
buf[n++] = 0;
|
||||||
|
@@ -22,7 +22,9 @@ namespace Pins {
|
|||||||
bool shouldEvent();
|
bool shouldEvent();
|
||||||
|
|
||||||
public:
|
public:
|
||||||
DebugPinDetail(PinDetail* implementation) : _implementation(implementation), _lastEvent(0), _eventCount(0) {}
|
DebugPinDetail(PinDetail* implementation) :
|
||||||
|
PinDetail(implementation->number()), _implementation(implementation), _lastEvent(0), _eventCount(0), _isrHandler({ 0 }) {
|
||||||
|
}
|
||||||
|
|
||||||
PinCapabilities capabilities() const override { return _implementation->capabilities(); }
|
PinCapabilities capabilities() const override { return _implementation->capabilities(); }
|
||||||
|
|
||||||
|
@@ -2,9 +2,9 @@
|
|||||||
#include "../Assert.h"
|
#include "../Assert.h"
|
||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
ErrorPinDetail::ErrorPinDetail(const PinOptionsParser& options) {}
|
ErrorPinDetail::ErrorPinDetail(const PinOptionsParser& options) : PinDetail(0) {}
|
||||||
|
|
||||||
PinCapabilities ErrorPinDetail::capabilities() const { return PinCapabilities::None; }
|
PinCapabilities ErrorPinDetail::capabilities() const { return PinCapabilities::Error; }
|
||||||
|
|
||||||
void ErrorPinDetail::write(int high) { Assert(false, "Cannot write to an error pin."); }
|
void ErrorPinDetail::write(int high) { Assert(false, "Cannot write to an error pin."); }
|
||||||
int ErrorPinDetail::read() { Assert(false, "Cannot read from an error pin."); }
|
int ErrorPinDetail::read() { Assert(false, "Cannot read from an error pin."); }
|
||||||
|
@@ -77,10 +77,15 @@ namespace Pins {
|
|||||||
}
|
}
|
||||||
|
|
||||||
GPIOPinDetail::GPIOPinDetail(uint8_t index, PinOptionsParser options) :
|
GPIOPinDetail::GPIOPinDetail(uint8_t index, PinOptionsParser options) :
|
||||||
_index(index), _capabilities(GetDefaultCapabilities(index)), _attributes(Pins::PinAttributes::Undefined), _readWriteMask(0) {
|
PinDetail(index), _capabilities(GetDefaultCapabilities(index)), _attributes(Pins::PinAttributes::Undefined), _readWriteMask(0) {
|
||||||
if (_capabilities == PinCapabilities::None) {
|
// NOTE:
|
||||||
throw std::runtime_error("Bad GPIO number");
|
//
|
||||||
}
|
// RAII is very important here! If we throw an exception in the constructor, the resources
|
||||||
|
// that were allocated by the constructor up to that point _MUST_ be freed! Otherwise, you
|
||||||
|
// WILL get into trouble.
|
||||||
|
|
||||||
|
Assert(_capabilities != PinCapabilities::None, "Bad GPIO number");
|
||||||
|
|
||||||
// User defined pin capabilities
|
// User defined pin capabilities
|
||||||
for (auto opt : options) {
|
for (auto opt : options) {
|
||||||
if (opt.is("pu")) {
|
if (opt.is("pu")) {
|
||||||
@@ -88,20 +93,16 @@ namespace Pins {
|
|||||||
} else if (opt.is("pd")) {
|
} else if (opt.is("pd")) {
|
||||||
_attributes = _attributes | PinAttributes::PullDown;
|
_attributes = _attributes | PinAttributes::PullDown;
|
||||||
} else if (opt.is("low")) {
|
} else if (opt.is("low")) {
|
||||||
_attributes = _attributes | PinAttributes::ActiveLow;
|
_attributes = _attributes | PinAttributes::ActiveLow;
|
||||||
} else if (opt.is("high")) {
|
} else if (opt.is("high")) {
|
||||||
// Default: Active HIGH.
|
// Default: Active HIGH.
|
||||||
} else {
|
} else {
|
||||||
throw std::runtime_error("Bad GPIO option");
|
Assert(false, "Bad GPIO option passed to pin: %s", opt());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// XXX This should not be done when the constructor is called for
|
|
||||||
// validating a setting.
|
|
||||||
// Update the R/W mask for ActiveLow setting
|
// Update the R/W mask for ActiveLow setting
|
||||||
if (_attributes.has(PinAttributes::ActiveLow)) {
|
if (_attributes.has(PinAttributes::ActiveLow)) {
|
||||||
__pinMode(_index, OUTPUT);
|
|
||||||
__digitalWrite(_index, HIGH);
|
|
||||||
_readWriteMask = HIGH;
|
_readWriteMask = HIGH;
|
||||||
} else {
|
} else {
|
||||||
_readWriteMask = LOW;
|
_readWriteMask = LOW;
|
||||||
@@ -146,9 +147,11 @@ namespace Pins {
|
|||||||
pinModeValue |= OUTPUT;
|
pinModeValue |= OUTPUT;
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO: If the pin is ActiveLow, should we take this into account or not?
|
// If the pin is ActiveLow, we should take that into account here:
|
||||||
if (value.has(PinAttributes::InitialHigh)) {
|
if (value.has(PinAttributes::InitialOn)) {
|
||||||
__digitalWrite(_index, HIGH);
|
__digitalWrite(_index, HIGH ^ _readWriteMask);
|
||||||
|
} else {
|
||||||
|
__digitalWrite(_index, LOW ^ _readWriteMask);
|
||||||
}
|
}
|
||||||
|
|
||||||
__pinMode(_index, pinModeValue);
|
__pinMode(_index, pinModeValue);
|
||||||
|
@@ -4,7 +4,6 @@
|
|||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
class GPIOPinDetail : public PinDetail {
|
class GPIOPinDetail : public PinDetail {
|
||||||
uint8_t _index;
|
|
||||||
PinCapabilities _capabilities;
|
PinCapabilities _capabilities;
|
||||||
PinAttributes _attributes;
|
PinAttributes _attributes;
|
||||||
int _readWriteMask;
|
int _readWriteMask;
|
||||||
|
@@ -8,7 +8,7 @@ extern "C" void __digitalWrite(uint8_t pin, uint8_t val);
|
|||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
I2SPinDetail::I2SPinDetail(uint8_t index, const PinOptionsParser& options) :
|
I2SPinDetail::I2SPinDetail(uint8_t index, const PinOptionsParser& options) :
|
||||||
_index(index), _capabilities(PinCapabilities::Output | PinCapabilities::I2S), _attributes(Pins::PinAttributes::Undefined),
|
PinDetail(index), _capabilities(PinCapabilities::Output | PinCapabilities::I2S), _attributes(Pins::PinAttributes::Undefined),
|
||||||
_readWriteMask(0) {
|
_readWriteMask(0) {
|
||||||
// User defined pin capabilities
|
// User defined pin capabilities
|
||||||
for (auto opt : options) {
|
for (auto opt : options) {
|
||||||
@@ -24,8 +24,7 @@ namespace Pins {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Update the R/W mask for ActiveLow setting
|
// Update the R/W mask for ActiveLow setting
|
||||||
if (_attributes.ActiveLow) {
|
if (_attributes.has(PinAttributes::ActiveLow)) {
|
||||||
__digitalWrite(_index, HIGH);
|
|
||||||
_readWriteMask = 1;
|
_readWriteMask = 1;
|
||||||
} else {
|
} else {
|
||||||
_readWriteMask = 0;
|
_readWriteMask = 0;
|
||||||
@@ -54,6 +53,9 @@ namespace Pins {
|
|||||||
// I2S out pins cannot be configured, hence there
|
// I2S out pins cannot be configured, hence there
|
||||||
// is nothing to do here for them. We basically
|
// is nothing to do here for them. We basically
|
||||||
// just check for conflicts above...
|
// just check for conflicts above...
|
||||||
|
|
||||||
|
// If the pin is ActiveLow, we should take that into account here:
|
||||||
|
write(value.has(PinAttributes::InitialOn) ? HIGH : LOW);
|
||||||
}
|
}
|
||||||
|
|
||||||
String I2SPinDetail::toString() const { return String("I2S_") + int(_index); }
|
String I2SPinDetail::toString() const { return String("I2S_") + int(_index); }
|
||||||
|
@@ -18,7 +18,7 @@ namespace Pins {
|
|||||||
const int capabilityMask = (1 << (__LINE__ - START_LINE)) - 1; // -------- Mask capabilities till here
|
const int capabilityMask = (1 << (__LINE__ - START_LINE)) - 1; // -------- Mask capabilities till here
|
||||||
PinAttributes PinAttributes::ActiveLow(1 << (__LINE__ - START_LINE));
|
PinAttributes PinAttributes::ActiveLow(1 << (__LINE__ - START_LINE));
|
||||||
PinAttributes PinAttributes::Exclusive(1 << (__LINE__ - START_LINE)); // \/ These are attributes
|
PinAttributes PinAttributes::Exclusive(1 << (__LINE__ - START_LINE)); // \/ These are attributes
|
||||||
PinAttributes PinAttributes::InitialHigh(1 << (__LINE__ - START_LINE)); // \/ These are attributes
|
PinAttributes PinAttributes::InitialOn(1 << (__LINE__ - START_LINE)); // \/ These are attributes
|
||||||
|
|
||||||
bool PinAttributes::validateWith(PinCapabilities caps) {
|
bool PinAttributes::validateWith(PinCapabilities caps) {
|
||||||
auto capMask = (caps._value & capabilityMask);
|
auto capMask = (caps._value & capabilityMask);
|
||||||
|
@@ -35,7 +35,7 @@ namespace Pins {
|
|||||||
|
|
||||||
static PinAttributes ActiveLow;
|
static PinAttributes ActiveLow;
|
||||||
static PinAttributes Exclusive;
|
static PinAttributes Exclusive;
|
||||||
static PinAttributes InitialHigh;
|
static PinAttributes InitialOn;
|
||||||
|
|
||||||
inline PinAttributes operator|(PinAttributes rhs) { return PinAttributes(_value | rhs._value); }
|
inline PinAttributes operator|(PinAttributes rhs) { return PinAttributes(_value | rhs._value); }
|
||||||
inline PinAttributes operator&(PinAttributes rhs) { return PinAttributes(_value & rhs._value); }
|
inline PinAttributes operator&(PinAttributes rhs) { return PinAttributes(_value & rhs._value); }
|
||||||
|
@@ -17,10 +17,14 @@ namespace Pins {
|
|||||||
PinCapabilities PinCapabilities::PullUp(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
PinCapabilities PinCapabilities::PullUp(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
||||||
PinCapabilities PinCapabilities::PullDown(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
PinCapabilities PinCapabilities::PullDown(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
||||||
PinCapabilities PinCapabilities::ISR(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
PinCapabilities PinCapabilities::ISR(1 << (__LINE__ - START_LINE)); // NOTE: Mapped in PinAttributes!
|
||||||
PinCapabilities PinCapabilities::Native(1 << (__LINE__ - START_LINE));
|
|
||||||
PinCapabilities PinCapabilities::PWM(1 << (__LINE__ - START_LINE));
|
PinCapabilities PinCapabilities::PWM(1 << (__LINE__ - START_LINE));
|
||||||
PinCapabilities PinCapabilities::UART(1 << (__LINE__ - START_LINE));
|
PinCapabilities PinCapabilities::UART(1 << (__LINE__ - START_LINE));
|
||||||
PinCapabilities PinCapabilities::ADC(1 << (__LINE__ - START_LINE));
|
PinCapabilities PinCapabilities::ADC(1 << (__LINE__ - START_LINE));
|
||||||
PinCapabilities PinCapabilities::DAC(1 << (__LINE__ - START_LINE));
|
PinCapabilities PinCapabilities::DAC(1 << (__LINE__ - START_LINE));
|
||||||
|
|
||||||
|
PinCapabilities PinCapabilities::Native(1 << (__LINE__ - START_LINE));
|
||||||
PinCapabilities PinCapabilities::I2S(1 << (__LINE__ - START_LINE));
|
PinCapabilities PinCapabilities::I2S(1 << (__LINE__ - START_LINE));
|
||||||
|
PinCapabilities PinCapabilities::Error(1 << (__LINE__ - START_LINE));
|
||||||
|
PinCapabilities PinCapabilities::Void(1 << (__LINE__ - START_LINE));
|
||||||
}
|
}
|
||||||
|
@@ -30,12 +30,18 @@ namespace Pins {
|
|||||||
static PinCapabilities PullDown; // NOTE: Mapped in PinAttributes!
|
static PinCapabilities PullDown; // NOTE: Mapped in PinAttributes!
|
||||||
static PinCapabilities ISR; // NOTE: Mapped in PinAttributes!
|
static PinCapabilities ISR; // NOTE: Mapped in PinAttributes!
|
||||||
|
|
||||||
static PinCapabilities Native;
|
// Other capabilities:
|
||||||
static PinCapabilities ADC;
|
static PinCapabilities ADC;
|
||||||
static PinCapabilities DAC;
|
static PinCapabilities DAC;
|
||||||
static PinCapabilities PWM;
|
static PinCapabilities PWM;
|
||||||
static PinCapabilities UART;
|
static PinCapabilities UART;
|
||||||
|
|
||||||
|
// Each class of pins (e.g. GPIO, etc) should have their own 'capability'. This ensures that we
|
||||||
|
// can compare classes of pins along with their properties by just looking at the capabilities.
|
||||||
|
static PinCapabilities Native;
|
||||||
static PinCapabilities I2S;
|
static PinCapabilities I2S;
|
||||||
|
static PinCapabilities Error;
|
||||||
|
static PinCapabilities Void;
|
||||||
|
|
||||||
inline PinCapabilities operator|(PinCapabilities rhs) { return PinCapabilities(_value | rhs._value); }
|
inline PinCapabilities operator|(PinCapabilities rhs) { return PinCapabilities(_value | rhs._value); }
|
||||||
inline PinCapabilities operator&(PinCapabilities rhs) { return PinCapabilities(_value & rhs._value); }
|
inline PinCapabilities operator&(PinCapabilities rhs) { return PinCapabilities(_value & rhs._value); }
|
||||||
@@ -44,6 +50,6 @@ namespace Pins {
|
|||||||
|
|
||||||
inline operator bool() { return _value != 0; }
|
inline operator bool() { return _value != 0; }
|
||||||
|
|
||||||
inline bool has(PinCapabilities t) { return (*this & t).operator bool(); }
|
inline bool has(PinCapabilities t) { return (*this & t) == t; }
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
@@ -9,10 +9,14 @@
|
|||||||
#include <cstring>
|
#include <cstring>
|
||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
|
|
||||||
// Implementation details of pins.
|
// Implementation details of pins.
|
||||||
class PinDetail {
|
class PinDetail {
|
||||||
|
protected:
|
||||||
|
int _index;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PinDetail() = default;
|
PinDetail(int number) : _index(number) {}
|
||||||
PinDetail(const PinDetail& o) = delete;
|
PinDetail(const PinDetail& o) = delete;
|
||||||
PinDetail(PinDetail&& o) = delete;
|
PinDetail(PinDetail&& o) = delete;
|
||||||
PinDetail& operator=(const PinDetail& o) = delete;
|
PinDetail& operator=(const PinDetail& o) = delete;
|
||||||
@@ -31,6 +35,8 @@ namespace Pins {
|
|||||||
|
|
||||||
virtual String toString() const = 0;
|
virtual String toString() const = 0;
|
||||||
|
|
||||||
|
inline int number() const { return _index; }
|
||||||
|
|
||||||
virtual ~PinDetail() {}
|
virtual ~PinDetail() {}
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
@@ -15,8 +15,8 @@ namespace Pins {
|
|||||||
// probably do with less, but this is as safe as it gets.
|
// probably do with less, but this is as safe as it gets.
|
||||||
PinDetail* _pins[256];
|
PinDetail* _pins[256];
|
||||||
|
|
||||||
// Should be plenty for the GPIO _pins:
|
// According to Arduino.h there are 40 GPIO pins. So, let's start at 41.
|
||||||
static const int NumberNativePins = 64;
|
static const int NumberNativePins = 41;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PinLookup();
|
PinLookup();
|
||||||
@@ -53,6 +53,20 @@ namespace Pins {
|
|||||||
return _pins[index];
|
return _pins[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int FindExisting(PinDetail* instance) const {
|
||||||
|
// Checks if a pin with this number and capabilities already exists:
|
||||||
|
for (int i = 0; i < 256; ++i) {
|
||||||
|
if (_pins[i] != nullptr) {
|
||||||
|
if (_pins[i]->number() == instance->number() && // check number of pin
|
||||||
|
_pins[i]->capabilities() == instance->capabilities()) // check pin capabilities
|
||||||
|
{
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
PinLookup(const PinLookup&) = delete;
|
PinLookup(const PinLookup&) = delete;
|
||||||
PinLookup(PinLookup&&) = delete;
|
PinLookup(PinLookup&&) = delete;
|
||||||
|
|
||||||
|
@@ -1,10 +1,13 @@
|
|||||||
#include "VoidPinDetail.h"
|
#include "VoidPinDetail.h"
|
||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
VoidPinDetail::VoidPinDetail() : _frequency(0), _maxDuty(0) {}
|
VoidPinDetail::VoidPinDetail(int number) : PinDetail(number) {}
|
||||||
VoidPinDetail::VoidPinDetail(const PinOptionsParser& options) : VoidPinDetail() {}
|
VoidPinDetail::VoidPinDetail(const PinOptionsParser& options) : VoidPinDetail() {}
|
||||||
|
|
||||||
PinCapabilities VoidPinDetail::capabilities() const { return PinCapabilities::None; } // Should we?
|
PinCapabilities VoidPinDetail::capabilities() const {
|
||||||
|
// Void pins support basic functionality. It just won't do you any good.
|
||||||
|
return PinCapabilities::Output | PinCapabilities::Input | PinCapabilities::ISR | PinCapabilities::Void;
|
||||||
|
}
|
||||||
|
|
||||||
void VoidPinDetail::write(int high) {}
|
void VoidPinDetail::write(int high) {}
|
||||||
int VoidPinDetail::read() { return 0; }
|
int VoidPinDetail::read() { return 0; }
|
||||||
|
@@ -5,11 +5,8 @@
|
|||||||
|
|
||||||
namespace Pins {
|
namespace Pins {
|
||||||
class VoidPinDetail : public PinDetail {
|
class VoidPinDetail : public PinDetail {
|
||||||
uint32_t _frequency;
|
|
||||||
uint32_t _maxDuty;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
VoidPinDetail();
|
VoidPinDetail(int number = 0);
|
||||||
VoidPinDetail(const PinOptionsParser& options);
|
VoidPinDetail(const PinOptionsParser& options);
|
||||||
|
|
||||||
PinCapabilities capabilities() const override;
|
PinCapabilities capabilities() const override;
|
||||||
|
@@ -450,7 +450,7 @@ void PinSetting::load() {
|
|||||||
esp_err_t err = nvs_get_str(_handle, _keyName, NULL, &len);
|
esp_err_t err = nvs_get_str(_handle, _keyName, NULL, &len);
|
||||||
if (err) {
|
if (err) {
|
||||||
#ifdef PIN_DEBUG
|
#ifdef PIN_DEBUG
|
||||||
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s with: %s", _keyName, _defaultValue);
|
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s as '%s'", _fullName, _defaultValue);
|
||||||
#endif
|
#endif
|
||||||
_storedValue = _defaultValue;
|
_storedValue = _defaultValue;
|
||||||
_currentValue = Pin::create(_defaultValue);
|
_currentValue = Pin::create(_defaultValue);
|
||||||
@@ -460,7 +460,7 @@ void PinSetting::load() {
|
|||||||
err = nvs_get_str(_handle, _keyName, buf, &len);
|
err = nvs_get_str(_handle, _keyName, buf, &len);
|
||||||
if (err) {
|
if (err) {
|
||||||
#ifdef PIN_DEBUG
|
#ifdef PIN_DEBUG
|
||||||
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s with: %s", _keyName, _defaultValue);
|
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s as '%s'", _fullName, _defaultValue);
|
||||||
#endif
|
#endif
|
||||||
_storedValue = _defaultValue;
|
_storedValue = _defaultValue;
|
||||||
_currentValue = Pin::create(_defaultValue);
|
_currentValue = Pin::create(_defaultValue);
|
||||||
@@ -468,7 +468,7 @@ void PinSetting::load() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
#ifdef PIN_DEBUG
|
#ifdef PIN_DEBUG
|
||||||
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s with: %s", _keyName, _storedValue);
|
grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing pin %s as '%s'", _fullName, _storedValue);
|
||||||
#endif
|
#endif
|
||||||
_storedValue = String(buf);
|
_storedValue = String(buf);
|
||||||
_currentValue = Pin::create(_storedValue);
|
_currentValue = Pin::create(_storedValue);
|
||||||
|
@@ -339,7 +339,6 @@ private:
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
PinSetting(const char* description, const char* name, const char* defVal, bool (*checker)(char*));
|
PinSetting(const char* description, const char* name, const char* defVal, bool (*checker)(char*));
|
||||||
|
|
||||||
PinSetting(const char* name, const char* defVal, bool (*checker)(char*) = NULL) : PinSetting(NULL, name, defVal, checker) {};
|
PinSetting(const char* name, const char* defVal, bool (*checker)(char*) = NULL) : PinSetting(NULL, name, defVal, checker) {};
|
||||||
|
|
||||||
void load();
|
void load();
|
||||||
|
@@ -19,7 +19,7 @@ namespace Pins {
|
|||||||
AssertThrow(errorPin.attachInterrupt([](void* arg) {}, CHANGE));
|
AssertThrow(errorPin.attachInterrupt([](void* arg) {}, CHANGE));
|
||||||
AssertThrow(errorPin.detachInterrupt());
|
AssertThrow(errorPin.detachInterrupt());
|
||||||
|
|
||||||
Assert(errorPin.capabilities() == Pin::Capabilities::None, "Incorrect caps");
|
Assert(errorPin.capabilities() == Pin::Capabilities::Error, "Incorrect caps");
|
||||||
|
|
||||||
Assert(errorPin.name() == "ERROR_PIN");
|
Assert(errorPin.name() == "ERROR_PIN");
|
||||||
}
|
}
|
||||||
|
@@ -255,26 +255,26 @@ namespace Pins {
|
|||||||
Pin gpio16 = Pin::create("gpio.16:low");
|
Pin gpio16 = Pin::create("gpio.16:low");
|
||||||
Pin gpio17 = Pin::create("gpio.17");
|
Pin gpio17 = Pin::create("gpio.17");
|
||||||
|
|
||||||
gpio16.setAttr(Pin::Attr::Output | Pin::Attr::InitialHigh);
|
gpio16.setAttr(Pin::Attr::Output);
|
||||||
gpio17.setAttr(Pin::Attr::Input);
|
gpio17.setAttr(Pin::Attr::Input);
|
||||||
|
|
||||||
Assert(false == gpio16.read());
|
Assert(false == gpio16.read());
|
||||||
Assert(true == gpio17.read());
|
Assert(true == gpio17.read());
|
||||||
Assert(false == GPIONative::read(16));
|
Assert(true == GPIONative::read(16));
|
||||||
Assert(true == GPIONative::read(17));
|
Assert(true == GPIONative::read(17));
|
||||||
|
|
||||||
gpio16.on();
|
gpio16.on();
|
||||||
|
|
||||||
Assert(true == gpio16.read());
|
Assert(true == gpio16.read());
|
||||||
Assert(false == gpio17.read());
|
Assert(false == gpio17.read());
|
||||||
Assert(true == GPIONative::read(16));
|
Assert(false == GPIONative::read(16));
|
||||||
Assert(false == GPIONative::read(17));
|
Assert(false == GPIONative::read(17));
|
||||||
|
|
||||||
gpio16.off();
|
gpio16.off();
|
||||||
|
|
||||||
Assert(false == gpio16.read());
|
Assert(false == gpio16.read());
|
||||||
Assert(true == gpio17.read());
|
Assert(true == gpio17.read());
|
||||||
Assert(false == GPIONative::read(16));
|
Assert(true == GPIONative::read(16));
|
||||||
Assert(true == GPIONative::read(17));
|
Assert(true == GPIONative::read(17));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@@ -24,7 +24,38 @@ namespace Pins {
|
|||||||
AssertThrow(unassigned.attachInterrupt([](void* arg) {}, CHANGE));
|
AssertThrow(unassigned.attachInterrupt([](void* arg) {}, CHANGE));
|
||||||
AssertThrow(unassigned.detachInterrupt());
|
AssertThrow(unassigned.detachInterrupt());
|
||||||
|
|
||||||
Assert(unassigned.capabilities() == Pin::Capabilities::None);
|
Assert(unassigned.capabilities().has(Pin::Capabilities::Void));
|
||||||
Assert(unassigned.name().equals(UNDEFINED_PIN));
|
Assert(unassigned.name().equals(""));
|
||||||
|
}
|
||||||
|
|
||||||
|
Test(Undefined, MultipleInstances) {
|
||||||
|
{
|
||||||
|
Pin unassigned = Pin::UNDEFINED;
|
||||||
|
Pin unassigned2 = Pin::UNDEFINED;
|
||||||
|
|
||||||
|
Assert(unassigned == unassigned2, "Should evaluate to true.");
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
Pin unassigned = Pin::create("");
|
||||||
|
Pin unassigned2 = Pin::UNDEFINED;
|
||||||
|
|
||||||
|
Assert(unassigned == unassigned2, "Should evaluate to true.");
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
Pin unassigned = Pin::create("void.2");
|
||||||
|
Pin unassigned2 = Pin::UNDEFINED;
|
||||||
|
|
||||||
|
Assert(unassigned != unassigned2, "Second void pin should match first.");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
{
|
||||||
|
Pin unassigned = Pin::create("void.2");
|
||||||
|
Pin unassigned2 = Pin::create("void.2");
|
||||||
|
|
||||||
|
Assert(unassigned == unassigned2, "Second void pin should match first.");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@@ -5,45 +5,44 @@
|
|||||||
#include <random>
|
#include <random>
|
||||||
|
|
||||||
struct SoftwarePin {
|
struct SoftwarePin {
|
||||||
SoftwarePin() : callback(), argument(nullptr), mode(0), padValue(false), pinMode(0) {}
|
SoftwarePin() : callback(), argument(nullptr), mode(0), driverValue(false), padValue(false), pinMode(0) {}
|
||||||
|
|
||||||
void (*callback)(void*);
|
void (*callback)(void*);
|
||||||
void* argument;
|
void* argument;
|
||||||
int mode;
|
int mode;
|
||||||
|
|
||||||
|
bool driverValue;
|
||||||
bool padValue;
|
bool padValue;
|
||||||
int pinMode;
|
int pinMode;
|
||||||
|
|
||||||
void handleISR() { callback(argument); }
|
void handleISR() { callback(argument); }
|
||||||
|
|
||||||
void reset() {
|
void reset() {
|
||||||
callback = nullptr;
|
callback = nullptr;
|
||||||
argument = nullptr;
|
argument = nullptr;
|
||||||
mode = 0;
|
mode = 0;
|
||||||
padValue = false;
|
driverValue = false;
|
||||||
pinMode = 0;
|
padValue = false;
|
||||||
|
pinMode = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void handlePadChangeWithHystesis(bool newval) {
|
void handlePadChangeWithHystesis(bool newval) {
|
||||||
auto oldval = padValue;
|
auto oldval = padValue;
|
||||||
if (oldval != newval)
|
if (oldval != newval) {
|
||||||
{
|
std::default_random_engine generator;
|
||||||
std::default_random_engine generator;
|
|
||||||
std::normal_distribution<double> distribution(5, 2);
|
std::normal_distribution<double> distribution(5, 2);
|
||||||
int count = int(distribution(generator));
|
int count = int(distribution(generator));
|
||||||
|
|
||||||
// Bound it a bit
|
// Bound it a bit
|
||||||
if (count < 0) {
|
if (count < 0) {
|
||||||
count = 0;
|
count = 0;
|
||||||
}
|
} else if (count > 8) {
|
||||||
else if (count > 8) {
|
|
||||||
count = 8;
|
count = 8;
|
||||||
}
|
}
|
||||||
count = count * 2 + 1; // make it odd.
|
count = count * 2 + 1; // make it odd.
|
||||||
|
|
||||||
auto currentVal = oldval;
|
auto currentVal = oldval;
|
||||||
for (int i = 0; i < count; ++i)
|
for (int i = 0; i < count; ++i) {
|
||||||
{
|
|
||||||
currentVal = !currentVal;
|
currentVal = !currentVal;
|
||||||
handlePadChange(currentVal);
|
handlePadChange(currentVal);
|
||||||
}
|
}
|
||||||
@@ -52,24 +51,23 @@ struct SoftwarePin {
|
|||||||
|
|
||||||
void handlePadChange(bool newval) {
|
void handlePadChange(bool newval) {
|
||||||
auto oldval = padValue;
|
auto oldval = padValue;
|
||||||
if (oldval != newval)
|
if (oldval != newval) {
|
||||||
{
|
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case RISING:
|
case RISING:
|
||||||
if (!oldval && newval) {
|
if (!oldval && newval) {
|
||||||
handleISR();
|
handleISR();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case FALLING:
|
case FALLING:
|
||||||
if (oldval && !newval) {
|
if (oldval && !newval) {
|
||||||
handleISR();
|
handleISR();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case CHANGE:
|
case CHANGE:
|
||||||
if (oldval != newval) {
|
if (oldval != newval) {
|
||||||
handleISR();
|
handleISR();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
padValue = newval;
|
padValue = newval;
|
||||||
}
|
}
|
||||||
@@ -103,8 +101,18 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
void setMode(int index, int mode) {
|
void setMode(int index, int mode) {
|
||||||
auto& pin = pins[index];
|
auto& pin = pins[index];
|
||||||
pin.pinMode = mode;
|
auto oldModeHasOutput = (pin.pinMode & OUTPUT) == OUTPUT;
|
||||||
|
pin.pinMode = mode;
|
||||||
|
auto modeHasOutput = (pin.pinMode & OUTPUT) == OUTPUT;
|
||||||
|
|
||||||
|
if (modeHasOutput && !oldModeHasOutput) {
|
||||||
|
if (virtualCircuit != nullptr) {
|
||||||
|
virtualCircuit(pins, index, pin.driverValue);
|
||||||
|
} else if (circuitHandlesHystesis) {
|
||||||
|
pins[index].handlePadChange(pin.driverValue);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeOutput(int index, bool value) {
|
void writeOutput(int index, bool value) {
|
||||||
@@ -115,10 +123,11 @@ public:
|
|||||||
virtualCircuit(pins, index, value);
|
virtualCircuit(pins, index, value);
|
||||||
} else if (circuitHandlesHystesis) {
|
} else if (circuitHandlesHystesis) {
|
||||||
pins[index].handlePadChange(value);
|
pins[index].handlePadChange(value);
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
pins[index].handlePadChangeWithHystesis(value);
|
pins[index].handlePadChangeWithHystesis(value);
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
pins[index].driverValue = value;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user