diff --git a/IO_EXIOExpander.h b/IO_EXIOExpander.h
index 8ec1f48..afe6457 100644
--- a/IO_EXIOExpander.h
+++ b/IO_EXIOExpander.h
@@ -49,6 +49,17 @@
*/
class EXIOExpander : public IODevice {
public:
+
+ enum ProfileType : uint8_t {
+ Instant = 0, // Moves immediately between positions (if duration not specified)
+ UseDuration = 0, // Use specified duration
+ Fast = 1, // Takes around 500ms end-to-end
+ Medium = 2, // 1 second end-to-end
+ Slow = 3, // 2 seconds end-to-end
+ Bounce = 4, // For semaphores/turnouts with a bit of bounce!!
+ NoPowerOff = 0x80, // Flag to be ORed in to suppress power off after move.
+ };
+
static void create(VPIN vpin, int nPins, uint8_t i2cAddress) {
if (checkNoOverlap(vpin, nPins, i2cAddress)) new EXIOExpander(vpin, nPins, i2cAddress);
}
@@ -59,6 +70,12 @@ private:
_firstVpin = firstVpin;
_nPins = nPins;
_i2cAddress = i2cAddress;
+ // To save RAM, space for servo configuration is not allocated unless a pin is used.
+ // Initialise the pointers to NULL.
+ _servoData = (ServoData**) calloc(_nPins, sizeof(ServoData*));
+ for (int i=0; i<_nPins; i++) {
+ _servoData[i] = NULL;
+ }
addDevice(this);
}
@@ -146,10 +163,19 @@ private:
// Main loop, collect both digital and analogue pin states continuously (faster sensor/input reads)
void _loop(unsigned long currentMicros) override {
(void)currentMicros; // remove warning
+ if (_deviceState == DEVSTATE_FAILED) return;
_command1Buffer[0] = EXIORDD;
I2CManager.read(_i2cAddress, _digitalInputStates, _digitalPinBytes, _command1Buffer, 1);
_command1Buffer[0] = EXIORDAN;
I2CManager.read(_i2cAddress, _analogueInputStates, _analoguePinBytes, _command1Buffer, 1);
+ if ((currentMicros - _lastRefresh) / 1000UL > refreshInterval) {
+ _lastRefresh = currentMicros;
+ for (int pin=0; pin<_nPins; pin++) {
+ if (_servoData[pin] != NULL) {
+ updatePosition(pin);
+ }
+ }
+ }
}
// Obtain the correct analogue input value
@@ -167,25 +193,114 @@ private:
// Obtain the correct digital input value
int _read(VPIN vpin) override {
+ if (_deviceState == DEVSTATE_FAILED) return 0;
int pin = vpin - _firstVpin;
- uint8_t pinByte = pin / 8;
- bool value = bitRead(_digitalInputStates[pinByte], pin - pinByte * 8);
- return value;
- }
-
- void _write(VPIN vpin, int value) override {
- int pin = vpin - _firstVpin;
- _digitalOutBuffer[0] = EXIOWRD;
- _digitalOutBuffer[1] = pin;
- _digitalOutBuffer[2] = value;
- I2CManager.read(_i2cAddress, _command1Buffer, 1, _digitalOutBuffer, 3);
- if (_command1Buffer[0] != EXIORDY) {
- DIAG(F("Vpin %d cannot be used as a digital output pin"), (int)vpin);
+ if (_servoData[pin] == NULL) {
+ uint8_t pinByte = pin / 8;
+ bool value = bitRead(_digitalInputStates[pinByte], pin - pinByte * 8);
+ return value;
+ } else {
+ struct ServoData *s = _servoData[pin];
+ if (s == NULL) {
+ return false; // No structure means no animation!
+ } else {
+ return (s->stepNumber < s->numSteps);
+ }
}
}
- void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) override {
+ void _write(VPIN vpin, int value) override {
+ if (_deviceState == DEVSTATE_FAILED) return;
int pin = vpin - _firstVpin;
+ if (_servoData[pin] == NULL) {
+ _digitalOutBuffer[0] = EXIOWRD;
+ _digitalOutBuffer[1] = pin;
+ _digitalOutBuffer[2] = value;
+ I2CManager.read(_i2cAddress, _command1Buffer, 1, _digitalOutBuffer, 3);
+ if (_command1Buffer[0] != EXIORDY) {
+ DIAG(F("Vpin %d cannot be used as a digital output pin"), (int)vpin);
+ }
+ } else {
+ if (value) value = 1;
+ struct ServoData *s = _servoData[pin];
+ if (s != NULL) {
+ // Use configured parameters
+ this->_writeAnalogue(vpin, value ? s->activePosition : s->inactivePosition, s->profile, s->duration);
+ } else {
+ /* simulate digital pin on PWM */
+ this->_writeAnalogue(vpin, value ? 4095 : 0, Instant | NoPowerOff, 0);
+ }
+ }
+ }
+
+ void _writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) override {
+ int pin = vpin - _firstVpin;
+#ifdef DIAG_IO
+ DIAG(F("Servo: WriteAnalogue Vpin:%d Value:%d Profile:%d Duration:%d %S"),
+ vpin, value, profile, duration, _deviceState == DEVSTATE_FAILED?F("DEVSTATE_FAILED"):F(""));
+#endif
+ if (_deviceState == DEVSTATE_FAILED) return;
+ if (value > 4095) value = 4095;
+ else if (value < 0) value = 0;
+
+ struct ServoData *s = _servoData[pin];
+ if (s == NULL) {
+ // Servo pin not configured, so configure now using defaults
+ s = _servoData[pin] = (struct ServoData *) calloc(sizeof(struct ServoData), 1);
+ if (s == NULL) return; // Check for memory allocation failure
+ s->activePosition = 4095;
+ s->inactivePosition = 0;
+ s->currentPosition = value;
+ s->profile = Instant | NoPowerOff; // Use instant profile (but not this time)
+ }
+
+ // Animated profile. Initiate the appropriate action.
+ s->currentProfile = profile;
+ uint8_t profileValue = profile & ~NoPowerOff; // Mask off 'don't-power-off' bit.
+ s->numSteps = profileValue==Fast ? 10 : // 0.5 seconds
+ profileValue==Medium ? 20 : // 1.0 seconds
+ profileValue==Slow ? 40 : // 2.0 seconds
+ profileValue==Bounce ? sizeof(_bounceProfile)-1 : // ~ 1.5 seconds
+ duration * 2 + 1; // Convert from deciseconds (100ms) to refresh cycles (50ms)
+ s->stepNumber = 0;
+ s->toPosition = value;
+ s->fromPosition = s->currentPosition;
+ }
+
+ void updatePosition(uint8_t pin) {
+ struct ServoData *s = _servoData[pin];
+ if (s == NULL) return; // No pin configuration/state data
+
+ if (s->numSteps == 0) return; // No animation in progress
+
+ if (s->stepNumber == 0 && s->fromPosition == s->toPosition) {
+ // Go straight to end of sequence, output final position.
+ s->stepNumber = s->numSteps-1;
+ }
+
+ if (s->stepNumber < s->numSteps) {
+ // Animation in progress, reposition servo
+ s->stepNumber++;
+ if ((s->currentProfile & ~NoPowerOff) == Bounce) {
+ // Retrieve step positions from array in flash
+ uint8_t profileValue = GETFLASH(&_bounceProfile[s->stepNumber]);
+ s->currentPosition = map(profileValue, 0, 100, s->fromPosition, s->toPosition);
+ } else {
+ // All other profiles - calculate step by linear interpolation between from and to positions.
+ s->currentPosition = map(s->stepNumber, 0, s->numSteps, s->fromPosition, s->toPosition);
+ }
+ // Send servo command
+ this->writePWM(pin, s->currentPosition);
+ } else if (s->stepNumber < s->numSteps + _catchupSteps) {
+ // We've finished animation, wait a little to allow servo to catch up
+ s->stepNumber++;
+ } else if (s->stepNumber == s->numSteps + _catchupSteps
+ && s->currentPosition != 0) {
+ s->numSteps = 0; // Done now.
+ }
+ }
+
+ void writePWM(int pin, uint16_t value) {
_command4Buffer[0] = EXIOWRAN;
_command4Buffer[1] = pin;
_command4Buffer[2] = value & 0xFF;
@@ -218,6 +333,36 @@ private:
byte _receive3Buffer[3];
uint8_t* _analoguePinMap;
+ // Servo specific
+ struct ServoData {
+ uint16_t activePosition : 12; // Config parameter
+ uint16_t inactivePosition : 12; // Config parameter
+ uint16_t currentPosition : 12;
+ uint16_t fromPosition : 12;
+ uint16_t toPosition : 12;
+ uint8_t profile; // Config parameter
+ uint16_t stepNumber; // Index of current step (starting from 0)
+ uint16_t numSteps; // Number of steps in animation, or 0 if none in progress.
+ uint8_t currentProfile; // profile being used for current animation.
+ uint16_t duration; // time (tenths of a second) for animation to complete.
+ }; // 14 bytes per element, i.e. per pin in use
+
+ // struct ServoData *_servoData[256];
+ ServoData** _servoData;
+
+ static const uint8_t _catchupSteps = 5; // number of steps to wait before switching servo off
+
+ const unsigned int refreshInterval = 50; // refresh every 50ms
+ unsigned long _lastRefresh = 0;
+
+ // Profile for a bouncing signal or turnout
+ // The profile below is in the range 0-100% and should be combined with the desired limits
+ // of the servo set by _activePosition and _inactivePosition. The profile is symmetrical here,
+ // i.e. the bounce is the same on the down action as on the up action. First entry isn't used.
+ const byte FLASH _bounceProfile[30] =
+ {0,2,3,7,13,33,50,83,100,83,75,70,65,60,60,65,74,84,100,83,75,70,70,72,75,80,87,92,97,100};
+
+ // EX-IOExpander protocol flags
enum {
EXIOINIT = 0xE0, // Flag to initialise setup procedure
EXIORDY = 0xE1, // Flag we have completed setup procedure, also for EX-IO to ACK setup
diff --git a/IO_PCA9685_basic.h b/IO_PCA9685_basic.h
deleted file mode 100644
index 4f809aa..0000000
--- a/IO_PCA9685_basic.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * © 2023, Neil McKechnie. All rights reserved.
- *
- * This file is part of DCC++EX API
- *
- * This is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * It is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with CommandStation. If not, see .
- */
-
-/*
- * This driver performs the basic interface between the HAL and an
- * I2C-connected PCA9685 16-channel PWM module. When requested, it
- * commands the device to set the PWM mark-to-period ratio accordingly.
- * The call to IODevice::writeAnalogue(vpin, value) specifies the
- * desired value in the range 0-4095 (0=0% and 4095=100%).
- */
-
-#ifndef PCA9685_BASIC_H
-#define PCA9685_BASIC_H
-
-#include "IODevice.h"
-#include "I2CManager.h"
-#include "DIAG.h"
-
-/*
- * IODevice subclass for PCA9685 16-channel PWM module.
- */
-
-class PCA9685_basic : public IODevice {
-public:
- // Create device driver instance.
- static void create(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
- if (checkNoOverlap(firstVpin, nPins,I2CAddress)) new PCA9685_basic(firstVpin, nPins, I2CAddress);
- }
-
-private:
-
- // structures for setting up non-blocking writes to servo controller
- I2CRB requestBlock;
- uint8_t outputBuffer[5];
-
- // REGISTER ADDRESSES
- const byte PCA9685_MODE1=0x00; // Mode Register
- const byte PCA9685_FIRST_SERVO=0x06; /** low byte first servo register ON*/
- const byte PCA9685_PRESCALE=0xFE; /** Prescale register for PWM output frequency */
- // MODE1 bits
- const byte MODE1_SLEEP=0x10; /**< Low power mode. Oscillator off */
- const byte MODE1_AI=0x20; /**< Auto-Increment enabled */
- const byte MODE1_RESTART=0x80; /**< Restart enabled */
-
- const float FREQUENCY_OSCILLATOR=25000000.0; /** Accurate enough for our purposes */
- const uint8_t PRESCALE_50HZ = (uint8_t)(((FREQUENCY_OSCILLATOR / (50.0 * 4096.0)) + 0.5) - 1);
- const uint32_t MAX_I2C_SPEED = 1000000L; // PCA9685 rated up to 1MHz I2C clock speed
-
- // Constructor
- PCA9685_basic(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
- _firstVpin = firstVpin;
- _nPins = min(nPins, 16);
- _I2CAddress = I2CAddress;
- addDevice(this);
-
- // Initialise structure used for setting pulse rate
- requestBlock.setWriteParams(_I2CAddress, outputBuffer, sizeof(outputBuffer));
- }
-
- // Device-specific initialisation
- void _begin() override {
- I2CManager.begin();
- I2CManager.setClock(1000000); // Nominally able to run up to 1MHz on I2C
- // In reality, other devices including the Arduino will limit
- // the clock speed to a lower rate.
-
- // Initialise I/O module here.
- if (I2CManager.exists(_I2CAddress)) {
- writeRegister(_I2CAddress, PCA9685_MODE1, MODE1_SLEEP | MODE1_AI);
- writeRegister(_I2CAddress, PCA9685_PRESCALE, PRESCALE_50HZ); // 50Hz clock, 20ms pulse period.
- writeRegister(_I2CAddress, PCA9685_MODE1, MODE1_AI);
- writeRegister(_I2CAddress, PCA9685_MODE1, MODE1_RESTART | MODE1_AI);
- // In theory, we should wait 500us before sending any other commands to each device, to allow
- // the PWM oscillator to get running. However, we don't do any specific wait, as there's
- // plenty of other stuff to do before we will send a command.
- #if defined(DIAG_IO)
- _display();
- #endif
- } else
- _deviceState = DEVSTATE_FAILED;
- }
-
- // Device-specific writeAnalogue function, invoked from IODevice::writeAnalogue().
- //
- void _writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) override {
- #ifdef DIAG_IO
- DIAG(F("PCA9685 WriteAnalogue Vpin:%d Value:%d Profile:%d Duration:%d %S"),
- vpin, value, profile, duration, _deviceState == DEVSTATE_FAILED?F("DEVSTATE_FAILED"):F(""));
- #endif
- if (_deviceState == DEVSTATE_FAILED) return;
- int pin = vpin - _firstVpin;
- if (value > 4095) value = 4095;
- else if (value < 0) value = 0;
-
- writeDevice(pin, value);
- }
-
- // Display details of this device.
- void _display() override {
- DIAG(F("PCA9685 I2C:x%x Configured on Vpins:%d-%d %S"), _I2CAddress, (int)_firstVpin,
- (int)_firstVpin+_nPins-1, (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
- }
-
- // writeDevice (helper function) takes a pin in range 0 to _nPins-1 within the device, and a value
- // between 0 and 4095 for the PWM mark-to-period ratio, with 4095 being 100%.
- void writeDevice(uint8_t pin, int value) {
- #ifdef DIAG_IO
- DIAG(F("PCA9685 I2C:x%x WriteDevice Pin:%d Value:%d"), _I2CAddress, pin, value);
- #endif
- // Wait for previous request to complete
- uint8_t status = requestBlock.wait();
- if (status != I2C_STATUS_OK) {
- _deviceState = DEVSTATE_FAILED;
- DIAG(F("PCA9685 I2C:x%x failed %S"), _I2CAddress, I2CManager.getErrorMessage(status));
- } else {
- // Set up new request.
- outputBuffer[0] = PCA9685_FIRST_SERVO + 4 * pin;
- outputBuffer[1] = 0;
- outputBuffer[2] = (value == 4095 ? 0x10 : 0); // 4095=full on
- outputBuffer[3] = value & 0xff;
- outputBuffer[4] = value >> 8;
- I2CManager.queueRequest(&requestBlock);
- }
- }
-
- // Internal helper function for this device
- static void writeRegister(byte address, byte reg, byte value) {
- I2CManager.write(address, 2, reg, value);
- }
-
-};
-
-#endif
\ No newline at end of file
diff --git a/IO_Servo.h b/IO_Servo.h
deleted file mode 100644
index b1935b6..0000000
--- a/IO_Servo.h
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
- * © 2023, Neil McKechnie. All rights reserved.
- *
- * This file is part of DCC++EX API
- *
- * This is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * It is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with CommandStation. If not, see .
- */
-#ifndef IO_SERVO_H
-
-#include "IODevice.h"
-#include "I2CManager.h"
-#include "DIAG.h"
-
-class Servo : IODevice {
-
-public:
- enum ProfileType : uint8_t {
- Instant = 0, // Moves immediately between positions (if duration not specified)
- UseDuration = 0, // Use specified duration
- Fast = 1, // Takes around 500ms end-to-end
- Medium = 2, // 1 second end-to-end
- Slow = 3, // 2 seconds end-to-end
- Bounce = 4, // For semaphores/turnouts with a bit of bounce!!
- NoPowerOff = 0x80, // Flag to be ORed in to suppress power off after move.
- };
-
- // Create device driver instance.
- static void create(VPIN firstVpin, int nPins, VPIN firstSlavePin) {
- if (checkNoOverlap(firstVpin, nPins)) new Servo(firstVpin, nPins, firstSlavePin);
- }
-
-private:
- VPIN _firstSlavePin;
- IODevice *_slaveDevice = NULL;
-
- struct ServoData {
- uint16_t activePosition : 12; // Config parameter
- uint16_t inactivePosition : 12; // Config parameter
- uint16_t currentPosition : 12;
- uint16_t fromPosition : 12;
- uint16_t toPosition : 12;
- uint8_t profile; // Config parameter
- uint16_t stepNumber; // Index of current step (starting from 0)
- uint16_t numSteps; // Number of steps in animation, or 0 if none in progress.
- uint8_t currentProfile; // profile being used for current animation.
- uint16_t duration; // time (tenths of a second) for animation to complete.
- }; // 14 bytes per element, i.e. per pin in use
-
- struct ServoData *_servoData [16];
-
- static const uint8_t _catchupSteps = 5; // number of steps to wait before switching servo off
- static const uint8_t FLASH _bounceProfile[30];
-
- const unsigned int refreshInterval = 50; // refresh every 50ms
-
-
- // Configure a port on the Servo.
- bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) {
- if (configType != CONFIGURE_SERVO) return false;
- if (paramCount != 5) return false;
- #ifdef DIAG_IO
- DIAG(F("Servo: Configure VPIN:%d Apos:%d Ipos:%d Profile:%d Duration:%d state:%d"),
- vpin, params[0], params[1], params[2], params[3], params[4]);
- #endif
-
- int8_t pin = vpin - _firstVpin;
- VPIN slavePin = vpin - _firstVpin + _firstSlavePin;
- struct ServoData *s = _servoData[pin];
- if (s == NULL) {
- _servoData[pin] = (struct ServoData *)calloc(1, sizeof(struct ServoData));
- s = _servoData[pin];
- if (!s) return false; // Check for failed memory allocation
- }
-
- s->activePosition = params[0];
- s->inactivePosition = params[1];
- s->profile = params[2];
- s->duration = params[3];
- int state = params[4];
-
- if (state != -1) {
- // Position servo to initial state
- IODevice::writeAnalogue(slavePin, state ? s->activePosition : s->inactivePosition, 0, 0);
- }
- return true;
- }
-
- // Constructor
- Servo(VPIN firstVpin, int nPins, VPIN firstSlavePin) {
- _firstVpin = firstVpin;
- _nPins = (nPins > 16) ? 16 : nPins;
- _firstSlavePin = firstSlavePin;
-
- // To save RAM, space for servo configuration is not allocated unless a pin is used.
- // Initialise the pointers to NULL.
- for (int i=0; i<_nPins; i++)
- _servoData[i] = NULL;
-
- addDevice(this);
- }
-
- // Device-specific initialisation
- void _begin() override {
- // Get reference to slave device to make accesses faster.
- _slaveDevice = this->findDevice(_firstSlavePin);
- // Check firstSlavePin is actually allocated to a device
- if (!_slaveDevice) {
- DIAG(F("Servo: Slave device not found on pins %d-%d"),
- _firstSlavePin, _firstSlavePin+_nPins-1);
- _deviceState = DEVSTATE_FAILED;
- }
- // Check that the last slave pin is allocated to the same device.
- if (_slaveDevice != this->findDevice(_firstSlavePin+_nPins-1)) {
- DIAG(F("Servo: Slave device does not cover all pins %d-%d"),
- _firstSlavePin, _firstSlavePin+_nPins-1);
- _deviceState = DEVSTATE_FAILED;
- }
- #if defined(DIAG_IO)
- _display();
- #endif
- }
-
- // Device-specific write function, invoked from IODevice::write().
- // For this function, the configured profile is used.
- void _write(VPIN vpin, int value) override {
- if (_deviceState == DEVSTATE_FAILED) return;
- #ifdef DIAG_IO
- DIAG(F("Servo Write Vpin:%d Value:%d"), vpin, value);
- #endif
- int pin = vpin - _firstVpin;
- // VPIN slavePin = vpin - _firstVpin + _firstSlavePin;
- if (value) value = 1;
-
- struct ServoData *s = _servoData[pin];
- if (s != NULL) {
- // Use configured parameters
- this->_writeAnalogue(vpin, value ? s->activePosition : s->inactivePosition, s->profile, s->duration);
- } else {
- /* simulate digital pin on PWM */
- this->_writeAnalogue(vpin, value ? 4095 : 0, Instant | NoPowerOff, 0);
- }
- }
-
- // Device-specific writeAnalogue function, invoked from IODevice::writeAnalogue().
- // Profile is as follows:
- // Bit 7: 0=Set PWM to 0% to power off servo motor when finished
- // 1=Keep PWM pulses on (better when using PWM to drive an LED)
- // Bits 6-0: 0 Use specified duration (defaults to 0 deciseconds)
- // 1 (Fast) Move servo in 0.5 seconds
- // 2 (Medium) Move servo in 1.0 seconds
- // 3 (Slow) Move servo in 2.0 seconds
- // 4 (Bounce) Servo 'bounces' at extremes.
- //
- void _writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) override {
- #ifdef DIAG_IO
- DIAG(F("Servo: WriteAnalogue Vpin:%d Value:%d Profile:%d Duration:%d %S"),
- vpin, value, profile, duration, _deviceState == DEVSTATE_FAILED?F("DEVSTATE_FAILED"):F(""));
- #endif
- if (_deviceState == DEVSTATE_FAILED) return;
- int pin = vpin - _firstVpin;
- if (value > 4095) value = 4095;
- else if (value < 0) value = 0;
-
- struct ServoData *s = _servoData[pin];
- if (s == NULL) {
- // Servo pin not configured, so configure now using defaults
- s = _servoData[pin] = (struct ServoData *) calloc(sizeof(struct ServoData), 1);
- if (s == NULL) return; // Check for memory allocation failure
- s->activePosition = 4095;
- s->inactivePosition = 0;
- s->currentPosition = value;
- s->profile = Instant | NoPowerOff; // Use instant profile (but not this time)
- }
-
- // Animated profile. Initiate the appropriate action.
- s->currentProfile = profile;
- uint8_t profileValue = profile & ~NoPowerOff; // Mask off 'don't-power-off' bit.
- s->numSteps = profileValue==Fast ? 10 : // 0.5 seconds
- profileValue==Medium ? 20 : // 1.0 seconds
- profileValue==Slow ? 40 : // 2.0 seconds
- profileValue==Bounce ? sizeof(_bounceProfile)-1 : // ~ 1.5 seconds
- duration * 2 + 1; // Convert from deciseconds (100ms) to refresh cycles (50ms)
- s->stepNumber = 0;
- s->toPosition = value;
- s->fromPosition = s->currentPosition;
- }
-
- // _read returns true if the device is currently in executing an animation,
- // changing the output over a period of time.
- int _read(VPIN vpin) override {
- if (_deviceState == DEVSTATE_FAILED) return 0;
- int pin = vpin - _firstVpin;
- struct ServoData *s = _servoData[pin];
- if (s == NULL)
- return false; // No structure means no animation!
- else
- return (s->stepNumber < s->numSteps);
- }
-
- void _loop(unsigned long currentMicros) override {
- if (_deviceState == DEVSTATE_FAILED) return;
- for (int pin=0; pin<_nPins; pin++) {
- updatePosition(pin);
- }
- delayUntil(currentMicros + refreshInterval * 1000UL);
- }
-
- // Private function to reposition servo
- // TODO: Could calculate step number from elapsed time, to allow for erratic loop timing.
- void updatePosition(uint8_t pin) {
- struct ServoData *s = _servoData[pin];
- if (s == NULL) return; // No pin configuration/state data
-
- if (s->numSteps == 0) return; // No animation in progress
-
- if (s->stepNumber == 0 && s->fromPosition == s->toPosition) {
- // Go straight to end of sequence, output final position.
- s->stepNumber = s->numSteps-1;
- }
-
- if (s->stepNumber < s->numSteps) {
- // Animation in progress, reposition servo
- s->stepNumber++;
- if ((s->currentProfile & ~NoPowerOff) == Bounce) {
- // Retrieve step positions from array in flash
- uint8_t profileValue = GETFLASH(&_bounceProfile[s->stepNumber]);
- s->currentPosition = map(profileValue, 0, 100, s->fromPosition, s->toPosition);
- } else {
- // All other profiles - calculate step by linear interpolation between from and to positions.
- s->currentPosition = map(s->stepNumber, 0, s->numSteps, s->fromPosition, s->toPosition);
- }
- // Send servo command
- _slaveDevice->writeAnalogue(_firstSlavePin+pin, s->currentPosition);
- } else if (s->stepNumber < s->numSteps + _catchupSteps) {
- // We've finished animation, wait a little to allow servo to catch up
- s->stepNumber++;
- } else if (s->stepNumber == s->numSteps + _catchupSteps
- && s->currentPosition != 0) {
- #ifdef IO_SWITCH_OFF_SERVO
- if ((s->currentProfile & NoPowerOff) == 0) {
- // Wait has finished, so switch off PWM to prevent annoying servo buzz
- _slaveDevice->writeAnalogue(_firstSlavePin+pin, 0);
- }
- #endif
- s->numSteps = 0; // Done now.
- }
- }
-
- // Display details of this device.
- void _display() override {
- DIAG(F("Servo Configured on Vpins:%d-%d, slave pins:%d-%d %S"),
- (int)_firstVpin, (int)_firstVpin+_nPins-1,
- (int)_firstSlavePin, (int)_firstSlavePin+_nPins-1,
- (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
- }
-};
-
-// Profile for a bouncing signal or turnout
-// The profile below is in the range 0-100% and should be combined with the desired limits
-// of the servo set by _activePosition and _inactivePosition. The profile is symmetrical here,
-// i.e. the bounce is the same on the down action as on the up action. First entry isn't used.
-const byte FLASH Servo::_bounceProfile[30] =
- {0,2,3,7,13,33,50,83,100,83,75,70,65,60,60,65,74,84,100,83,75,70,70,72,75,80,87,92,97,100};
-
-
-#endif
\ No newline at end of file