Can Teensy drive this somewhat odd servo system?

rzeune · Apr 22, 2023

I have a machine that has 6 primary axes and 2 auxiliary axes. It is design for contouring applications i.e. each axis follows some sort of Bezier curve. The motors are intelligent servos, with the drives built in. For this type of application, the motors take a step/direction pulse train where each pulse essentially commands the motor to move 1 encoder count in distance. The motor filters the pulse train to limit the motion jerk to a predefined value.

The trajectory planner will break the move into 10ms segments and send me an array of positions. Each value will be how many pulses to change the current position for each 10ms interval. So in essence, my job is to output the specified number of pulses (plus the direction signal) to each of 8 motors, every 10ms for the duration of the move.

Since the motor filters the input, the pulses do not have to be evenly spaced and can be up to 700KHz. After looking at the max speeds and accel/decel, etc. it looks like I can set the frequency to 500KHz and have a margin of comfort.

Ideally, I'd like to load each of the 8 PWM units with the number of pulses to output during the next period, every 10ms. Also ideally, I'd like to use DMA so I could do other things while waiting for the move to complete.

The manual for the various PWM and Flex units on the Teensy 4.1 is quite complex and I don't see a clear way to accomplish this. Can anyone with related experience please lend some guidance? Any examples?
Thanks

jibaer · Apr 22, 2023

I did not dive into the manual, but my idea for your problem would be the following:
- use one clock for all channels as external clock to the PWM/timers
- use external HW to gate the clock with the output from the PWM/timers

Every 10ms run the following:
- stop clock
- load each PWM channel with clock count needed
- prepare DMA for all PWM channels with a reload value of 0
- start clock

luni · Apr 23, 2023

How many steps do you expect per 10ms interval from your motion planner? If it is not too much, you could simply send them to the controllers @700kHz in a 10ms periodic interrupt (or triggered by the arrival of a new packet from the motion planner)

Just a quick estimation: If your servos run with a max step rate of 100kHz you would need to send a maximum of 100'000stp/sec * 0.01 sec = 1000 pulses per motor to the driver each 10ms. At a max pulse frequency of 700kHz that would take 1/(700'000 pulses/sec) *1000pulses = 1.4ms. Basically you can send the pulses to all controllers in parallel in the same loop, so that the additional overhead for 8 controllers instead of 1 controller would be small. Processor load would be roughly 1.4ms / 10ms = 14%.

rzeune · Apr 23, 2023

jibaer said:
I did not dive into the manual, but my idea for your problem would be the following:
- use one clock for all channels as external clock to the PWM/timers
- use external HW to gate the clock with the output from the PWM/timers

Every 10ms run the following:
- stop clock
- load each PWM channel with clock count needed
- prepare DMA for all PWM channels with a reload value of 0
- start clock

Thanks for the reply but I'm hoping to find a cleaner solution without external h/w.

rzeune · Apr 23, 2023

luni said:
How many steps do you expect per 10ms interval from your motion planner? If it is not too much, you could simply send them to the controllers @700kHz in a 10ms periodic interrupt (or triggered by the arrival of a new packet from the motion planner)

Just a quick estimation: If your servos run with a max step rate of 100kHz you would need to send a maximum of 100'000stp/sec * 0.01 sec = 1000 pulses per motor to the driver each 10ms. At a max pulse frequency of 700kHz that would take 1/(700'000 pulses/sec) *1000pulses = 1.4ms. Basically you can send the pulses to all controllers in parallel in the same loop, so that the additional overhead for 8 controllers instead of 1 controller would be small. Processor load would be roughly 1.4ms / 10ms = 14%.

The servos have a max rate of 700KHz and the practical limit is closer to 500KHz or 5000 steps per 10ms tic.

luni · Apr 23, 2023

The trajectory planner will break the move into 10ms segments and send me an array of positions. Each value will be how many pulses to change the current position for each 10ms interval.

Just to make sure: the trajectory planner will send you values of up to 5000 per 10ms per motor right?

rzeune · Apr 23, 2023

I just realized that this board has 8 serial ports with FIFOs. Perhaps I could set the baud rate near 500KHz and just construct a string of 8-bit data with the correct number b01 pairs? Anyone ever tried this?

