From 3d480ee9ef378e32b03947ab342179f1b83c04fb Mon Sep 17 00:00:00 2001 From: peteGSX Date: Thu, 9 Feb 2023 05:32:27 +1000 Subject: [PATCH] Start adding servo to EX-IO --- IO_EXIOExpander.h | 142 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 141 insertions(+), 1 deletion(-) diff --git a/IO_EXIOExpander.h b/IO_EXIOExpander.h index 8ec1f48..6549873 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,11 @@ 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. + for (int i=0; i<_nPins; i++) { + _servoData[i] = NULL; + } addDevice(this); } @@ -123,6 +139,28 @@ private: DIAG(F("Vpin %d cannot be used as a digital input pin"), (int)vpin); return false; } + } else if (configType == CONFIGURE_SERVO) { + 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 + 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(pin, state ? s->activePosition : s->inactivePosition, 0, 0); + } + return true; } else { return false; } @@ -184,13 +222,86 @@ private: } } - void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) override { + // void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) override { + void _writeAnalogue(VPIN vpin, int value, uint8_t profile, uint16_t duration) override { int pin = vpin - _firstVpin; + /* Initial _writeAnalogue here _command4Buffer[0] = EXIOWRAN; _command4Buffer[1] = pin; _command4Buffer[2] = value & 0xFF; _command4Buffer[3] = value >> 8; I2CManager.write(_i2cAddress, _command4Buffer, 4); + */ + #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; + } + + 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. + } } void _display() override { @@ -218,6 +329,35 @@ 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 [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 + +// 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 EXIOExpander::_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