[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]