rzeune · Apr 23, 2023

Yes luni, that's correct. 5000 pulses per 10ms period max. That corresponds to a motor speed of about 4700 rpm.

luni · Apr 23, 2023

Yes luni, that's correct. 5000 pulses per 10ms period max. That corresponds to a motor speed of about 4700 rpm.

Thanks, I can do a quick test, shouldn't be too difficult.

luni · Apr 23, 2023

Here a prove of principle. It uses GPIO6_16- GPIO6-31 which are routed to teensy pins. Of course, if you want to output 5000 pulses at 500kHz the pulstrain will be 10 ms long which is borderline if you need a pulsetrain every 10ms. I therfore generate the pulsetrain with some 700kHz frequency which should still be OK according to your information in #1.

To minimize the ISR time I prepared the data in advance (random distances), so that one only needs to copy the prepared data to the corresponding GPIO registers in the ISR. The code is not really optimized but shows how it could be done.

Code:

[size=3][color=#000000][/color][color=#000099]#include[/color] [color=#000099]"Arduino.h"[/color][color=#000099][/color]
[color=#000000][/color][color=#000099]#include[/color] [color=#000099]"TeensyTimerTool.h"[/color][color=#000099][/color]
[color=#000000][/color][color=#001177]using namespace[/color] [color=#000000]TeensyTimerTool[/color][color=#000000];[/color] [color=#000000][/color][color=#0b810d]// https://github.com/luni64/TeensyTimerTool[/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color][color=#001177]constexpr[/color] [color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]maxSteps[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]5000[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]maxSteps[/color][color=#000000]];[/color] [color=#000000][/color][color=#0b810d]// one unsigned per step[/color]
[color=#000000][/color]
[color=#000000]PeriodicTimer[/color] [color=#000000][b]pulseTimer[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]TMR4[/color][color=#000000]);[/color]  [color=#000000][/color][color=#0b810d]// use TMR since they run with 150MHz, set prescaler to PSC_1 in the config file to get better granularity as the default PSC_128[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]prepareData[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#000000][b]memset[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]data[/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#001177]sizeof[/color][color=#000000][/color][color=#000000]([/color][color=#000000]data[/color][color=#000000]));[/color]
[color=#000000][/color]
[color=#000000][/color]    [color=#0000ff]unsigned[/color] [color=#000000]distance[/color][color=#000000][[/color][color=#000000][/color][color=#a52a2a]8[/color][color=#000000][/color][color=#000000]];[/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color][color=#000000][/color][color=#000000]&[/color] [color=#000000]dist[/color] [color=#000000]:[/color] [color=#000000]distance[/color][color=#000000])[/color] [color=#000000]dist[/color] [color=#000000]=[/color] [color=#000000][/color][color=#000000][b]random[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]maxSteps[/color][color=#000000]);[/color]
[color=#000000][/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]stp[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]stp[/color] [color=#000000]<[/color] [color=#000000]maxSteps[/color][color=#000000];[/color] [color=#000000]stp[/color][color=#000000]++)[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000][/color]        [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]mot[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]mot[/color] [color=#000000]<[/color] [color=#000000][/color][color=#a52a2a]8[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]mot[/color][color=#000000]++)[/color]
[color=#000000][/color]        [color=#000000]{[/color]
[color=#000000][/color]            [color=#001177]if[/color] [color=#000000][/color][color=#000000]([/color][color=#000000]stp[/color] [color=#000000]<=[/color] [color=#000000]distance[/color][color=#000000][[/color][color=#000000]mot[/color][color=#000000]])[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stp[/color][color=#000000]] |= ([/color][color=#000000][/color][color=#a52a2a]1[/color] [color=#000000][/color][color=#000000]<< ([/color][color=#000000][/color][color=#a52a2a]16[/color] [color=#000000][/color][color=#000000]+[/color] [color=#000000]mot[/color][color=#000000]));[/color]
[color=#000000][/color]        [color=#000000]}[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]doPulse[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#0000ff]static unsigned[/color] [color=#000000]stepNr[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#0000ff]static bool[/color] [color=#000000]reset[/color]      [color=#000000]=[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#001177]if[/color] [color=#000000][/color][color=#000000](![/color][color=#000000]reset[/color][color=#000000])[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000]        GPIO6_DR_SET[/color] [color=#000000]=[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stepNr[/color][color=#000000]];[/color]
[color=#000000]        reset[/color]        [color=#000000]=[/color] [color=#000000][/color][color=#001177]true[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color]    [color=#001177]else[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000]        GPIO6_DR_CLEAR[/color] [color=#000000]=[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stepNr[/color][color=#000000]++];[/color]
[color=#000000]        reset[/color]          [color=#000000]=[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]        [color=#001177]if[/color] [color=#000000][/color][color=#000000]([/color][color=#000000]stepNr[/color] [color=#000000]==[/color] [color=#000000]maxSteps[/color][color=#000000])[/color]
[color=#000000][/color]        [color=#000000]{[/color]
[color=#000000]            pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]stop[/b][/color][color=#000000][/color][color=#000000]();[/color]
[color=#000000]            stepNr[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]        [color=#000000]}[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]setup[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]pin[/color] [color=#000000]: {[/color][color=#000000][/color][color=#a52a2a]25[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]19[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]18[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]14[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]15[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]40[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]41[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]17[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]16[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]22[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]23[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]20[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]21[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]38[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]39[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]26[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]27[/color][color=#000000][/color][color=#000000]})[/color] [color=#000000][/color][color=#000000][b]pinMode[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]pin[/color][color=#000000],[/color] [color=#000000]OUTPUT[/color][color=#000000]);[/color] [color=#000000][/color][color=#0b810d]// those pins form a 16bit word on GPIO6 (GPIO6_16 - GPIO6_31)[/color]
[color=#000000][/color]    [color=#000000][b]pinMode[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]OUTPUT[/color][color=#000000]);[/color]
[color=#000000][/color]
[color=#000000]    pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]begin[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]doPulse[/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]0.7[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000]);[/color]  [color=#000000][/color][color=#0b810d]// set period to ~0.7µs, don't start the timer yet[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]loop[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#000000][b]digitalWriteFast[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]HIGH[/color][color=#000000]);[/color] [color=#000000][/color][color=#0b810d]// measure time needed for data preparation[/color]
[color=#000000][/color]    [color=#000000][b]prepareData[/b][/color][color=#000000][/color][color=#000000]();[/color]
[color=#000000][/color]    [color=#000000][b]digitalWriteFast[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]LOW[/color][color=#000000]);[/color]
[color=#000000]    pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]start[/b][/color][color=#000000][/color][color=#000000]();[/color] [color=#000000][/color][color=#0b810d]// output one pulse train[/color]
[color=#000000][/color]    [color=#000000][b]delay[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]10[/color][color=#000000][/color][color=#000000]);[/color]          [color=#000000][/color][color=#0b810d]// this is just a quick and dirty test, should be restarted by the motion planner of course[/color]
[color=#000000][/color][color=#000000]}[/color][color=#000000][/color]
[/size]

Channel 0 shows the time during prepareData, Channels 1-7 show the first 7 motors

Zoomed in

I didn't take care of the direction pins. But, since dir is constant during one pulsetrain it should be straight forward to set the pins before starting the sequence. All in all it works but generating 1.4MHz interrupt frequency is generating quite some load on the processor. Probably, your idea to use DMA is worth looking into it.
You could also look at FlexIO.

rzeune · Apr 23, 2023

luni said:

Here a prove of principle. It uses GPIO6_16- GPIO6-31 which are routed to teensy pins. Of course, if you want to output 5000 pulses at 500kHz the pulstrain will be 10 ms long which is borderline if you need a pulsetrain every 10ms. I therfore generate the pulsetrain with some 700kHz frequency which should still be OK according to your information in #1.

To minimize the ISR time I prepared the data in advance (random distances), so that one only needs to copy the prepared data to the corresponding GPIO registers in the ISR. The code is not really optimized but shows how it could be done.

Code:

[size=3][color=#000000][/color][color=#000099]#include[/color] [color=#000099]"Arduino.h"[/color][color=#000099][/color]
[color=#000000][/color][color=#000099]#include[/color] [color=#000099]"TeensyTimerTool.h"[/color][color=#000099][/color]
[color=#000000][/color][color=#001177]using namespace[/color] [color=#000000]TeensyTimerTool[/color][color=#000000];[/color] [color=#000000][/color][color=#0b810d]// https://github.com/luni64/TeensyTimerTool[/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color][color=#001177]constexpr[/color] [color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]maxSteps[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]5000[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]maxSteps[/color][color=#000000]];[/color] [color=#000000][/color][color=#0b810d]// one unsigned per step[/color]
[color=#000000][/color]
[color=#000000]PeriodicTimer[/color] [color=#000000][b]pulseTimer[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]TMR4[/color][color=#000000]);[/color]  [color=#000000][/color][color=#0b810d]// use TMR since they run with 150MHz, set prescaler to PSC_1 in the config file to get better granularity as the default PSC_128[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]prepareData[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#000000][b]memset[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]data[/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#001177]sizeof[/color][color=#000000][/color][color=#000000]([/color][color=#000000]data[/color][color=#000000]));[/color]
[color=#000000][/color]
[color=#000000][/color]    [color=#0000ff]unsigned[/color] [color=#000000]distance[/color][color=#000000][[/color][color=#000000][/color][color=#a52a2a]8[/color][color=#000000][/color][color=#000000]];[/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color][color=#000000][/color][color=#000000]&[/color] [color=#000000]dist[/color] [color=#000000]:[/color] [color=#000000]distance[/color][color=#000000])[/color] [color=#000000]dist[/color] [color=#000000]=[/color] [color=#000000][/color][color=#000000][b]random[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]maxSteps[/color][color=#000000]);[/color]
[color=#000000][/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]stp[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]stp[/color] [color=#000000]<[/color] [color=#000000]maxSteps[/color][color=#000000];[/color] [color=#000000]stp[/color][color=#000000]++)[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000][/color]        [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]mot[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]mot[/color] [color=#000000]<[/color] [color=#000000][/color][color=#a52a2a]8[/color][color=#000000][/color][color=#000000];[/color] [color=#000000]mot[/color][color=#000000]++)[/color]
[color=#000000][/color]        [color=#000000]{[/color]
[color=#000000][/color]            [color=#001177]if[/color] [color=#000000][/color][color=#000000]([/color][color=#000000]stp[/color] [color=#000000]<=[/color] [color=#000000]distance[/color][color=#000000][[/color][color=#000000]mot[/color][color=#000000]])[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stp[/color][color=#000000]] |= ([/color][color=#000000][/color][color=#a52a2a]1[/color] [color=#000000][/color][color=#000000]<< ([/color][color=#000000][/color][color=#a52a2a]16[/color] [color=#000000][/color][color=#000000]+[/color] [color=#000000]mot[/color][color=#000000]));[/color]
[color=#000000][/color]        [color=#000000]}[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]doPulse[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#0000ff]static unsigned[/color] [color=#000000]stepNr[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#0000ff]static bool[/color] [color=#000000]reset[/color]      [color=#000000]=[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#001177]if[/color] [color=#000000][/color][color=#000000](![/color][color=#000000]reset[/color][color=#000000])[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000]        GPIO6_DR_SET[/color] [color=#000000]=[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stepNr[/color][color=#000000]];[/color]
[color=#000000]        reset[/color]        [color=#000000]=[/color] [color=#000000][/color][color=#001177]true[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color]    [color=#001177]else[/color]
[color=#000000][/color]    [color=#000000]{[/color]
[color=#000000]        GPIO6_DR_CLEAR[/color] [color=#000000]=[/color] [color=#000000]data[/color][color=#000000][[/color][color=#000000]stepNr[/color][color=#000000]++];[/color]
[color=#000000]        reset[/color]          [color=#000000]=[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]        [color=#001177]if[/color] [color=#000000][/color][color=#000000]([/color][color=#000000]stepNr[/color] [color=#000000]==[/color] [color=#000000]maxSteps[/color][color=#000000])[/color]
[color=#000000][/color]        [color=#000000]{[/color]
[color=#000000]            pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]stop[/b][/color][color=#000000][/color][color=#000000]();[/color]
[color=#000000]            stepNr[/color] [color=#000000]=[/color] [color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000];[/color]
[color=#000000][/color]        [color=#000000]}[/color]
[color=#000000][/color]    [color=#000000]}[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]setup[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#001177]while[/color] [color=#000000][/color][color=#000000](![/color][color=#000000]Serial[/color][color=#000000]) {}[/color]
[color=#000000][/color]
[color=#000000][/color]    [color=#001177]for[/color] [color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#0000ff]unsigned[/color] [color=#000000]pin[/color] [color=#000000]: {[/color][color=#000000][/color][color=#a52a2a]25[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]19[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]18[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]14[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]15[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]40[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]41[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]17[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]16[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]22[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]23[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]20[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]21[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]38[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]39[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]26[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]27[/color][color=#000000][/color][color=#000000]})[/color] [color=#000000][/color][color=#000000][b]pinMode[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]pin[/color][color=#000000],[/color] [color=#000000]OUTPUT[/color][color=#000000]);[/color] [color=#000000][/color][color=#0b810d]// those pins form a 16bit word on GPIO6 (GPIO6_16 - GPIO6_31)[/color]
[color=#000000][/color]    [color=#000000][b]pinMode[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]OUTPUT[/color][color=#000000]);[/color]
[color=#000000][/color]
[color=#000000]    pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]begin[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000]doPulse[/color][color=#000000],[/color] [color=#000000][/color][color=#a52a2a]0.7[/color][color=#000000][/color][color=#000000],[/color] [color=#000000][/color][color=#001177]false[/color][color=#000000][/color][color=#000000]);[/color]  [color=#000000][/color][color=#0b810d]// set period to ~0.7µs, don't start the timer yet[/color]
[color=#000000][/color][color=#000000]}[/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color]
[color=#000000][/color][color=#0000ff]void[/color] [color=#000000][/color][color=#000000][b]loop[/b][/color][color=#000000][/color][color=#000000]()[/color]
[color=#000000][/color][color=#000000]{[/color]
[color=#000000][/color]    [color=#000000][b]digitalWriteFast[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]HIGH[/color][color=#000000]);[/color] [color=#000000][/color][color=#0b810d]// measure time needed for data preparation[/color]
[color=#000000][/color]    [color=#000000][b]prepareData[/b][/color][color=#000000][/color][color=#000000]();[/color]
[color=#000000][/color]    [color=#000000][b]digitalWriteFast[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]0[/color][color=#000000][/color][color=#000000],[/color] [color=#000000]LOW[/color][color=#000000]);[/color]
[color=#000000]    pulseTimer[/color][color=#000000].[/color][color=#000000][/color][color=#000000][b]start[/b][/color][color=#000000][/color][color=#000000]();[/color] [color=#000000][/color][color=#0b810d]// output one pulse train[/color]
[color=#000000][/color]    [color=#000000][b]delay[/b][/color][color=#000000][/color][color=#000000]([/color][color=#000000][/color][color=#a52a2a]10[/color][color=#000000][/color][color=#000000]);[/color]          [color=#000000][/color][color=#0b810d]// this is just a quick and dirty test, should be restarted by the motion planner of course[/color]
[color=#000000][/color][color=#000000]}[/color][color=#000000][/color]
[/size]

Channel 0 shows the time during prepareData, Channels 1-7 show the first 7 motors
View attachment 30977

Zoomed in
View attachment 30978

I didn't take care of the direction pins. But, since dir is constant during one pulse train it should be straight forward to set the pins before starting the sequence. All in all it works but generating 1.4MHz interrupt frequency is generating quite some load on the processor. Probably, your idea to use DMA is worth looking into it.
You could also look at FlexIO.

Thanks again luni for taking time to reply. This could be an option if I could automate it more with DMA. I'm new to the Teensy family so will invest some more time in the documents and examples.

Can Teensy drive this somewhat odd servo system?

rzeune

Member

jibaer

New member

luni

Well-known member

rzeune

Member

rzeune

Member

luni

Well-known member

rzeune

Member

rzeune

Member

luni

Well-known member

luni

Well-known member

rzeune

Member