diff --git a/_Boards/Atmel/Board_Mega/MFBoards.h b/_Boards/Atmel/Board_Mega/MFBoards.h index 650baefe..d97fd4b3 100644 --- a/_Boards/Atmel/Board_Mega/MFBoards.h +++ b/_Boards/Atmel/Board_Mega/MFBoards.h @@ -48,10 +48,6 @@ #define MAX_INPUT_SHIFTERS 4 #define MAX_DIGIN_MUX 4 -#define STEPS 64 -#define STEPPER_SPEED 400 // 300 already worked, 467, too? -#define STEPPER_ACCEL 800 - #define MOBIFLIGHT_TYPE "MobiFlight Mega" #define MOBIFLIGHT_SERIAL "1234567890" #define MOBIFLIGHT_NAME "MobiFlight Mega" diff --git a/_Boards/Atmel/Board_ProMicro/MFBoards.h b/_Boards/Atmel/Board_ProMicro/MFBoards.h index 48f33b8e..ab843239 100644 --- a/_Boards/Atmel/Board_ProMicro/MFBoards.h +++ b/_Boards/Atmel/Board_ProMicro/MFBoards.h @@ -48,10 +48,6 @@ #define MAX_INPUT_SHIFTERS 2 #define MAX_DIGIN_MUX 3 -#define STEPS 64 -#define STEPPER_SPEED 400 // 300 already worked, 467, too? -#define STEPPER_ACCEL 800 - #define MOBIFLIGHT_TYPE "MobiFlight Micro" #define MOBIFLIGHT_SERIAL "0987654321" #define MOBIFLIGHT_NAME "MobiFlight Micro" diff --git a/_Boards/Atmel/Board_Uno/MFBoards.h b/_Boards/Atmel/Board_Uno/MFBoards.h index 1cb74bc2..f8b050cb 100644 --- a/_Boards/Atmel/Board_Uno/MFBoards.h +++ b/_Boards/Atmel/Board_Uno/MFBoards.h @@ -48,10 +48,6 @@ #define MAX_INPUT_SHIFTERS 2 #define MAX_DIGIN_MUX 3 -#define STEPS 64 -#define STEPPER_SPEED 400 // 300 already worked, 467, too? -#define STEPPER_ACCEL 800 - #define MOBIFLIGHT_TYPE "MobiFlight Uno" #define MOBIFLIGHT_SERIAL "0987654321" #define MOBIFLIGHT_NAME "MobiFlight Uno" diff --git a/platformio.ini b/platformio.ini index f975a3b8..34eb6085 100644 --- a/platformio.ini +++ b/platformio.ini @@ -28,7 +28,7 @@ lib_deps_Atmel = arduino-libraries/Servo @ 1.1.8 build_flags = -DMF_REDUCE_FUNCT_LEDCONTROL - -DMAXCALLBACKS=30 + -DMAXCALLBACKS=35 -DSERIAL_RX_BUFFER_SIZE=96 -DMESSENGERBUFFERSIZE=96 -DMAXSTREAMBUFFERSIZE=96 diff --git a/src/CommandMessenger.cpp b/src/CommandMessenger.cpp index e7ad8b51..a82c526d 100644 --- a/src/CommandMessenger.cpp +++ b/src/CommandMessenger.cpp @@ -58,6 +58,7 @@ void attachCommandCallbacks() cmdMessenger.attach(kSetStepper, Stepper::OnSet); cmdMessenger.attach(kResetStepper, Stepper::OnReset); cmdMessenger.attach(kSetZeroStepper, Stepper::OnSetZero); + cmdMessenger.attach(kSetStepperSpeedAccel, Stepper::OnSetSpeedAccel); #endif #if MF_SERVO_SUPPORT == 1 diff --git a/src/Config.cpp b/src/Config.cpp index af8085fd..fd606ec4 100644 --- a/src/Config.cpp +++ b/src/Config.cpp @@ -232,10 +232,10 @@ void readConfig() { if (configLength == 0) // do nothing if no config is available return; - uint16_t addreeprom = MEM_OFFSET_CONFIG; // define first memory location where config is saved in EEPROM - uint16_t addrbuffer = 0; // and start with first memory location from nameBuffer - char params[6] = ""; - uint8_t command = readUintFromEEPROM(&addreeprom); // read the first value from EEPROM, it's a device definition + uint16_t addreeprom = MEM_OFFSET_CONFIG; // define first memory location where config is saved in EEPROM + uint16_t addrbuffer = 0; // and start with first memory location from nameBuffer + char params[8] = ""; // buffer for reading parameters from EEPROM and sending to ::Add() function of device + char command = readUintFromEEPROM(&addreeprom); // read the first value from EEPROM, it's a device definition bool copy_success = true; // will be set to false if copying input names to nameBuffer exceeds array dimensions // not required anymore when pins instead of names are transferred to the UI @@ -270,7 +270,7 @@ void readConfig() #endif #if MF_STEPPER_SUPPORT == 1 - case kTypeStepperDeprecated: + case kTypeStepperDeprecated1: // this is for backwards compatibility params[0] = readUintFromEEPROM(&addreeprom); // Pin1 number params[1] = readUintFromEEPROM(&addreeprom); // Pin2 number @@ -280,10 +280,8 @@ void readConfig() Stepper::Add(params[0], params[1], params[2], params[3], 0); copy_success = readEndCommandFromEEPROM(&addreeprom); // check EEPROM until end of name break; -#endif -#if MF_STEPPER_SUPPORT == 1 - case kTypeStepper: + case kTypeStepperDeprecated2: params[0] = readUintFromEEPROM(&addreeprom); // Pin1 number params[1] = readUintFromEEPROM(&addreeprom); // Pin2 number params[2] = readUintFromEEPROM(&addreeprom); // Pin3 number @@ -292,6 +290,21 @@ void readConfig() Stepper::Add(params[0], params[1], params[2], params[3], params[4]); copy_success = readEndCommandFromEEPROM(&addreeprom); // check EEPROM until end of name break; + + case kTypeStepper: + params[0] = readUintFromEEPROM(&addreeprom); // Pin1 number + params[1] = readUintFromEEPROM(&addreeprom); // Pin2 number + params[2] = readUintFromEEPROM(&addreeprom); // Pin3 number + params[3] = readUintFromEEPROM(&addreeprom); // Pin4 number + params[4] = readUintFromEEPROM(&addreeprom); // Button number + params[5] = readUintFromEEPROM(&addreeprom); // Stepper Mode + params[6] = readUintFromEEPROM(&addreeprom); // backlash + params[7] = readUintFromEEPROM(&addreeprom); // deactivate output + // there is an additional 9th parameter stored in the config (profileID) which is not needed in the firmware + // and therefor not read in, it is just skipped like the name with reading until end of command + Stepper::Add(params[0], params[1], params[2], params[3], params[4], params[5], params[6], params[7]); + copy_success = readEndCommandFromEEPROM(&addreeprom); // check EEPROM until end of name + break; #endif #if MF_SERVO_SUPPORT == 1 diff --git a/src/MF_Stepper/MFStepper.cpp b/src/MF_Stepper/MFStepper.cpp index 14ffc1b4..b894866e 100644 --- a/src/MF_Stepper/MFStepper.cpp +++ b/src/MF_Stepper/MFStepper.cpp @@ -7,32 +7,72 @@ #include "mobiflight.h" #include "MFStepper.h" +enum { // enumeration for stepper mode + FULL4WIRE, + HALF4WIRE, + DRIVER +}; + +enum { + MOVE_CCW, + MOVE_CW +}; + MFStepper::MFStepper() { _initialized = false; } -void MFStepper::attach(uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, uint8_t btnPin5) +void MFStepper::attach(uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, uint8_t btnPin5, uint8_t mode, int8_t backlash, bool deactivateOutput) { if (!FitInMemory(sizeof(AccelStepper))) { // Error Message to Connector cmdMessenger.sendCmd(kStatus, F("MFStepper does not fit in Memory")); return; } - if (pin2 == pin4 && pin1 == pin3) // if pin1/2 are identical to pin3/4 - { // init new stepper with external driver (step and direction) + uint16_t maxSpeed = 0; + uint16_t Accel = 0; + + switch (mode) { + case FULL4WIRE: + maxSpeed = STEPPER_SPEED; + Accel = STEPPER_ACCEL; + if (pin1 == pin3 && pin2 == pin4) // for backwards compatibility + _stepper = new (allocateMemory(sizeof(AccelStepper))) AccelStepper(AccelStepper::DRIVER, pin1, pin2); + else + _stepper = new (allocateMemory(sizeof(AccelStepper))) AccelStepper(AccelStepper::FULL4WIRE, pin4, pin2, pin1, pin3); + break; + case HALF4WIRE: + _stepper = new (allocateMemory(sizeof(AccelStepper))) AccelStepper(AccelStepper::HALF4WIRE, pin4, pin2, pin1, pin3); + maxSpeed = STEPPER_SPEED; + Accel = STEPPER_ACCEL; + break; + case DRIVER: _stepper = new (allocateMemory(sizeof(AccelStepper))) AccelStepper(AccelStepper::DRIVER, pin1, pin2); - } else { // otherwise init new stepper in full 4 wire mode - _stepper = new (allocateMemory(sizeof(AccelStepper))) AccelStepper(AccelStepper::FULL4WIRE, pin4, pin2, pin1, pin3); + maxSpeed = STEPPER_SPEED; + Accel = STEPPER_ACCEL; + break; + default: + _initialized = false; + return; + break; } + + _stepper->setMaxSpeed(maxSpeed); + _stepper->setAcceleration(Accel); _zeroPin = btnPin5; _zeroPinState = HIGH; if (_zeroPin) { - pinMode(_zeroPin, INPUT_PULLUP); // set pin to input + pinMode(_zeroPin, INPUT_PULLUP); } - _initialized = true; - _resetting = false; + + _backlash = backlash; + _deactivateOutput = deactivateOutput; + _initialized = true; + _resetting = false; + _isStopped = true; + _inMove = MOVE_CW; } void MFStepper::detach() @@ -40,12 +80,23 @@ void MFStepper::detach() _initialized = false; } -void MFStepper::moveTo(long absolute) +void MFStepper::moveTo(long newPosition) { - _resetting = false; - if (_targetPos != absolute) { - _targetPos = absolute; - _stepper->moveTo(absolute); + _resetting = false; + long currentPosition = _stepper->currentPosition(); + + if (_targetPos != newPosition) { + if (_deactivateOutput && _isStopped) { + _stepper->enableOutputs(); + _isStopped = false; + } + if (_inMove == MOVE_CW && newPosition < currentPosition && newPosition < _targetPos) + _inMove = MOVE_CCW; + if (_inMove == MOVE_CCW && newPosition > currentPosition && newPosition > _targetPos) + _inMove = MOVE_CW; + + _stepper->moveTo(newPosition + _backlash * _inMove); + _targetPos = newPosition; } } @@ -57,6 +108,9 @@ uint8_t MFStepper::getZeroPin() void MFStepper::setZero() { _stepper->setCurrentPosition(0); + if (_inMove == MOVE_CW) { + _stepper->moveTo(-_backlash); + } } void MFStepper::setZeroInReset() @@ -80,6 +134,10 @@ void MFStepper::update() { _stepper->run(); checkZeroPin(); + if (_stepper->currentPosition() == (_targetPos + _backlash * _inMove) && _deactivateOutput) { + _stepper->disableOutputs(); + _isStopped = true; + } } void MFStepper::reset() @@ -99,14 +157,22 @@ void MFStepper::reset() _stepper->moveTo(-100000); } -void MFStepper::setMaxSpeed(float speed) +void MFStepper::setMaxSpeed(uint16_t speed) { _stepper->setMaxSpeed(speed); } -void MFStepper::setAcceleration(float acceleration) +void MFStepper::setAcceleration(uint16_t acceleration) { _stepper->setAcceleration(acceleration); } +void MFStepper::powerSavingMode(bool state) +{ + if (state) + _stepper->disableOutputs(); + else + _stepper->enableOutputs(); +} + // MFStepper.cpp diff --git a/src/MF_Stepper/MFStepper.h b/src/MF_Stepper/MFStepper.h index eb890200..2e914e16 100644 --- a/src/MF_Stepper/MFStepper.h +++ b/src/MF_Stepper/MFStepper.h @@ -11,17 +11,21 @@ class MFStepper { +#define STEPPER_SPEED 400 +#define STEPPER_ACCEL 800 + public: MFStepper(); - void attach(uint8_t pin1 = 1, uint8_t pin2 = 2, uint8_t pin3 = 3, uint8_t pin4 = 4, uint8_t btnPin1 = 0); + void attach(uint8_t pin1 = 1, uint8_t pin2 = 2, uint8_t pin3 = 3, uint8_t pin4 = 4, uint8_t btnPin1 = 0, uint8_t mode = 0, int8_t backlash = 0, bool deactivateOutput = false); void detach(); void update(); void reset(); void moveTo(long absolute); - void setMaxSpeed(float speed); - void setAcceleration(float acceleration); + void setMaxSpeed(uint16_t speed); + void setAcceleration(uint16_t acceleration); void setZero(); uint8_t getZeroPin(); + void powerSavingMode(bool state); private: bool _initialized; @@ -30,9 +34,13 @@ class MFStepper uint8_t _zeroPin; uint8_t _zeroPinState; long _targetPos; + uint8_t _backlash; + bool _deactivateOutput; + bool _inMove; + bool _isStopped; - void checkZeroPin(void); - void setZeroInReset(void); + void checkZeroPin(void); + void setZeroInReset(void); }; // MFStepper.h diff --git a/src/MF_Stepper/Stepper.cpp b/src/MF_Stepper/Stepper.cpp index 8ad6eabb..580d93fb 100644 --- a/src/MF_Stepper/Stepper.cpp +++ b/src/MF_Stepper/Stepper.cpp @@ -13,7 +13,7 @@ namespace Stepper MFStepper *steppers[MAX_STEPPERS]; uint8_t steppersRegistered = 0; - void Add(int pin1, int pin2, int pin3, int pin4, int btnPin1) + void Add(uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, uint8_t btnPin1, uint8_t mode, int8_t backlash, bool deactivateOutput) { if (steppersRegistered == MAX_STEPPERS) return; @@ -23,11 +23,10 @@ namespace Stepper cmdMessenger.sendCmd(kStatus, F("Stepper does not fit in Memory!")); return; } + steppers[steppersRegistered] = new (allocateMemory(sizeof(MFStepper))) MFStepper; - steppers[steppersRegistered]->attach(pin1, pin2, pin3, pin4, btnPin1); - steppers[steppersRegistered]->setMaxSpeed(STEPPER_SPEED); - steppers[steppersRegistered]->setAcceleration(STEPPER_ACCEL); - // autoreset is not released yet + steppers[steppersRegistered]->attach(pin1, pin2, pin3, pin4, btnPin1, mode, backlash, deactivateOutput); + if (btnPin1 > 0) { // this triggers the auto reset if we need to reset steppers[steppersRegistered]->reset(); @@ -54,8 +53,8 @@ namespace Stepper void OnSet() { - int stepper = cmdMessenger.readInt16Arg(); - long newPos = cmdMessenger.readInt32Arg(); + uint8_t stepper = (uint8_t)cmdMessenger.readInt16Arg(); + long newPos = cmdMessenger.readInt32Arg(); if (stepper >= steppersRegistered) return; @@ -65,7 +64,7 @@ namespace Stepper void OnReset() { - int stepper = cmdMessenger.readInt16Arg(); + uint8_t stepper = (uint8_t)cmdMessenger.readInt16Arg(); if (stepper >= steppersRegistered) return; @@ -75,7 +74,7 @@ namespace Stepper void OnSetZero() { - int stepper = cmdMessenger.readInt16Arg(); + uint8_t stepper = (uint8_t)cmdMessenger.readInt16Arg(); if (stepper >= steppersRegistered) return; @@ -83,12 +82,32 @@ namespace Stepper setLastCommandMillis(); } + void OnSetSpeedAccel() + { + uint8_t stepper = (uint8_t)cmdMessenger.readInt16Arg(); + uint16_t maxSpeed = cmdMessenger.readInt16Arg(); + uint16_t maxAccel = cmdMessenger.readInt16Arg(); + + if (stepper >= steppersRegistered) + return; + steppers[stepper]->setMaxSpeed(maxSpeed); + steppers[stepper]->setAcceleration(maxAccel); + } + void update() { for (uint8_t i = 0; i < steppersRegistered; i++) { steppers[i]->update(); } } + + void PowerSave(bool state) + { + for (uint8_t i = 0; i < steppersRegistered; ++i) { + steppers[i]->powerSavingMode(state); + } + } + } // namespace // Stepper.cpp diff --git a/src/MF_Stepper/Stepper.h b/src/MF_Stepper/Stepper.h index 6331e128..455272c7 100644 --- a/src/MF_Stepper/Stepper.h +++ b/src/MF_Stepper/Stepper.h @@ -1,6 +1,6 @@ -// +// // Stepper.h -// +// // (C) MobiFlight Project 2022 // @@ -8,12 +8,14 @@ namespace Stepper { - void Add(int pin1, int pin2, int pin3, int pin4, int btnPin1); + void Add(uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, uint8_t btnPin1, uint8_t mode = 0, int8_t backlash = 0, bool deactivateOutput = false); void Clear(); void OnSet(); void OnReset(); void OnSetZero(); void update(); + void OnSetSpeedAccel(); + void PowerSave(bool state); } // Stepper.h \ No newline at end of file diff --git a/src/commandmessenger.h b/src/commandmessenger.h index 855286a4..4e3382f7 100644 --- a/src/commandmessenger.h +++ b/src/commandmessenger.h @@ -43,11 +43,12 @@ enum { kAnalogChange, // 28 kInputShifterChange, // 29 kDigInMuxChange, // 30 + kSetStepperSpeedAccel, // 31 kDebug = 0xFF // 255 }; -void attachCommandCallbacks(); -uint32_t getLastCommandMillis(); -void setLastCommandMillis(); +void attachCommandCallbacks(); +uint32_t getLastCommandMillis(); +void setLastCommandMillis(); extern CmdMessenger cmdMessenger; \ No newline at end of file diff --git a/src/config.h b/src/config.h index ba6d43e3..a882abad 100644 --- a/src/config.h +++ b/src/config.h @@ -12,16 +12,17 @@ enum { kTypeEncoderSingleDetent, // 2 (retained for backwards compatibility, use kTypeEncoder for new configs) kTypeOutput, // 3 kTypeLedSegment, // 4 - kTypeStepperDeprecated, // 5 (keep for backwards compatibility, doesn't support autohome) + kTypeStepperDeprecated1, // 5 (keep for backwards compatibility, doesn't support autohome) kTypeServo, // 6 kTypeLcdDisplayI2C, // 7 kTypeEncoder, // 8 - kTypeStepper, // 9 (new stepper type with auto zero support if btnPin is > 0) + kTypeStepperDeprecated2, // 9 (keep for backwards compatibility, stepper type with auto zero support if btnPin is > 0) kTypeOutputShifter, // 10 Shift register support (example: 74HC595, TLC592X) kTypeAnalogInput, // 11 Analog Device with 1 pin kTypeInputShifter, // 12 Input shift register support (example: 74HC165) kTypeMuxDriver, // 13 Multiplexer selector support (generates select outputs) kTypeDigInMux, // 14 Digital input multiplexer support (example: 74HCT4067, 74HCT4051) + kTypeStepper // new stepper type with settings for backlash and deactivate output }; void loadConfig(void); diff --git a/src/mobiflight.cpp b/src/mobiflight.cpp index 4dc39817..865be740 100644 --- a/src/mobiflight.cpp +++ b/src/mobiflight.cpp @@ -112,6 +112,9 @@ void SetPowerSavingMode(bool state) #if MF_SEGMENT_SUPPORT == 1 LedSegment::PowerSave(state); #endif +#if MF_STEPPER_SUPPORT == 1 + Stepper::PowerSave(state); +#endif #ifdef DEBUG2CMDMESSENGER if (state)