I've been working on the coding of the feedback interface boards. As a reminder, these are NOT needed to run the robot with the original control, but they will be necessary if the robot is to be run by a Rockwell Automation servo drive.
I'll spare the nitty gritty details here since they are probably boring, but here is a link to another forum where I got a lot of help on the code:
https://forum.pjrc.com/threads/54962-Interrupt-on-Rising-and-Falling-on-the-same-pin
The current implementation of the code (not written by me!) uses a hardware based clock cycle capture function to measure pulse width of the bit stream, pass that to a circular buffer using Direct Memory Addressing (DMA), and is decoded and printed to the serial port. The hardware capture and DMA make sure that all timing critical functions are handled in hardware and are free of processor latency. The decoding is done more slowly when the processor has time to service the requests.
My original code and the hardware cycle counter capture method without DMA both failed occasionally when some high priority processor interrupt took over and blocked reading the edges. The CRC still needs to be implemented, and this would help resolve that issue, but the DMA route does not seem to suffer from this issue.
I still need to read the quadrature signals from the encoder to increase the single turn resolution from 11 bits to 13 bits, borrow an Allen Bradley TLY motor with Tamagawa feedback from work and characterize its communication, then finally program the microcontroller to generate response packets to the drive that emulate the TLY motor. Lots of work to go, but having this part working is a big step forward.
Here is the code (Reminder, this is a Teensy 4.1 running Teensyduino firmware, so the implementation on another microcontroller will be different, especially the hardware and DMA calls):
Main function:
C++:
#include "Arduino.h"
#include "decoder.h"
Manchester::Decoder decoder;
Manchester::TS5643Field old, result;
void setup()
{
decoder.begin(2E6); // 500ns clock
}
void loop()
{
while (!decoder.resultBuffer.isEmpty())
{
decoder.resultBuffer.pop(result);
Serial.println(result);
/*if (result.count != old.count + 1)
{
Serial.printf("Error %d, %d --------------------------------\n", old.count, result.count);
}*/
old = result;
}
delay(1); // do something else, make sure you don't spend too much time otherwise the edge buffer might overflow and you'll loose data
}
void yield()
{
decoder.tick(); // tick the decoder from yield to get ticks even during delays and other time consuming tasks...
}
A non-interrupt blocking circular buffer implementation:
C++:
#pragma once
#include <Arduino.h>
template <typename ET, size_t S>
class RingBuf
{
public:
/* Constructor. Init mReadIndex to 0 and mSize to 0 */
RingBuf();
/* Push a data at the end of the buffer */
bool push(const ET inElement);
/* Pop the data at the beginning of the buffer */
bool pop(ET& outElement);
/* Return true if the buffer is full */
bool isFull() const { return mSize == S; }
/* Return true if the buffer is empty */
bool isEmpty() const { return mSize == 0; }
/* Reset the buffer to an empty state */
void clear() { mSize = 0; }
/* return the size of the buffer */
size_t size() const { return mSize; }
/* return the maximum size of the buffer */
size_t maxSize() const { return S; }
private:
ET mBuffer[S];
size_t mReadIndex;
size_t mSize;
size_t writeIndex();
};
template <typename ET, size_t S>
size_t RingBuf<ET, S>::writeIndex()
{
size_t wi = mReadIndex + mSize;
if (wi >= S) wi -= S;
return wi;
}
template <typename ET, size_t S>
RingBuf<ET, S>::RingBuf()
: mReadIndex(0), mSize(0)
{
}
template <typename ET, size_t S>
bool RingBuf<ET, S>::push(const ET inElement)
{
if (isFull()) return false;
mBuffer[writeIndex()] = inElement;
mSize++;
return true;
}
template <typename ET, size_t S>
bool RingBuf<ET, S>::pop(ET& outElement)
{
if (isEmpty()) return false;
outElement = mBuffer[mReadIndex];
mReadIndex++;
mSize--;
if (mReadIndex == S) mReadIndex = 0;
return true;
}
Header for the DMA edge capture:
C++:
#pragma once
#include "DMAChannel.h"
namespace Manchester
{
class EdgeProviderDMA
{
public:
static void init();
static uint16_t popTimestamp();
static bool hasElements();
protected:
static uint8_t tail;
static DMAChannel dmaChannel;
static constexpr IMXRT_TMR_CH_t* ch = &IMXRT_TMR1.CH[2]; // TMR1 channel 2 -> input pin 11,
};
}
Code for the DMA edge capture:
C++:
#include "edgeproviderDMA.h"
#include "pins_Arduino.h"
#include <cstdlib>
namespace Manchester
{
constexpr size_t bufSize = 256; // DMA buffer for edge timestamps (512 bytes, 256 timestamps)
uint16_t buf[bufSize] __attribute__((aligned(512))); // The DMA controller will replace the lowest n-bits of the address by a counter
// to implement the circular buffer -> we need to align the start address of the buffer
void EdgeProviderDMA::init() // such that it corresponds to a countervalue of 0
{ //
*(portConfigRegister(11)) = 1; // ALT1, use pin 11 as input to TMR1_2
//
ch->CTRL = 0; // stop timer
ch->SCTRL = TMR_SCTRL_CAPTURE_MODE(3); // both edges, enable edge interrupt
ch->LOAD = 0; // reload the counter with 0 at rollover (doesn't work without setting this explicitely)
ch->DMA = TMR_DMA_IEFDE; // DMA on capture events
ch->CTRL = TMR_CTRL_CM(1) | TMR_CTRL_PCS(8 + 3) | TMR_CTRL_SCS(2); // start, source: peripheral clock, prescaler 3 (=> dt = 1/150Mhz * 8 = 53ns resolution, 2^15 * 53ns = 3.5ms max), use counter 2 input pin for capture
//
dmaChannel.begin(); //
dmaChannel.triggerAtHardwareEvent(DMAMUX_SOURCE_QTIMER1_READ2); // trigger DMA by capture event on channel 2
dmaChannel.source(ch->CAPT); // DMA source = capture register (16 bit)
dmaChannel.destinationCircular(buf, bufSize * sizeof(uint16_t)); // use a circular buffer as destination. Buffer size in bytes
dmaChannel.enable();
}
uint16_t EdgeProviderDMA::popTimestamp()
{
return buf[tail++];
}
bool EdgeProviderDMA::hasElements()
{
return dmaChannel.destinationAddress() != (buf + tail);
}
DMAChannel EdgeProviderDMA::dmaChannel;
uint8_t EdgeProviderDMA::tail = 0;
}
Header file for the bit stream decoder:
C++:
#pragma once
#include "RingBuf.h"
#include "TS5643Field.h"
namespace Manchester
{
union frame_t
{
struct
{
uint32_t modemAddress : 2;
uint32_t payload : 15;
uint32_t frameAddress : 1;
uint32_t frameCRC : 3;
} bits;
uint32_t data;
};
using ResultBuffer = RingBuf<TS5643Field, 10000>; // Keep results
class Decoder
{
public:
void begin(float baudrate);
void tick();
ResultBuffer resultBuffer;
protected:
bool decode(uint16_t dt, uint32_t* f0, uint32_t* f1);
bool initialized = false;
uint32_t bitCnt;
frame_t frame0, frame1;
frame_t* currentFrame;
TS5643Field received;
bool edge;
enum states {
SYNCING,
SYNCED,
} state = states::SYNCING;
};
}
Code for the Bitstream Decoder:
C++:
#include "decoder.h"
#include "edgeproviderDMA.h"
namespace Manchester
{
void Decoder::begin(float baudrate)
{
resultBuffer.clear();
EdgeProviderDMA::init();
state = states::SYNCING;
currentFrame = &frame0;
initialized = true;
}
void Decoder::tick()
{
if (!initialized) return;
//static uint32_t oldCount = 0;
static uint16_t oldTimestamp = 0;
while (EdgeProviderDMA::hasElements())
{
uint32_t payload_0, payload_1;
uint16_t timestamp = EdgeProviderDMA::popTimestamp();
uint16_t dt = timestamp - oldTimestamp;
oldTimestamp = timestamp;
if (decode(dt, &payload_0, &payload_1)) // If decoder returns true, the payloads of the first and second frame are valid
{
TS5643Field field;
field.count = (payload_0 & 0x7FFF) | ((payload_1 & 0x1FF) << 15); // bit 0-14 in frame0 bit 15-23 in frame1
field.BE_OS = payload_1 & 1 << 9;
field.OF = payload_1 & 1 << 10;
field.OS = payload_1 & 1 << 11;
field.BA = payload_1 & 1 << 12;
field.PS = payload_1 & 1 << 13;
field.CE = payload_1 & 1 << 14;
// if (field.count != oldCount + 1) // error detection
// {
// digitalWriteFast(10, HIGH);
// }
// oldCount = field.count;
resultBuffer.push(field);
// digitalWriteFast(10, LOW);
}
}
}
bool Decoder::decode(uint16_t delta_t, uint32_t* payload_0, uint32_t* payload_1)
{
constexpr uint16_t T = 500; // timing of a half bit
constexpr uint16_t syncMin = 2625 - 100; // min duration of sync pulse
constexpr uint16_t syncMax = 2625 + 100; // max duration of sync pulse
static bool first = true; // keep track of the 2T timing
//
uint16_t dt = delta_t * 8.0f * 1E9f / F_BUS_ACTUAL; // time in ns since last timestamp
edge = !edge; // we detect both edges -> direction will necessarily toggle at each event
switch (state)
{
case SYNCING:
if (dt > syncMin && dt < syncMax)
{
currentFrame->data = 0;
bitCnt = 0;
first = true;
state = SYNCED;
// Serial.println("Sync");
edge = 0;
}
break;
case SYNCED:
if (dt > T * 3.5 || dt < T * 0.5) // timing to much off, probably a break, need to resync
{
Serial.printf("bad %d\n", dt);
state = SYNCING;
break;
}
// decode
if (dt > T * 1.5) // dt = 2T
{
currentFrame->data |= (!edge << bitCnt++);
}
else // dt = T
{
if (!first)
{
currentFrame->data |= (!edge << bitCnt++);
}
first = !first;
}
if (bitCnt > 21) // got complete frame
{
if (currentFrame == &frame0 && currentFrame->bits.frameAddress == 0)
{
currentFrame = &frame1;
}
else if (currentFrame == &frame1 && currentFrame->bits.frameAddress == 1)
{
currentFrame = &frame0;
*payload_0 = frame0.bits.payload;
*payload_1 = frame1.bits.payload;
state = states::SYNCING;
digitalWriteFast(8, LOW); //
return true;
}
else
{
currentFrame = &frame0;
Serial.println("framesync");
}
state = states::SYNCING;
}
break;
default:
break;
}
digitalWriteFast(8, LOW); //
return false;
}
}
Header file for the TS5643 implementation:
C++:
#pragma once
#include "Arduino.h"
namespace Manchester
{
struct TS5643Field : Printable
{
uint32_t count; // counter value
bool BE_OS; // Battery error | Overspeed (ored)
bool OF; // Overflow
bool OS; // Overspeed
bool BA; // Battery alarm
bool PS; // Preload status
bool CE; // Count Error
TS5643Field();
void clear();
size_t printTo(Print& p) const override;
};
}
Code for the TS5643 implementation:
C++:
#include "TS5643Field.h"
namespace Manchester
{
TS5643Field::TS5643Field()
{
clear();
}
void TS5643Field::clear()
{
count = 0;
BE_OS = LOW;
OF = LOW;
OS = LOW;
BA = LOW;
PS = LOW;
CE = LOW;
}
size_t TS5643Field::printTo(Print& p) const
{
return p.printf("cnt: %d, BE+OS:%d OF:%d OS:%d BA:%d PS:%d CE:%d", count, BE_OS, OF, OS, BA, PS, CE);
}
}
And some sample encoder data coming out of the serial port:
Code:
cnt: 2837, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2838, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2838, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2838, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2838, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2839, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2839, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2839, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2839, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2840, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2840, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2840, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2840, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2841, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2841, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2841, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2841, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2842, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2842, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2842, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2842, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2843, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2843, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2843, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2843, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2844, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2844, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2844, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2844, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
cnt: 2845, BE+OS:0 OF:0 OS:0 BA:1 PS:0 CE:0
In the next version of this code, I will try to add a hardware quadrature encoder decoder, and compare that data to the serial transmissions. This cross checks against bad data or lost counts. Not sure which piece of data I will trust. This should save the positions to the tightly coupled memory on the chip for fast read/write access when it needs to generate a response to the servo drive.
I've also bought some extra hardware to let me wire in the serial channel from the drive as well as the quadrature encoder signals. This was all cheap from Digikey, but man is everything out of stock. It is crazy.
. I have an ESTOP box which I can put on the other end. Two conductors are soldered to pins and the other two are jumpered. This allows me to use two ESTOP contacts if I'd like for better safety.
