1
0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-09-01 18:32:37 +02:00

Added 'getAttr' to pin class. Needed for RMT

Got rid of ifdefs in standard stepper and most old config values.
This commit is contained in:
Stefan de Bruijn
2021-05-20 14:34:48 +02:00
parent 8a50ffccca
commit f346da7013
15 changed files with 88 additions and 53 deletions

View File

@@ -45,50 +45,53 @@ namespace Motors {
}
void StandardStepper::read_settings() { init_step_dir_pins(); }
void StandardStepper::init_step_dir_pins() {
auto axisIndex = axis_index();
_invert_step_pin = bitnum_istrue(step_invert_mask->get(), axisIndex);
_invert_dir_pin = bitnum_istrue(dir_invert_mask->get(), axisIndex);
_invert_step = _step_pin.getAttr().has(Pin::Attr::ActiveLow);
_invert_disable = _disable_pin.getAttr().has(Pin::Attr::ActiveLow);
_dir_pin.setAttr(Pin::Attr::Output);
#ifdef USE_RMT_STEPS
rmtConfig.rmt_mode = RMT_MODE_TX;
rmtConfig.clk_div = 20;
rmtConfig.mem_block_num = 2;
rmtConfig.tx_config.loop_en = false;
rmtConfig.tx_config.carrier_en = false;
rmtConfig.tx_config.carrier_freq_hz = 0;
rmtConfig.tx_config.carrier_duty_percent = 50;
rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW;
rmtConfig.tx_config.idle_output_en = true;
if (_use_rmt_steps)
{
rmtConfig.rmt_mode = RMT_MODE_TX;
rmtConfig.clk_div = 20;
rmtConfig.mem_block_num = 2;
rmtConfig.tx_config.loop_en = false;
rmtConfig.tx_config.carrier_en = false;
rmtConfig.tx_config.carrier_freq_hz = 0;
rmtConfig.tx_config.carrier_duty_percent = 50;
rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW;
rmtConfig.tx_config.idle_output_en = true;
auto stepPulseDelay = direction_delay_microseconds->get();
rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4;
auto stepPulseDelay = _direction_delay_ms;
rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4;
rmtItem[0].duration1 = 4 * pulse_microseconds->get();
rmtItem[1].duration0 = 0;
rmtItem[1].duration1 = 0;
rmtItem[0].duration1 = 4 * pulse_microseconds->get();
rmtItem[1].duration0 = 0;
rmtItem[1].duration1 = 0;
_rmt_chan_num = get_next_RMT_chan_num();
if (_rmt_chan_num == RMT_CHANNEL_MAX) {
return;
_rmt_chan_num = get_next_RMT_chan_num();
if (_rmt_chan_num == RMT_CHANNEL_MAX) {
return;
}
auto step_pin_gpio = _step_pin.getNative(Pin::Capabilities::Output);
rmt_set_source_clk(_rmt_chan_num, RMT_BASECLK_APB);
rmtConfig.channel = _rmt_chan_num;
rmtConfig.tx_config.idle_level = _invert_step ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
rmtConfig.gpio_num = gpio_num_t(step_pin_gpio);
rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
rmt_config(&rmtConfig);
rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
}
else {
_step_pin.setAttr(Pin::Attr::Output);
}
auto step_pin_gpio = _step_pin.getNative(Pin::Capabilities::Output);
rmt_set_source_clk(_rmt_chan_num, RMT_BASECLK_APB);
rmtConfig.channel = _rmt_chan_num;
rmtConfig.tx_config.idle_level = _invert_step_pin ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
rmtConfig.gpio_num = gpio_num_t(step_pin_gpio);
rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
rmt_config(&rmtConfig);
rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
#else
_step_pin.setAttr(Pin::Attr::Output);
#endif // USE_RMT_STEPS
_disable_pin.setAttr(Pin::Attr::Output);
}
@@ -104,21 +107,24 @@ namespace Motors {
}
void StandardStepper::step() {
#ifdef USE_RMT_STEPS
RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1;
RMT.conf_ch[_rmt_chan_num].conf1.tx_start = 1;
#else
_step_pin.write(!_invert_step_pin);
#endif // USE_RMT_STEPS
if (_use_rmt_steps)
{
RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1;
RMT.conf_ch[_rmt_chan_num].conf1.tx_start = 1;
}
else {
_step_pin.on();
}
}
void StandardStepper::unstep() {
#ifndef USE_RMT_STEPS
_step_pin.write(_invert_step_pin);
#endif // USE_RMT_STEPS
if (_use_rmt_steps)
{
_step_pin.off();
}
}
void StandardStepper::set_direction(bool dir) { _dir_pin.write(dir ^ _invert_dir_pin); }
void StandardStepper::set_direction(bool dir) { _dir_pin.write(dir); }
void StandardStepper::set_disable(bool disable) { _disable_pin.write(disable); }

View File

@@ -25,15 +25,14 @@ namespace Motors {
protected:
void config_message() override;
#ifdef USE_RMT_STEPS
rmt_channel_t _rmt_chan_num;
#endif
bool _invert_step_pin;
bool _invert_dir_pin;
bool _use_rmt_steps = false;
Pin _step_pin;
Pin _dir_pin;
Pin _disable_pin;
int _direction_delay_ms = 0;
int _enable_delay_ms = 0;
// Configuration handlers:
void validate() const override {
Assert(!_step_pin.undefined(), "Step pin should be configured.");
@@ -41,15 +40,24 @@ namespace Motors {
}
void handle(Configuration::HandlerBase& handler) override {
handler.handle("use_rmt_steps", _use_rmt_steps);
handler.handle("step", _step_pin);
handler.handle("direction", _dir_pin);
handler.handle("disable", _disable_pin);
handler.handle("direction_delay_ms", _direction_delay_ms);
handler.handle("enable_delay_ms", _enable_delay_ms);
}
// Name of the configurable. Must match the name registered in the cpp file.
const char* name() const override { return "standard_stepper"; }
private:
// Initialized after configuration for RMT steps:
bool _invert_step;
bool _invert_disable;
rmt_channel_t _rmt_chan_num;
static rmt_channel_t get_next_RMT_chan_num();
static rmt_item32_t rmtItem[2];
static rmt_config_t rmtConfig;

View File

@@ -89,6 +89,11 @@ public:
detail->setAttr(attributes);
}
inline Attr getAttr() const {
auto detail = Pins::PinLookup::_instance.GetPin(_index);
return detail->getAttr();
}
inline void on() const { write(1); }
inline void off() const { write(0); }

View File

@@ -70,6 +70,8 @@ namespace Pins {
_implementation->setAttr(value);
}
PinAttributes DebugPinDetail::getAttr() const { return _implementation->getAttr(); }
void DebugPinDetail::CallbackHandler::handle(void* arg) {
auto handler = static_cast<CallbackHandler*>(arg);
if (handler->_myPin->shouldEvent()) {

View File

@@ -33,6 +33,8 @@ namespace Pins {
void write(int high) override;
int read() override;
void setAttr(PinAttributes value) override;
PinAttributes getAttr() const override;
// ISR's:
void attachInterrupt(void (*callback)(void*), void* arg, int mode) override;

View File

@@ -10,6 +10,7 @@ namespace Pins {
int ErrorPinDetail::read() { Assert(false, "Cannot read from an error pin."); }
void ErrorPinDetail::setAttr(PinAttributes value) { /* Fine, this won't get you anywhere. */
}
PinAttributes ErrorPinDetail::getAttr() const { return PinAttributes::None; }
String ErrorPinDetail::toString() const { return "ERROR_PIN"; }
}

View File

@@ -14,6 +14,7 @@ namespace Pins {
void write(int high) override;
int read() override;
void setAttr(PinAttributes value) override;
PinAttributes getAttr() const override;
String toString() const override;

View File

@@ -109,6 +109,8 @@ namespace Pins {
}
}
PinAttributes GPIOPinDetail::getAttr() const { return _attributes; }
PinCapabilities GPIOPinDetail::capabilities() const { return _capabilities; }
void GPIOPinDetail::write(int high) {

View File

@@ -19,6 +19,7 @@ namespace Pins {
void write(int high) override;
int read() override;
void setAttr(PinAttributes value) override;
PinAttributes getAttr() const override;
// ISR's:
void attachInterrupt(void (*callback)(void*), void* arg, int mode) override;

View File

@@ -61,6 +61,8 @@ namespace Pins {
}
}
PinAttributes I2SOPinDetail::getAttr() const { return _attributes; }
String I2SOPinDetail::toString() const { return String("I2SO.") + int(_index); }
}

View File

@@ -18,6 +18,7 @@ namespace Pins {
void write(int high) override;
int read() override;
void setAttr(PinAttributes value) override;
PinAttributes getAttr() const override;
String toString() const override;

View File

@@ -25,9 +25,10 @@ namespace Pins {
virtual PinCapabilities capabilities() const = 0;
// I/O:
virtual void write(int high) = 0;
virtual int read() = 0;
virtual void setAttr(PinAttributes value) = 0;
virtual void write(int high) = 0;
virtual int read() = 0;
virtual void setAttr(PinAttributes value) = 0;
virtual PinAttributes getAttr() const = 0;
// ISR's.
virtual void attachInterrupt(void (*callback)(void*), void* arg, int mode);

View File

@@ -12,6 +12,7 @@ namespace Pins {
void VoidPinDetail::write(int high) {}
int VoidPinDetail::read() { return 0; }
void VoidPinDetail::setAttr(PinAttributes value) {}
PinAttributes VoidPinDetail::getAttr() const { return PinAttributes::None; }
String VoidPinDetail::toString() const { return ""; }

View File

@@ -15,6 +15,7 @@ namespace Pins {
void write(int high) override;
int read() override;
void setAttr(PinAttributes value) override;
PinAttributes getAttr() const override;
String toString() const override;

View File

@@ -28,6 +28,7 @@ public:
};
Uart(int uart_num);
bool setHalfDuplex();
bool setPins(int tx_pin, int rx_pin, int rts_pin = -1, int cts_pin = -1);
void begin(unsigned long baud, Data dataBits, Stop stopBits, Parity parity);