dani
/
CommandStation-EX
mirror of https://github.com/DCC-EX/CommandStation-EX.git
1
0
Fork 0
Code Releases Activity

Servo driver - split PCA9685 into a filter + PWM driver. 

Two new drivers: PCA9685pwm drives the PCA9685 I2C device directly, and Servo driver which acts as a 'shim' over the top to control animations.  This is aimed at supporting devices like the EXIOExpander by allowing the Servo driver to talk to the EXIOExpander driver instead, to animate servos on another controller.
This commit is contained in:
Neil McKechnie 2023-02-07 17:24:05 +00:00
parent 19070d33ba
commit aad0d28d1f
3 changed files with 480 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
IO_PCA9685pwm.h Normal file
View File

@ -0,0 +1,149 @@
/*
* © 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 <https://www.gnu.org/licenses/>.
*/
/*
* 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 PCA9685pwm : public IODevice {
public:
// Create device driver instance.
static void create(VPIN firstVpin, int nPins, I2CAddress i2cAddress) {
if (checkNoOverlap(firstVpin, nPins, i2cAddress)) new PCA9685pwm(firstVpin, nPins, i2cAddress);
}
private:
// structures for setting up non-blocking writes to servo controller
I2CRB requestBlock;
uint8_t outputBuffer[5];
// REGISTER ADDRESSES
const uint8_t PCA9685_MODE1=0x00; // Mode Register
const uint8_t PCA9685_FIRST_SERVO=0x06; /** low uint8_t first servo register ON*/
const uint8_t PCA9685_PRESCALE=0xFE; /** Prescale register for PWM output frequency */
// MODE1 bits
const uint8_t MODE1_SLEEP=0x10; /**< Low power mode. Oscillator off */
const uint8_t MODE1_AI=0x20; /**< Auto-Increment enabled */
const uint8_t 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
PCA9685pwm(VPIN firstVpin, int nPins, I2CAddress i2cAddress) {
_firstVpin = firstVpin;
_nPins = (nPins>16) ? 16 : nPins;
_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("PCA9685pwm 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("PCA9685pwm I2C:x%x Configured on Vpins:%d-%d %S"), (int)_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("PCA9685pwm I2C:x%x WriteDevice Pin:%d Value:%d"), (int)_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("PCA9685pwm I2C:x%x failed %S"), (int)_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(I2CAddress address, uint8_t reg, uint8_t value) {
I2CManager.write(address, 2, reg, value);
}
};
#endif

33
IO_Servo.cpp Normal file
View File

@ -0,0 +1,33 @@
/*
* © 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 <https://www.gnu.org/licenses/>.
*/
#include "IO_Servo.h"
#include "FSH.h"
// 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.
//
// Note: This has been put into its own .CPP file to ensure that duplicates aren't created
// if the IO_Servo.h library is #include'd in multiple source files.
//
const uint8_t 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};

298
IO_Servo.h Normal file
View File

@ -0,0 +1,298 @@
/*
* © 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 <https://www.gnu.org/licenses/>.
*/
/*
* This device is a layered device which is designed to sit on top of another
* device. The underlying device class is expected to accept writeAnalogue calls
* which will normally cause some physical movement of something. The device may be a servo,
* a motor or some other kind of positioner, and the something might be a turnout,
* a semaphore signal or something else. One user has used this capability for
* moving a figure along the platform on their layout!
*
* Example of use:
* In myHal.cpp,
*
* #include "IO_Servo.h"
* ...
* PCA9685::create(100,16,0x40); // First create the hardware interface device
* Servo::create(300,16,100); // Then create the higher level device which
* // references pins 100-115 or a subset of them.
*
* Then any reference to pins 300-315 will cause the servo driver to send output
* PWM commands to the corresponding PCA9685 driver pins 100-115. The PCA9685 driver may
* be substituted with any other driver which provides analogue output
* capability, e.g. EX-IOExpander devices, as long as they are capable of interpreting
* the writeAnalogue() function calls.
*/
#include "IODevice.h"
#ifndef IO_SERVO_H
#define IO_SERVO_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=VPIN_NONE) {
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 (_deviceState == DEVSTATE_FAILED) return false;
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;
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
writeAnalogue(vpin, state ? s->activePosition : s->inactivePosition);
}
return true;
}
// Constructor
Servo(VPIN firstVpin, int nPins, VPIN firstSlavePin = VPIN_NONE) {
_firstVpin = firstVpin;
_nPins = (nPins > 16) ? 16 : nPins;
if (firstSlavePin == VPIN_NONE)
_firstSlavePin = firstVpin;
else
_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;
// Get reference to slave device.
_slaveDevice = findDevice(_firstSlavePin);
if (!_slaveDevice) {
DIAG(F("Servo: Slave device not found on pins %d-%d"),
_firstSlavePin, _firstSlavePin+_nPins-1);
_deviceState = DEVSTATE_FAILED;
}
if (_slaveDevice != findDevice(_firstSlavePin+_nPins-1)) {
DIAG(F("Servo: Slave device does not cover all pins %d-%d"),
_firstSlavePin, _firstSlavePin+_nPins-1);
_deviceState = DEVSTATE_FAILED;
}
addDevice(this, _slaveDevice); // Link device ahead of slave device to intercept requests
}
// Device-specific initialisation
void _begin() override {
#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;
if (value) value = 1;
struct ServoData *s = _servoData[pin];
if (s != NULL) {
// Use configured parameters
writeAnalogue(vpin, value ? s->activePosition : s->inactivePosition, s->profile, s->duration);
} else {
/* simulate digital pin on PWM */
writeAnalogue(vpin, value ? 4095 : 0, Instant | NoPowerOff, 0);
}
}
// Device-specific writeAnalogue function, invoked from IODevice::writeAnalogue().
// Profile is as follows:
// Bit 7: 0=Set output to 0% to power off servo motor when finished
// 1=Keep output at final position (better with LEDs, which will stay lit)
// 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.
// Duration is in deciseconds (tenths of a second) and defaults to 0.
//
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 to output driver
_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 output driver to avoid 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(""));
}
};
#endif