Move the library to the separate repository
This commit is contained in:
218
.qtcreator/libsidservo.creator.user
Normal file
218
.qtcreator/libsidservo.creator.user
Normal file
@@ -0,0 +1,218 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
218
.qtcreator/libsidservo.creator.user.cf63021
Normal file
218
.qtcreator/libsidservo.creator.user.cf63021
Normal file
@@ -0,0 +1,218 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 18.0.0, 2025-11-27T17:22:09. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
87
CMakeLists.txt
Normal file
87
CMakeLists.txt
Normal file
@@ -0,0 +1,87 @@
|
||||
cmake_minimum_required(VERSION 3.30)
|
||||
set(PROJ sidservo)
|
||||
set(MINOR_VERSION "1")
|
||||
set(MID_VERSION "0")
|
||||
set(MAJOR_VERSION "0")
|
||||
set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}")
|
||||
|
||||
project(${PROJ} VERSION ${VERSION} LANGUAGES C)
|
||||
|
||||
# default flags
|
||||
set(CMAKE_C_FLAGS "${CFLAGS} -O2")
|
||||
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS}")
|
||||
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS} -Wextra -Wall -Werror -W")
|
||||
set(CMAKE_COLOR_MAKEFILE ON)
|
||||
|
||||
option(DEBUG "Compile in debug mode" OFF)
|
||||
option(EXAMPLES "Compile also some examples" ON)
|
||||
|
||||
# cmake -DDEBUG=on -> debugging
|
||||
if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
|
||||
if(DEBUG)
|
||||
set(CMAKE_BUILD_TYPE "Debug")
|
||||
else()
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
|
||||
add_definitions(-DEBUG)
|
||||
set(CMAKE_VERBOSE_MAKEFILE true)
|
||||
if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
|
||||
message("install to ${CMAKE_CURRENT_SOURCE_DIR}/install ")
|
||||
set(CMAKE_INSTALL_PREFIX ${CMAKE_CURRENT_SOURCE_DIR}/install)
|
||||
endif()
|
||||
set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS_DEBUG})
|
||||
else()
|
||||
set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS_RELEASE})
|
||||
endif()
|
||||
|
||||
message("Build type: ${CMAKE_BUILD_TYPE}, cflags: ${CMAKE_C_FLAGS}")
|
||||
|
||||
aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES)
|
||||
|
||||
###### pkgconfig ######
|
||||
# pkg-config modules (for pkg-check-modules)
|
||||
#set(MODULES cfitsio fftw3)
|
||||
# find packages:
|
||||
#find_package(PkgConfig REQUIRED)
|
||||
#pkg_check_modules(${PROJ} REQUIRED ${MODULES})
|
||||
|
||||
###### additional flags ######
|
||||
#list(APPEND ${PROJ}_LIBRARIES "-lfftw3_threads")
|
||||
|
||||
# library
|
||||
add_library(${PROJ} SHARED ${SOURCES})
|
||||
# library header files
|
||||
set(LIBHEADER "sidservo.h")
|
||||
# -I
|
||||
include_directories(${${PROJ}_INCLUDE_DIRS})
|
||||
# -L
|
||||
link_directories(${${PROJ}_LIBRARY_DIRS})
|
||||
# -D
|
||||
add_definitions(
|
||||
-DPACKAGE_VERSION=\"${VERSION}\" -DMINOR_VERSION=\"${MINOR_VERSION}\"
|
||||
-DMID_VERSION=\"${MID_VERSION}\" -DMAJOR_VERSION=\"${MAJOR_VESION}\"
|
||||
)
|
||||
|
||||
# -l
|
||||
target_link_libraries(${PROJ} ${${PROJ}_LIBRARIES})
|
||||
|
||||
set(PCFILE "${CMAKE_BINARY_DIR}/${PROJ}.pc")
|
||||
configure_file("${PROJ}.pc.in" ${PCFILE} @ONLY)
|
||||
|
||||
set_target_properties(${PROJ} PROPERTIES VERSION ${VERSION})
|
||||
set_target_properties(${PROJ} PROPERTIES PUBLIC_HEADER ${LIBHEADER})
|
||||
|
||||
# Installation of the program
|
||||
include(GNUInstallDirs)
|
||||
#install(TARGETS ${PROJ} DESTINATION "bin")
|
||||
install(TARGETS ${PROJ} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
|
||||
PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
|
||||
install(FILES ${PCFILE} DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/pkgconfig)
|
||||
|
||||
# EXAMPLES
|
||||
if(EXAMPLES)
|
||||
add_subdirectory(examples)
|
||||
endif()
|
||||
221
PID.c
Normal file
221
PID.c
Normal file
@@ -0,0 +1,221 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <strings.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "PID.h"
|
||||
#include "serial.h"
|
||||
|
||||
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){
|
||||
if(!gain || Iarrsz < 3) return NULL;
|
||||
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
|
||||
pid->gain = *gain;
|
||||
DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D);
|
||||
pid->pidIarrSize = Iarrsz;
|
||||
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
|
||||
return pid;
|
||||
}
|
||||
|
||||
// don't clear lastT!
|
||||
void pid_clear(PIDController_t *pid){
|
||||
if(!pid) return;
|
||||
DBG("CLEAR PID PARAMETERS");
|
||||
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||
pid->integral = 0.;
|
||||
pid->prev_error = 0.;
|
||||
pid->curIidx = 0;
|
||||
}
|
||||
|
||||
void pid_delete(PIDController_t **pid){
|
||||
if(!pid || !*pid) return;
|
||||
if((*pid)->pidIarray) free((*pid)->pidIarray);
|
||||
free(*pid);
|
||||
*pid = NULL;
|
||||
}
|
||||
|
||||
double pid_calculate(PIDController_t *pid, double error, double dt){
|
||||
// calculate flowing integral
|
||||
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
|
||||
//DBG("oldi/new: %g, %g", oldi, newi);
|
||||
pid->pidIarray[pid->curIidx++] = newi;
|
||||
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
|
||||
pid->integral += newi - oldi;
|
||||
double derivative = (error - pid->prev_error) / dt;
|
||||
pid->prev_error = error;
|
||||
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
|
||||
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
|
||||
return sum;
|
||||
}
|
||||
|
||||
typedef struct{
|
||||
PIDController_t *PIDC;
|
||||
PIDController_t *PIDV;
|
||||
} PIDpair_t;
|
||||
|
||||
typedef struct{
|
||||
axis_status_t state;
|
||||
coordval_t position;
|
||||
coordval_t speed;
|
||||
} axisdata_t;
|
||||
/**
|
||||
* @brief process - Process PID for given axis
|
||||
* @param tagpos - given coordinate of target position
|
||||
* @param endpoint - endpoint for this coordinate
|
||||
* @param pid - pid itself
|
||||
* @return calculated new speed or -1 for max speed
|
||||
*/
|
||||
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
|
||||
double dt = timediff(&tagpos->t, &axis->position.t);
|
||||
if(dt < 0 || dt > Conf.PIDMaxDt){
|
||||
DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt);
|
||||
return axis->speed.val; // data is too old or wrong
|
||||
}
|
||||
double error = tagpos->val - axis->position.val, fe = fabs(error);
|
||||
DBG("error: %g", error);
|
||||
PIDController_t *pid = NULL;
|
||||
switch(axis->state){
|
||||
case AXIS_SLEWING:
|
||||
if(fe < Conf.MaxPointingErr){
|
||||
axis->state = AXIS_POINTING;
|
||||
DBG("--> Pointing");
|
||||
pid = pidpair->PIDC;
|
||||
}else{
|
||||
DBG("Slewing...");
|
||||
return NAN; // max speed for given axis
|
||||
}
|
||||
break;
|
||||
case AXIS_POINTING:
|
||||
if(fe < Conf.MaxFinePointingErr){
|
||||
axis->state = AXIS_GUIDING;
|
||||
DBG("--> Guiding");
|
||||
pid = pidpair->PIDV;
|
||||
}else if(fe > Conf.MaxPointingErr){
|
||||
DBG("--> Slewing");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return NAN;
|
||||
} else pid = pidpair->PIDC;
|
||||
break;
|
||||
case AXIS_GUIDING:
|
||||
pid = pidpair->PIDV;
|
||||
if(fe > Conf.MaxFinePointingErr){
|
||||
DBG("--> Pointing");
|
||||
axis->state = AXIS_POINTING;
|
||||
pid = pidpair->PIDC;
|
||||
}else if(fe < Conf.MaxGuidingErr){
|
||||
DBG("At target");
|
||||
// TODO: we can point somehow that we are at target or introduce new axis state
|
||||
}else DBG("Current error: %g", fe);
|
||||
break;
|
||||
case AXIS_STOPPED: // start pointing to target; will change speed next time
|
||||
DBG("AXIS STOPPED!!!! --> Slewing");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return getspeed(tagpos, pidpair, axis);
|
||||
case AXIS_ERROR:
|
||||
DBG("Can't move from erroneous state");
|
||||
return 0.;
|
||||
}
|
||||
if(!pid){
|
||||
DBG("WTF? Where is a PID?");
|
||||
return axis->speed.val;
|
||||
}
|
||||
double dtpid = timediff(&tagpos->t, &pid->prevT);
|
||||
if(dtpid < 0 || dtpid > Conf.PIDMaxDt){
|
||||
DBG("time diff too big: clear PID");
|
||||
pid_clear(pid);
|
||||
}
|
||||
if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt;
|
||||
pid->prevT = tagpos->t;
|
||||
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state);
|
||||
double tagspeed = pid_calculate(pid, error, dtpid);
|
||||
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based
|
||||
return tagspeed; // coordinate-based
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief correct2 - recalculate PID and move telescope to new point with new speed
|
||||
* @param target - target position (for error calculations)
|
||||
* @param endpoint - stop point (some far enough point to stop in case of hang)
|
||||
* @return error code
|
||||
*/
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target){
|
||||
static PIDpair_t pidX = {0}, pidY = {0};
|
||||
if(!pidX.PIDC){
|
||||
pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDC) return MCC_E_FATAL;
|
||||
pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
if(!pidY.PIDC){
|
||||
pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDC) return MCC_E_FATAL;
|
||||
pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
mountdata_t m;
|
||||
coordpair_t tagspeed; // absolute value of speed
|
||||
double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
|
||||
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
|
||||
axisdata_t axis;
|
||||
DBG("state: %d/%d", m.Xstate, m.Ystate);
|
||||
axis.state = m.Xstate;
|
||||
axis.position = m.encXposition;
|
||||
axis.speed = m.encXspeed;
|
||||
tagspeed.X = getspeed(&target->X, &pidX, &axis);
|
||||
if(isnan(tagspeed.X)){ // max speed
|
||||
if(target->X.val < axis.position.val) Xsign = -1.;
|
||||
tagspeed.X = Xlimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
|
||||
if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
|
||||
}
|
||||
axis_status_t xstate = axis.state;
|
||||
axis.state = m.Ystate;
|
||||
axis.position = m.encYposition;
|
||||
axis.speed = m.encYspeed;
|
||||
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
|
||||
if(isnan(tagspeed.Y)){ // max speed
|
||||
if(target->Y.val < axis.position.val) Ysign = -1.;
|
||||
tagspeed.Y = Ylimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
|
||||
if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
|
||||
}
|
||||
axis_status_t ystate = axis.state;
|
||||
if(m.Xstate != xstate || m.Ystate != ystate){
|
||||
DBG("State changed");
|
||||
setStat(xstate, ystate);
|
||||
}
|
||||
coordpair_t endpoint;
|
||||
// allow at least PIDMaxDt moving with target speed
|
||||
double dv = fabs(tagspeed.X - m.encXspeed.val);
|
||||
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
|
||||
+ Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
|
||||
+ tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
|
||||
endpoint.X = m.encXposition.val + Xsign * adder;
|
||||
dv = fabs(tagspeed.Y - m.encYspeed.val);
|
||||
adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
|
||||
+ Conf.PIDMaxDt * tagspeed.Y
|
||||
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
|
||||
endpoint.Y = m.encYposition.val + Ysign * adder;
|
||||
DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
|
||||
return Mount.moveWspeed(&endpoint, &tagspeed);
|
||||
}
|
||||
40
PID.h
Normal file
40
PID.h
Normal file
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
typedef struct {
|
||||
PIDpar_t gain; // PID gains
|
||||
double prev_error; // Previous error
|
||||
double integral; // Integral term
|
||||
double *pidIarray; // array for Integral
|
||||
struct timespec prevT; // time of previous correction
|
||||
size_t pidIarrSize; // it's size
|
||||
size_t curIidx; // and index of current element
|
||||
} PIDController_t;
|
||||
|
||||
PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz);
|
||||
void pid_clear(PIDController_t *pid);
|
||||
void pid_delete(PIDController_t **pid);
|
||||
double pid_calculate(PIDController_t *pid, double error, double dt);
|
||||
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target);
|
||||
141
PID_test.deprecated/Dramp.c
Normal file
141
PID_test.deprecated/Dramp.c
Normal file
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Dramp.h"
|
||||
|
||||
static movestate_t state = ST_STOP;
|
||||
static moveparam_t target, Min, Max;
|
||||
static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate
|
||||
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||
static double T1 = -1.; // time to switch into minimal speed for best coord tolerance
|
||||
|
||||
typedef enum{
|
||||
STAGE_NORMALSPEED,
|
||||
STAGE_MINSPEED,
|
||||
STAGE_STOPPED
|
||||
} movingsage_t;
|
||||
|
||||
static movingsage_t movingstage = STAGE_STOPPED;
|
||||
|
||||
static int initlims(limits_t *lim){
|
||||
if(!lim) return FALSE;
|
||||
Min = lim->min;
|
||||
Max = lim->max;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static int calc(moveparam_t *x, double t){
|
||||
DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed);
|
||||
if(!x || t < 0.) return FALSE;
|
||||
if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
||||
double adist = fabs(x->coord - curparams.coord);
|
||||
DBG("want dist: %g", adist);
|
||||
if(adist < coord_tolerance) return TRUE; // we are at place
|
||||
if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters
|
||||
target = *x;
|
||||
if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position
|
||||
if(target.speed < Min.speed) target.speed = Min.speed;
|
||||
DBG("Approximate tag speed: %g", target.speed);
|
||||
T0 = t;
|
||||
// calculate time to switch into minimal speed
|
||||
T1 = -1.; // no min speed phase
|
||||
if(target.speed > Min.speed){
|
||||
double dxpertick = target.speed * time_tick;
|
||||
DBG("dX per one tick: %g", dxpertick);
|
||||
double ticks_need = floor(adist / dxpertick);
|
||||
DBG("ticks need: %g", ticks_need);
|
||||
if(ticks_need < 1.) return FALSE; // cannot reach
|
||||
if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){
|
||||
DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need);
|
||||
double dxpersmtick = Min.speed * time_tick;
|
||||
DBG("dX per smallest tick: %g", dxpersmtick);
|
||||
while(--ticks_need > 1.){
|
||||
double part = adist - ticks_need * dxpertick;
|
||||
double smticks = floor(part / dxpersmtick);
|
||||
double least = part - smticks * dxpersmtick;
|
||||
if(least < coord_tolerance) break;
|
||||
}
|
||||
DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick);
|
||||
T1 = t + ticks_need * time_tick;
|
||||
}
|
||||
}
|
||||
state = ST_MOVE;
|
||||
Xlast0 = curparams.coord;
|
||||
if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED;
|
||||
else movingstage = STAGE_MINSPEED;
|
||||
if(x->coord < curparams.coord) target.speed *= -1.; // real speed
|
||||
curparams.speed = target.speed;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void stop(double _U_ t){
|
||||
T0 = -1.;
|
||||
curparams.accel = 0.;
|
||||
curparams.speed = 0.;
|
||||
state = ST_STOP;
|
||||
movingstage = STAGE_STOPPED;
|
||||
}
|
||||
|
||||
static movestate_t proc(moveparam_t *next, double t){
|
||||
if(T0 < 0.) return ST_STOP;
|
||||
curparams.coord = Xlast0 + (t - T0) * curparams.speed;
|
||||
//DBG("coord: %g (dTmon: %g, speed: %g)", curparams.coord, t-T0, curparams.speed);
|
||||
int ooops = FALSE; // oops - we are over target!
|
||||
if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;}
|
||||
else{ if(curparams.coord > target.coord) ooops = TRUE; }
|
||||
if(ooops){
|
||||
DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord);
|
||||
stop(t);
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1
|
||||
if(t >= T1){
|
||||
DBG("T1=%g, t=%g -->", T1, t);
|
||||
curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed;
|
||||
movingstage = STAGE_MINSPEED;
|
||||
Xlast0 = curparams.coord;
|
||||
T0 = T1;
|
||||
DBG("Go further with minimal speed");
|
||||
}
|
||||
}
|
||||
if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place
|
||||
DBG("OK, we are in place");
|
||||
stop(t);
|
||||
}
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static movestate_t getst(moveparam_t *cur){
|
||||
if(cur) *cur = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
movemodel_t dumb = {
|
||||
.init_limits = initlims,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stop = stop,
|
||||
.emergency_stop = stop,
|
||||
.get_state = getst,
|
||||
};
|
||||
23
PID_test.deprecated/Dramp.h
Normal file
23
PID_test.deprecated/Dramp.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t dumb;
|
||||
57
PID_test.deprecated/Makefile
Normal file
57
PID_test.deprecated/Makefile
Normal file
@@ -0,0 +1,57 @@
|
||||
# run `make DEF=...` to add extra defines
|
||||
PROGRAM := moving
|
||||
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
|
||||
LDFLAGS += -lusefull_macros -lm
|
||||
SRCS := $(wildcard *.c)
|
||||
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
|
||||
OBJDIR := mk
|
||||
CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
|
||||
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
TARGFILE := $(OBJDIR)/TARGET
|
||||
CC = gcc
|
||||
#TARGET := RELEASE
|
||||
|
||||
ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
|
||||
TARGET := $(file < $(TARGFILE))
|
||||
else
|
||||
TARGET := RELEASE
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET), DEBUG)
|
||||
.DEFAULT_GOAL := debug
|
||||
endif
|
||||
|
||||
release: $(PROGRAM)
|
||||
|
||||
debug: CFLAGS += -DEBUG -Werror
|
||||
debug: TARGET := DEBUG
|
||||
debug: $(PROGRAM)
|
||||
|
||||
$(TARGFILE): $(OBJDIR)
|
||||
@echo -e "\t\tTARGET: $(TARGET)"
|
||||
@echo "$(TARGET)" > $(TARGFILE)
|
||||
|
||||
$(PROGRAM) : $(TARGFILE) $(OBJS)
|
||||
@echo -e "\t\tLD $(PROGRAM)"
|
||||
$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
|
||||
|
||||
$(OBJDIR):
|
||||
@mkdir $(OBJDIR)
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include $(DEPS)
|
||||
endif
|
||||
|
||||
$(OBJDIR)/%.o: %.c
|
||||
@echo -e "\t\tCC $<"
|
||||
$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@
|
||||
|
||||
clean:
|
||||
@echo -e "\t\tCLEAN"
|
||||
@rm -rf $(OBJDIR) 2>/dev/null || true
|
||||
|
||||
xclean: clean
|
||||
@rm -f $(PROGRAM)
|
||||
|
||||
.PHONY: clean xclean
|
||||
27
PID_test.deprecated/Sramp.c
Normal file
27
PID_test.deprecated/Sramp.c
Normal file
@@ -0,0 +1,27 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// quasi non-jerk s-ramp
|
||||
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Sramp.h"
|
||||
|
||||
|
||||
movemodel_t s_shaped = { 0 };
|
||||
23
PID_test.deprecated/Sramp.h
Normal file
23
PID_test.deprecated/Sramp.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t s_shaped;
|
||||
223
PID_test.deprecated/Tramp.c
Normal file
223
PID_test.deprecated/Tramp.c
Normal file
@@ -0,0 +1,223 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// simplest trapezioidal ramp
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Tramp.h"
|
||||
|
||||
#undef DBG
|
||||
#define DBG(...)
|
||||
|
||||
static movestate_t state = ST_STOP;
|
||||
static moveparam_t Min, Max; // `Min` acceleration not used!
|
||||
|
||||
typedef enum{
|
||||
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
|
||||
STAGE_MAXSPEED, // go with target speed
|
||||
STAGE_DECEL, // go from target speed to zero
|
||||
STAGE_STOPPED, // stop
|
||||
STAGE_AMOUNT
|
||||
} movingstage_t;
|
||||
|
||||
static movingstage_t movingstage = STAGE_STOPPED;
|
||||
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
|
||||
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
|
||||
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||
|
||||
static int initlims(limits_t *lim){
|
||||
if(!lim) return FALSE;
|
||||
Min = lim->min;
|
||||
Max = lim->max;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void emstop(double _U_ t){
|
||||
curparams.accel = 0.;
|
||||
curparams.speed = 0.;
|
||||
bzero(Times, sizeof(Times));
|
||||
bzero(Params, sizeof(Params));
|
||||
state = ST_STOP;
|
||||
movingstage = STAGE_STOPPED;
|
||||
}
|
||||
|
||||
static void stop(double t){
|
||||
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
|
||||
movingstage = STAGE_DECEL;
|
||||
state = ST_MOVE;
|
||||
Times[STAGE_DECEL] = t;
|
||||
Params[STAGE_DECEL].speed = curparams.speed;
|
||||
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
|
||||
else Params[STAGE_DECEL].accel = Max.accel;
|
||||
Params[STAGE_DECEL].coord = curparams.coord;
|
||||
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
|
||||
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||
double dt = Times[STAGE_STOPPED] - t;
|
||||
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
|
||||
Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief calc - moving calculation
|
||||
* @param x - using max speed (>0!!!) and coordinate
|
||||
* @param t - current time value
|
||||
* @return FALSE if can't move with given parameters
|
||||
*/
|
||||
static int calc(moveparam_t *x, double t){
|
||||
if(!x) return FALSE;
|
||||
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
||||
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
|
||||
double Dx = fabs(x->coord - curparams.coord); // full distance
|
||||
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
|
||||
// we have two variants: with or without stage with constant speed
|
||||
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
|
||||
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
|
||||
DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
|
||||
double setspeed = x->speed; // new max speed (we can change it if need)
|
||||
double dt01, dx01; // we'll fill them depending on starting conditions
|
||||
Times[0] = t;
|
||||
Params[0].speed = curparams.speed;
|
||||
Params[0].coord = curparams.coord;
|
||||
|
||||
double curspeed = fabs(curparams.speed);
|
||||
double dt0s = curspeed / Max.accel; // time of stopping phase
|
||||
double dx0s = curspeed * dt0s / 2.; // distance
|
||||
DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
|
||||
if(dx0s > Dx){
|
||||
WARNX("distance too short");
|
||||
return FALSE;
|
||||
}
|
||||
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
|
||||
DBG("Distance good to just stop");
|
||||
stop(t);
|
||||
return TRUE;
|
||||
}
|
||||
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
|
||||
// after stop we will have full profile
|
||||
double dxs3 = Dx - dx0s;
|
||||
double newspeed = sqrt(Max.accel * dxs3);
|
||||
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
|
||||
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
|
||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||
Params[0].accel = sign * Max.accel;
|
||||
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
|
||||
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
|
||||
DBG("dx01=%g, dt01=%g", dx01, dt01);
|
||||
}else{ // increase or decrease speed without stopping phase
|
||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||
double a = sign * Max.accel;
|
||||
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
|
||||
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
|
||||
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
|
||||
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
|
||||
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
|
||||
if(dx01 + dx23 > Dx){ // calculate max speed
|
||||
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
|
||||
if(setspeed < curspeed){
|
||||
setspeed = curparams.speed;
|
||||
dt01 = 0.; dx01 = 0.;
|
||||
Params[0].accel = 0.;
|
||||
}else{
|
||||
Params[0].accel = a;
|
||||
dt01 = fabs(setspeed - curspeed) / Max.accel;
|
||||
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
|
||||
}
|
||||
}else Params[0].accel = a;
|
||||
}
|
||||
if(setspeed < Min.speed){
|
||||
WARNX("New speed should be too small");
|
||||
return FALSE;
|
||||
}
|
||||
moveparam_t *p = &Params[STAGE_MAXSPEED];
|
||||
p->accel = 0.; p->speed = sign * setspeed;
|
||||
p->coord = curparams.coord + dx01 * sign;
|
||||
Times[STAGE_MAXSPEED] = Times[0] + dt01;
|
||||
dt23 = setspeed / Max.accel;
|
||||
dx23 = setspeed * dt23 / 2.;
|
||||
// calculate dx12 and dt12
|
||||
double dx12 = Dx - dx01 - dx23;
|
||||
if(dx12 < -coord_tolerance){
|
||||
WARNX("Oops, WTF dx12=%g?", dx12);
|
||||
return FALSE;
|
||||
}
|
||||
double dt12 = dx12 / setspeed;
|
||||
p = &Params[STAGE_DECEL];
|
||||
p->accel = -sign * Max.accel;
|
||||
p->speed = sign * setspeed;
|
||||
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
|
||||
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
|
||||
p = &Params[STAGE_STOPPED];
|
||||
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
|
||||
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
|
||||
for(int i = 0; i < 4; ++i)
|
||||
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
|
||||
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
|
||||
state = ST_MOVE;
|
||||
movingstage = STAGE_ACCEL;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static movestate_t proc(moveparam_t *next, double t){
|
||||
if(state == ST_STOP) goto ret;
|
||||
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||
if(Times[s] <= t){ // check time for current stage
|
||||
movingstage = s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(movingstage == STAGE_STOPPED){
|
||||
curparams.coord = Params[STAGE_STOPPED].coord;
|
||||
emstop(t);
|
||||
goto ret;
|
||||
}
|
||||
// calculate current parameters
|
||||
double dt = t - Times[movingstage];
|
||||
double a = Params[movingstage].accel;
|
||||
double v0 = Params[movingstage].speed;
|
||||
double x0 = Params[movingstage].coord;
|
||||
curparams.accel = a;
|
||||
curparams.speed = v0 + a * dt;
|
||||
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||
ret:
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static movestate_t getst(moveparam_t *cur){
|
||||
if(cur) *cur = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static double gettstop(){
|
||||
return Times[STAGE_STOPPED];
|
||||
}
|
||||
|
||||
movemodel_t trapez = {
|
||||
.init_limits = initlims,
|
||||
.stop = stop,
|
||||
.emergency_stop = emstop,
|
||||
.get_state = getst,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stoppedtime = gettstop,
|
||||
};
|
||||
23
PID_test.deprecated/Tramp.h
Normal file
23
PID_test.deprecated/Tramp.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t trapez;
|
||||
243
PID_test.deprecated/main.c
Normal file
243
PID_test.deprecated/main.c
Normal file
@@ -0,0 +1,243 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
// errors for states: slewing/pointing/guiding
|
||||
#define MAX_POINTING_ERR (50.)
|
||||
#define MAX_GUIDING_ERR (5.)
|
||||
// timeout to "forget" old data from I sum array; seconds
|
||||
#define PID_I_PERIOD (3.)
|
||||
|
||||
static movemodel_t *model = NULL;
|
||||
static FILE *coordslog = NULL;
|
||||
|
||||
typedef enum{
|
||||
Slewing,
|
||||
Pointing,
|
||||
Guiding
|
||||
} state_t;
|
||||
|
||||
static state_t state = Slewing;
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
char *ramptype;
|
||||
char *xlog;
|
||||
double dTmon;
|
||||
double dTcorr;
|
||||
double Tend;
|
||||
double minerr;
|
||||
double P, I, D;
|
||||
} pars;
|
||||
|
||||
static pars G = {
|
||||
.ramptype = "t",
|
||||
.dTmon = 0.01,
|
||||
.dTcorr = 0.05,
|
||||
.Tend = 100.,
|
||||
.minerr = 0.1,
|
||||
.P = 0.8,
|
||||
};
|
||||
|
||||
static limits_t limits = {
|
||||
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
||||
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
|
||||
.jerk = 10.
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
double kp, ki, kd; // PID gains
|
||||
double prev_error; // Previous error
|
||||
double integral; // Integral term
|
||||
double *pidIarray; // array for Integral
|
||||
size_t pidIarrSize; // it's size
|
||||
size_t curIidx; // and index of current element
|
||||
} PIDController;
|
||||
|
||||
static PIDController pid;
|
||||
|
||||
static sl_option_t opts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
|
||||
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
|
||||
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
|
||||
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
||||
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
|
||||
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
|
||||
{"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"},
|
||||
{"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"},
|
||||
{"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"},
|
||||
// TODO: add parameters for limits setting
|
||||
end_option
|
||||
};
|
||||
|
||||
// calculate coordinate target for given time (starting from zero)
|
||||
static double target_coord(double t){
|
||||
if(t > 20. && t < 30.) return target_coord(20.);
|
||||
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
|
||||
return pos;
|
||||
}
|
||||
|
||||
/* P-only == oscillations
|
||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||
double error = targcoord - p->coord;
|
||||
if(fabs(error) < G.minerr) return p->speed;
|
||||
return p->speed + error / dt / 500.;
|
||||
}
|
||||
*/
|
||||
|
||||
static void pid_init(PIDController *pid, double kp, double ki, double kd) {
|
||||
pid->kp = fabs(kp);
|
||||
pid->ki = fabs(ki);
|
||||
pid->kd = fabs(kd);
|
||||
pid->prev_error = 0.;
|
||||
pid->integral = 0.;
|
||||
pid->curIidx = 0;
|
||||
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
|
||||
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
|
||||
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
|
||||
}
|
||||
|
||||
static void pid_clear(PIDController *pid){
|
||||
if(!pid) return;
|
||||
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||
pid->integral = 0.;
|
||||
pid->prev_error = 0.;
|
||||
pid->curIidx = 0;
|
||||
}
|
||||
|
||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||
double error = targcoord - p->coord, fe = fabs(error);
|
||||
switch(state){
|
||||
case Slewing:
|
||||
if(fe < MAX_POINTING_ERR){
|
||||
pid_clear(&pid);
|
||||
state = Pointing;
|
||||
green("--> Pointing\n");
|
||||
}else{
|
||||
red("Slewing...\n");
|
||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||
}
|
||||
break;
|
||||
case Pointing:
|
||||
if(fe < MAX_GUIDING_ERR){
|
||||
pid_clear(&pid);
|
||||
state = Guiding;
|
||||
green("--> Guiding\n");
|
||||
}else if(fe > MAX_POINTING_ERR){
|
||||
red("--> Slewing\n");
|
||||
state = Slewing;
|
||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||
}
|
||||
break;
|
||||
case Guiding:
|
||||
if(fe > MAX_GUIDING_ERR){
|
||||
red("--> Pointing\n");
|
||||
state = Pointing;
|
||||
}else if(fe < G.minerr){
|
||||
green("At target\n");
|
||||
//pid_clear(&pid);
|
||||
//return p->speed;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
red("Calculate PID\n");
|
||||
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
|
||||
pid.pidIarray[pid.curIidx++] = oldi;
|
||||
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
|
||||
pid.integral += newi - oldi;
|
||||
double derivative = (error - pid.prev_error) / dt;
|
||||
pid.prev_error = error;
|
||||
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
|
||||
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
|
||||
if(state == Pointing) add /= 3.;
|
||||
else if(state == Guiding) add /= 7.;
|
||||
DBG("ADD = %g; new speed = %g", add, p->speed + add);
|
||||
if(state == Guiding) return p->speed + add / dt / 10.;
|
||||
return add / dt;
|
||||
}
|
||||
// ./moving -l coords -P.5 -I.05 -D1.5
|
||||
// ./moving -l coords -P1.3 -D1.6
|
||||
|
||||
static void start_model(double Tend){
|
||||
double T = 0., Tcorr = 0.;//, Tlast = 0.;
|
||||
moveparam_t target;
|
||||
while(T <= Tend){
|
||||
moveparam_t p;
|
||||
movestate_t st = model->get_state(&p);
|
||||
if(st == ST_MOVE) st = model->proc_move(&p, T);
|
||||
double nextcoord = target_coord(T);
|
||||
double error = nextcoord - p.coord;
|
||||
if(T - Tcorr >= G.dTcorr){ // check correction
|
||||
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
|
||||
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
|
||||
target.speed = fabs(speed);
|
||||
double res_speed = limits.max.speed / 2.;
|
||||
if(target.speed > limits.max.speed){
|
||||
target.speed = limits.max.speed;
|
||||
res_speed = limits.max.speed / 4.;
|
||||
}else if(target.speed < limits.min.speed){
|
||||
target.speed = limits.min.speed;
|
||||
res_speed = limits.min.speed * 4.;
|
||||
}
|
||||
if(!move_to(&target, T)){
|
||||
target.speed = res_speed;
|
||||
if(!move_to(&target, T))
|
||||
WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed);
|
||||
}
|
||||
DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error);
|
||||
Tcorr = T;
|
||||
}
|
||||
// make log
|
||||
fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
|
||||
T, nextcoord, p.coord, p.speed, p.accel, error);
|
||||
T += G.dTmon;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, opts);
|
||||
if(G.help) sl_showhelp(-1, opts);
|
||||
if(G.xlog){
|
||||
coordslog = fopen(G.xlog, "w");
|
||||
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||
} else coordslog = stdout;
|
||||
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
||||
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
||||
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
||||
pid_init(&pid, G.P, G.I, G.D);
|
||||
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
||||
ramptype_t ramp = RAMP_AMOUNT;
|
||||
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
|
||||
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
|
||||
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
|
||||
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
|
||||
model = init_moving(ramp, &limits);
|
||||
if(!model) ERRX("Can't init moving model: check parameters");
|
||||
start_model(G.Tend);
|
||||
fclose(coordslog);
|
||||
return 0;
|
||||
}
|
||||
105
PID_test.deprecated/moving.c
Normal file
105
PID_test.deprecated/moving.c
Normal file
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
#include <pthread.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "moving.h"
|
||||
#include "moving_private.h"
|
||||
#include "Dramp.h"
|
||||
#include "Sramp.h"
|
||||
#include "Tramp.h"
|
||||
|
||||
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static movemodel_t *model = NULL;
|
||||
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
double time_tick = TIME_TICK_DEFAULT;
|
||||
|
||||
// difference of time from first call, using nanoseconds
|
||||
double nanot(){
|
||||
static struct timespec *start = NULL;
|
||||
struct timespec now;
|
||||
if(!start){
|
||||
start = MALLOC(struct timespec, 1);
|
||||
if(!start) return -1.;
|
||||
if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
|
||||
}
|
||||
if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
|
||||
//DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
|
||||
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
||||
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
||||
return sd + nd;
|
||||
}
|
||||
|
||||
static void chkminmax(double *min, double *max){
|
||||
if(*min <= *max) return;
|
||||
double t = *min;
|
||||
*min = *max;
|
||||
*max = t;
|
||||
}
|
||||
|
||||
movemodel_t *init_moving(ramptype_t type, limits_t *l){
|
||||
if(!l) return FALSE;
|
||||
switch(type){
|
||||
case RAMP_DUMB:
|
||||
model = &dumb;
|
||||
break;
|
||||
case RAMP_TRAPEZIUM:
|
||||
model = &trapez;
|
||||
break;
|
||||
case RAMP_S:
|
||||
model = &s_shaped;
|
||||
break;
|
||||
default:
|
||||
return FALSE;
|
||||
}
|
||||
if(!model->init_limits) return NULL;
|
||||
moveparam_t *max = &l->max, *min = &l->min;
|
||||
if(min->speed < 0.) min->speed = -min->speed;
|
||||
if(max->speed < 0.) max->speed = -max->speed;
|
||||
if(min->accel < 0.) min->accel = -min->accel;
|
||||
if(max->accel < 0.) max->accel = -max->accel;
|
||||
chkminmax(&min->coord, &max->coord);
|
||||
chkminmax(&min->speed, &max->speed);
|
||||
chkminmax(&min->accel, &max->accel);
|
||||
if(!model->init_limits(l)) return NULL;
|
||||
return model;
|
||||
}
|
||||
|
||||
int move_to(moveparam_t *target, double t){
|
||||
if(!target || !model) return FALSE;
|
||||
DBG("MOVE to %g at speed %g", target->coord, target->speed);
|
||||
// only positive velocity
|
||||
if(target->speed < 0.) target->speed = -target->speed;
|
||||
// don't mind about acceleration - user cannot set it now
|
||||
return model->calculate(target, t);
|
||||
}
|
||||
|
||||
int init_coordtol(double tolerance){
|
||||
if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
|
||||
coord_tolerance = tolerance;
|
||||
return TRUE;
|
||||
}
|
||||
int init_timetick(double tick){
|
||||
if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
|
||||
time_tick = tick;
|
||||
return TRUE;
|
||||
}
|
||||
70
PID_test.deprecated/moving.h
Normal file
70
PID_test.deprecated/moving.h
Normal file
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// tolerance, time ticks
|
||||
#define COORD_TOLERANCE_DEFAULT (0.01)
|
||||
#define COORD_TOLERANCE_MIN (0.0001)
|
||||
#define COORD_TOLERANCE_MAX (10.)
|
||||
#define TIME_TICK_DEFAULT (0.0001)
|
||||
#define TIME_TICK_MIN (1e-9)
|
||||
#define TIME_TICK_MAX (10.)
|
||||
|
||||
typedef enum{
|
||||
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
|
||||
RAMP_TRAPEZIUM, // trapezium ramp
|
||||
RAMP_S, // s-shaped ramp
|
||||
RAMP_AMOUNT
|
||||
} ramptype_t;
|
||||
|
||||
typedef enum{
|
||||
ST_STOP, // stopped
|
||||
ST_MOVE, // moving
|
||||
ST_AMOUNT
|
||||
} movestate_t;
|
||||
|
||||
typedef struct{ // all values could be both as positive and negative
|
||||
double coord;
|
||||
double speed;
|
||||
double accel;
|
||||
} moveparam_t;
|
||||
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
double jerk;
|
||||
} limits_t;
|
||||
|
||||
typedef struct{
|
||||
int (*init_limits)(limits_t *lim); // init values of limits, jerk
|
||||
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
|
||||
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
|
||||
void (*stop)(double t); // stop by ramp
|
||||
void (*emergency_stop)(double t); // stop with highest acceleration
|
||||
double (*stoppedtime)(); // time when moving will ends
|
||||
} movemodel_t;
|
||||
|
||||
extern double coord_tolerance;
|
||||
|
||||
double nanot();
|
||||
movemodel_t *init_moving(ramptype_t type, limits_t *l);
|
||||
int init_coordtol(double tolerance);
|
||||
int init_timetick(double tick);
|
||||
int move_to(moveparam_t *target, double t);
|
||||
1
PID_test.deprecated/moving_model.cflags
Normal file
1
PID_test.deprecated/moving_model.cflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c17
|
||||
4
PID_test.deprecated/moving_model.config
Normal file
4
PID_test.deprecated/moving_model.config
Normal file
@@ -0,0 +1,4 @@
|
||||
// Add predefined macros for your project here. For example:
|
||||
// #define THE_ANSWER 42
|
||||
#define _XOPEN_SOURCE 666
|
||||
#define EBUG
|
||||
1
PID_test.deprecated/moving_model.creator
Normal file
1
PID_test.deprecated/moving_model.creator
Normal file
@@ -0,0 +1 @@
|
||||
[General]
|
||||
221
PID_test.deprecated/moving_model.creator.user
Normal file
221
PID_test.deprecated/moving_model.creator.user
Normal file
@@ -0,0 +1,221 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">1</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/C-files/mountcontrol.git/moving_model</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
184
PID_test.deprecated/moving_model.creator.user.7bd84e3
Normal file
184
PID_test.deprecated/moving_model.creator.user.7bd84e3
Normal file
@@ -0,0 +1,184 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">8</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/moving_model</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
1
PID_test.deprecated/moving_model.cxxflags
Normal file
1
PID_test.deprecated/moving_model.cxxflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c++17
|
||||
10
PID_test.deprecated/moving_model.files
Normal file
10
PID_test.deprecated/moving_model.files
Normal file
@@ -0,0 +1,10 @@
|
||||
Dramp.c
|
||||
Dramp.h
|
||||
Sramp.c
|
||||
Sramp.h
|
||||
Tramp.c
|
||||
Tramp.h
|
||||
main.c
|
||||
moving.c
|
||||
moving.h
|
||||
moving_private.h
|
||||
0
PID_test.deprecated/moving_model.includes
Normal file
0
PID_test.deprecated/moving_model.includes
Normal file
26
PID_test.deprecated/moving_private.h
Normal file
26
PID_test.deprecated/moving_private.h
Normal file
@@ -0,0 +1,26 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
extern double coord_tolerance;
|
||||
extern double time_tick;
|
||||
|
||||
|
||||
4
PID_test.deprecated/plot
Executable file
4
PID_test.deprecated/plot
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
pause mouse
|
||||
8
PID_test.deprecated/plot.cont
Executable file
8
PID_test.deprecated/plot.cont
Executable file
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
#set term pdf
|
||||
#set output "output.pdf"
|
||||
while(1){
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
pause 1
|
||||
}
|
||||
6
PID_test.deprecated/plot_jpg
Executable file
6
PID_test.deprecated/plot_jpg
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set terminal jpeg size 1000,500
|
||||
set output "all.jpg"
|
||||
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
5
PID_test.deprecated/plot_pdf
Executable file
5
PID_test.deprecated/plot_pdf
Executable file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set term pdf
|
||||
set output "output.pdf"
|
||||
plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader
|
||||
4
PID_test.deprecated/plotacc
Executable file
4
PID_test.deprecated/plotacc
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot 'coords' using 1:5 with lines title columnheader
|
||||
pause mouse
|
||||
4
PID_test.deprecated/ploterr
Executable file
4
PID_test.deprecated/ploterr
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
pause mouse
|
||||
6
PID_test.deprecated/ploterr.cont
Executable file
6
PID_test.deprecated/ploterr.cont
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
while(1){
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
pause 1
|
||||
}
|
||||
5
PID_test.deprecated/ploterr_jpg
Executable file
5
PID_test.deprecated/ploterr_jpg
Executable file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set term jpeg size 1000,500
|
||||
set output "error.jpg"
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
57
PID_test/Makefile
Normal file
57
PID_test/Makefile
Normal file
@@ -0,0 +1,57 @@
|
||||
# run `make DEF=...` to add extra defines
|
||||
PROGRAM := moving
|
||||
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
|
||||
LDFLAGS += -lusefull_macros -lm
|
||||
SRCS := $(wildcard *.c)
|
||||
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
|
||||
OBJDIR := mk
|
||||
CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
|
||||
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
TARGFILE := $(OBJDIR)/TARGET
|
||||
CC = gcc
|
||||
#TARGET := RELEASE
|
||||
|
||||
ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
|
||||
TARGET := $(file < $(TARGFILE))
|
||||
else
|
||||
TARGET := RELEASE
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET), DEBUG)
|
||||
.DEFAULT_GOAL := debug
|
||||
endif
|
||||
|
||||
release: $(PROGRAM)
|
||||
|
||||
debug: CFLAGS += -DEBUG -Werror
|
||||
debug: TARGET := DEBUG
|
||||
debug: $(PROGRAM)
|
||||
|
||||
$(TARGFILE): $(OBJDIR)
|
||||
@echo -e "\t\tTARGET: $(TARGET)"
|
||||
@echo "$(TARGET)" > $(TARGFILE)
|
||||
|
||||
$(PROGRAM) : $(TARGFILE) $(OBJS)
|
||||
@echo -e "\t\tLD $(PROGRAM)"
|
||||
$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
|
||||
|
||||
$(OBJDIR):
|
||||
@mkdir $(OBJDIR)
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include $(DEPS)
|
||||
endif
|
||||
|
||||
$(OBJDIR)/%.o: %.c
|
||||
@echo -e "\t\tCC $<"
|
||||
$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@
|
||||
|
||||
clean:
|
||||
@echo -e "\t\tCLEAN"
|
||||
@rm -rf $(OBJDIR) 2>/dev/null || true
|
||||
|
||||
xclean: clean
|
||||
@rm -f $(PROGRAM)
|
||||
|
||||
.PHONY: clean xclean
|
||||
64
PID_test/PID.c
Normal file
64
PID_test/PID.c
Normal file
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "PID.h"
|
||||
|
||||
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){
|
||||
if(!gain || Iarrsz < 3) return NULL;
|
||||
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
|
||||
pid->gain = *gain;
|
||||
pid->pidIarrSize = Iarrsz;
|
||||
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
|
||||
return pid;
|
||||
}
|
||||
|
||||
void pid_clear(PIDController_t *pid){
|
||||
if(!pid) return;
|
||||
DBG("CLEAR PID PARAMETERS");
|
||||
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||
pid->integral = 0.;
|
||||
pid->prev_error = 0.;
|
||||
pid->curIidx = 0;
|
||||
}
|
||||
|
||||
void pid_delete(PIDController_t **pid){
|
||||
if(!pid || !*pid) return;
|
||||
if((*pid)->pidIarray) free((*pid)->pidIarray);
|
||||
free(*pid);
|
||||
*pid = NULL;
|
||||
}
|
||||
|
||||
double pid_calculate(PIDController_t *pid, double error, double dt){
|
||||
// calculate flowing integral
|
||||
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
|
||||
DBG("oldi/new: %g, %g", oldi, newi);
|
||||
pid->pidIarray[pid->curIidx++] = newi;
|
||||
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
|
||||
pid->integral += newi - oldi;
|
||||
double derivative = (error - pid->prev_error) / dt;
|
||||
pid->prev_error = error;
|
||||
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
|
||||
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
|
||||
return sum;
|
||||
}
|
||||
39
PID_test/PID.h
Normal file
39
PID_test/PID.h
Normal file
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
typedef struct{
|
||||
double P, I, D;
|
||||
} PIDpar_t;
|
||||
|
||||
typedef struct {
|
||||
PIDpar_t gain; // PID gains
|
||||
double prev_error; // Previous error
|
||||
double integral; // Integral term
|
||||
double *pidIarray; // array for Integral
|
||||
size_t pidIarrSize; // it's size
|
||||
size_t curIidx; // and index of current element
|
||||
} PIDController_t;
|
||||
|
||||
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz);
|
||||
void pid_clear(PIDController_t *pid);
|
||||
void pid_delete(PIDController_t **pid);
|
||||
double pid_calculate(PIDController_t *pid, double error, double dt);
|
||||
223
PID_test/Tramp.c
Normal file
223
PID_test/Tramp.c
Normal file
@@ -0,0 +1,223 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// simplest trapezioidal ramp
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Tramp.h"
|
||||
|
||||
#undef DBG
|
||||
#define DBG(...)
|
||||
|
||||
static movestate_t state = ST_STOP;
|
||||
static moveparam_t Min, Max; // `Min` acceleration not used!
|
||||
|
||||
typedef enum{
|
||||
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
|
||||
STAGE_MAXSPEED, // go with target speed
|
||||
STAGE_DECEL, // go from target speed to zero
|
||||
STAGE_STOPPED, // stop
|
||||
STAGE_AMOUNT
|
||||
} movingstage_t;
|
||||
|
||||
static movingstage_t movingstage = STAGE_STOPPED;
|
||||
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
|
||||
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
|
||||
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||
|
||||
static int initlims(limits_t *lim){
|
||||
if(!lim) return FALSE;
|
||||
Min = lim->min;
|
||||
Max = lim->max;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void emstop(double _U_ t){
|
||||
curparams.accel = 0.;
|
||||
curparams.speed = 0.;
|
||||
bzero(Times, sizeof(Times));
|
||||
bzero(Params, sizeof(Params));
|
||||
state = ST_STOP;
|
||||
movingstage = STAGE_STOPPED;
|
||||
}
|
||||
|
||||
static void stop(double t){
|
||||
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
|
||||
movingstage = STAGE_DECEL;
|
||||
state = ST_MOVE;
|
||||
Times[STAGE_DECEL] = t;
|
||||
Params[STAGE_DECEL].speed = curparams.speed;
|
||||
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
|
||||
else Params[STAGE_DECEL].accel = Max.accel;
|
||||
Params[STAGE_DECEL].coord = curparams.coord;
|
||||
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
|
||||
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||
double dt = Times[STAGE_STOPPED] - t;
|
||||
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
|
||||
Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief calc - moving calculation
|
||||
* @param x - using max speed (>0!!!) and coordinate
|
||||
* @param t - current time value
|
||||
* @return FALSE if can't move with given parameters
|
||||
*/
|
||||
static int calc(moveparam_t *x, double t){
|
||||
if(!x) return FALSE;
|
||||
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
||||
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
|
||||
double Dx = fabs(x->coord - curparams.coord); // full distance
|
||||
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
|
||||
// we have two variants: with or without stage with constant speed
|
||||
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
|
||||
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
|
||||
DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
|
||||
double setspeed = x->speed; // new max speed (we can change it if need)
|
||||
double dt01, dx01; // we'll fill them depending on starting conditions
|
||||
Times[0] = t;
|
||||
Params[0].speed = curparams.speed;
|
||||
Params[0].coord = curparams.coord;
|
||||
|
||||
double curspeed = fabs(curparams.speed);
|
||||
double dt0s = curspeed / Max.accel; // time of stopping phase
|
||||
double dx0s = curspeed * dt0s / 2.; // distance
|
||||
DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
|
||||
if(dx0s > Dx){
|
||||
WARNX("distance too short");
|
||||
return FALSE;
|
||||
}
|
||||
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
|
||||
DBG("Distance good to just stop");
|
||||
stop(t);
|
||||
return TRUE;
|
||||
}
|
||||
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
|
||||
// after stop we will have full profile
|
||||
double dxs3 = Dx - dx0s;
|
||||
double newspeed = sqrt(Max.accel * dxs3);
|
||||
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
|
||||
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
|
||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||
Params[0].accel = sign * Max.accel;
|
||||
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
|
||||
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
|
||||
DBG("dx01=%g, dt01=%g", dx01, dt01);
|
||||
}else{ // increase or decrease speed without stopping phase
|
||||
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
|
||||
double a = sign * Max.accel;
|
||||
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
|
||||
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
|
||||
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
|
||||
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
|
||||
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
|
||||
if(dx01 + dx23 > Dx){ // calculate max speed
|
||||
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
|
||||
if(setspeed < curspeed){
|
||||
setspeed = curparams.speed;
|
||||
dt01 = 0.; dx01 = 0.;
|
||||
Params[0].accel = 0.;
|
||||
}else{
|
||||
Params[0].accel = a;
|
||||
dt01 = fabs(setspeed - curspeed) / Max.accel;
|
||||
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
|
||||
}
|
||||
}else Params[0].accel = a;
|
||||
}
|
||||
if(setspeed < Min.speed){
|
||||
WARNX("New speed should be too small");
|
||||
return FALSE;
|
||||
}
|
||||
moveparam_t *p = &Params[STAGE_MAXSPEED];
|
||||
p->accel = 0.; p->speed = sign * setspeed;
|
||||
p->coord = curparams.coord + dx01 * sign;
|
||||
Times[STAGE_MAXSPEED] = Times[0] + dt01;
|
||||
dt23 = setspeed / Max.accel;
|
||||
dx23 = setspeed * dt23 / 2.;
|
||||
// calculate dx12 and dt12
|
||||
double dx12 = Dx - dx01 - dx23;
|
||||
if(dx12 < -coord_tolerance){
|
||||
WARNX("Oops, WTF dx12=%g?", dx12);
|
||||
return FALSE;
|
||||
}
|
||||
double dt12 = dx12 / setspeed;
|
||||
p = &Params[STAGE_DECEL];
|
||||
p->accel = -sign * Max.accel;
|
||||
p->speed = sign * setspeed;
|
||||
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
|
||||
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
|
||||
p = &Params[STAGE_STOPPED];
|
||||
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
|
||||
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
|
||||
for(int i = 0; i < 4; ++i)
|
||||
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
|
||||
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
|
||||
state = ST_MOVE;
|
||||
movingstage = STAGE_ACCEL;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static movestate_t proc(moveparam_t *next, double t){
|
||||
if(state == ST_STOP) goto ret;
|
||||
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||
if(Times[s] <= t){ // check time for current stage
|
||||
movingstage = s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(movingstage == STAGE_STOPPED){
|
||||
curparams.coord = Params[STAGE_STOPPED].coord;
|
||||
emstop(t);
|
||||
goto ret;
|
||||
}
|
||||
// calculate current parameters
|
||||
double dt = t - Times[movingstage];
|
||||
double a = Params[movingstage].accel;
|
||||
double v0 = Params[movingstage].speed;
|
||||
double x0 = Params[movingstage].coord;
|
||||
curparams.accel = a;
|
||||
curparams.speed = v0 + a * dt;
|
||||
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||
ret:
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static movestate_t getst(moveparam_t *cur){
|
||||
if(cur) *cur = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static double gettstop(){
|
||||
return Times[STAGE_STOPPED];
|
||||
}
|
||||
|
||||
movemodel_t trapez = {
|
||||
.init_limits = initlims,
|
||||
.stop = stop,
|
||||
.emergency_stop = emstop,
|
||||
.get_state = getst,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stoppedtime = gettstop,
|
||||
};
|
||||
23
PID_test/Tramp.h
Normal file
23
PID_test/Tramp.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t trapez;
|
||||
222
PID_test/main.c
Normal file
222
PID_test/main.c
Normal file
@@ -0,0 +1,222 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "moving.h"
|
||||
#include "PID.h"
|
||||
|
||||
// errors for states: slewing/pointing/guiding
|
||||
// 10-degrees zone - Coordinate-driven PID
|
||||
#define MAX_POINTING_ERR (36000.)
|
||||
// 1-arcminute zone - Velocity-dtiven PID
|
||||
#define MAX_GUIDING_ERR (60.)
|
||||
// timeout to "forget" old data from I sum array; seconds
|
||||
#define PID_I_PERIOD (3.)
|
||||
|
||||
// PID for coordinate-driven and velocity-driven parts
|
||||
static PIDController_t *pidC = NULL, *pidV = NULL;
|
||||
static movemodel_t *model = NULL;
|
||||
static FILE *coordslog = NULL;
|
||||
|
||||
typedef enum{
|
||||
Slewing,
|
||||
Pointing,
|
||||
Guiding
|
||||
} state_t;
|
||||
|
||||
static state_t state = Slewing;
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
char *ramptype;
|
||||
char *xlog;
|
||||
double dTmon;
|
||||
double dTcorr;
|
||||
double Tend;
|
||||
double minerr;
|
||||
double startcoord;
|
||||
double error;
|
||||
PIDpar_t gainC, gainV;
|
||||
} pars;
|
||||
|
||||
static pars G = {
|
||||
.dTmon = 0.01,
|
||||
.dTcorr = 0.05,
|
||||
.Tend = 100.,
|
||||
.minerr = 0.1,
|
||||
.gainC.P = 0.1,
|
||||
.gainV.P = 0.1,
|
||||
.startcoord = 100.,
|
||||
};
|
||||
|
||||
static limits_t limits = {
|
||||
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
||||
.max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
|
||||
};
|
||||
|
||||
|
||||
static sl_option_t opts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
|
||||
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
|
||||
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
||||
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
|
||||
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
|
||||
{"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"},
|
||||
{"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"},
|
||||
{"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"},
|
||||
{"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"},
|
||||
{"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"},
|
||||
{"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"},
|
||||
{"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"},
|
||||
{"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"},
|
||||
// TODO: add parameters for limits setting
|
||||
end_option
|
||||
};
|
||||
|
||||
// calculate coordinate target for given time (starting from zero)
|
||||
static double target_coord(double t){
|
||||
if(t > 20. && t < 30.) return 0.;
|
||||
//double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5);
|
||||
double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5);
|
||||
return pos;
|
||||
}
|
||||
|
||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||
double error = targcoord - p->coord, fe = fabs(error);
|
||||
PIDController_t *pid = NULL;
|
||||
switch(state){
|
||||
case Slewing:
|
||||
if(fe < MAX_POINTING_ERR){
|
||||
pid_clear(pidC);
|
||||
state = Pointing;
|
||||
green("--> Pointing\n");
|
||||
pid = pidC;
|
||||
}else{
|
||||
red("Slewing...\n");
|
||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||
}
|
||||
break;
|
||||
case Pointing:
|
||||
if(fe < MAX_GUIDING_ERR){
|
||||
pid_clear(pidV);
|
||||
state = Guiding;
|
||||
green("--> Guiding\n");
|
||||
pid = pidV;
|
||||
}else if(fe > MAX_POINTING_ERR){
|
||||
red("--> Slewing\n");
|
||||
state = Slewing;
|
||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||
} else pid = pidC;
|
||||
break;
|
||||
case Guiding:
|
||||
pid= pidV;
|
||||
if(fe > MAX_GUIDING_ERR){
|
||||
red("--> Pointing\n");
|
||||
state = Pointing;
|
||||
pid_clear(pidC);
|
||||
pid = pidC;
|
||||
}else if(fe < G.minerr){
|
||||
green("At target\n");
|
||||
}else printf("Current error: %g\n", fe);
|
||||
break;
|
||||
}
|
||||
if(!pid){
|
||||
WARNX("where is PID?"); return p->speed;
|
||||
}
|
||||
double tagspeed = pid_calculate(pid, error, dt);
|
||||
if(state == Guiding) return p->speed + tagspeed;
|
||||
return tagspeed;
|
||||
}
|
||||
// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
|
||||
// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
|
||||
|
||||
static void start_model(double Tend){
|
||||
double T = 0., Tcorr = 0.;
|
||||
moveparam_t target;
|
||||
uint64_t N = 0;
|
||||
double errmax = 0., errsum = 0., errsum2 = 0.;
|
||||
while(T <= Tend){
|
||||
moveparam_t p;
|
||||
movestate_t st = model->get_state(&p);
|
||||
if(st == ST_MOVE) st = model->proc_move(&p, T);
|
||||
double nextcoord = target_coord(T);
|
||||
double error = nextcoord - p.coord;
|
||||
if(state == Guiding){
|
||||
double ae = fabs(error);
|
||||
if(ae > errmax) errmax = ae;
|
||||
errsum += error; errsum2 += error * error;
|
||||
++N;
|
||||
}
|
||||
if(T - Tcorr >= G.dTcorr){ // check correction
|
||||
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
|
||||
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
|
||||
target.speed = fabs(speed);
|
||||
double res_speed = limits.max.speed / 2.;
|
||||
if(target.speed > limits.max.speed){
|
||||
target.speed = limits.max.speed;
|
||||
res_speed = limits.max.speed / 4.;
|
||||
}else if(target.speed < limits.min.speed){
|
||||
target.speed = limits.min.speed;
|
||||
res_speed = limits.min.speed * 4.;
|
||||
}
|
||||
if(!move_to(&target, T)){
|
||||
target.speed = res_speed;
|
||||
if(!move_to(&target, T))
|
||||
WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed);
|
||||
}
|
||||
DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error);
|
||||
Tcorr = T;
|
||||
}
|
||||
// make log
|
||||
fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
|
||||
T, nextcoord, p.coord, p.speed, p.accel, error);
|
||||
T += G.dTmon;
|
||||
}
|
||||
printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n");
|
||||
double mean = errsum / (double)N;
|
||||
printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean));
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, opts);
|
||||
if(G.help) sl_showhelp(-1, opts);
|
||||
if(G.xlog){
|
||||
coordslog = fopen(G.xlog, "w");
|
||||
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||
} else coordslog = stdout;
|
||||
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
||||
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
||||
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
||||
pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
|
||||
pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
|
||||
if(!pidC || !pidV) ERRX("Can't init PID regulators");
|
||||
model = init_moving(&limits);
|
||||
if(!model) ERRX("Can't init moving model: check parameters");
|
||||
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
||||
start_model(G.Tend);
|
||||
pid_delete(&pidC);
|
||||
pid_delete(&pidV);
|
||||
fclose(coordslog);
|
||||
return 0;
|
||||
}
|
||||
89
PID_test/moving.c
Normal file
89
PID_test/moving.c
Normal file
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
#include <pthread.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "moving.h"
|
||||
#include "moving_private.h"
|
||||
#include "Tramp.h"
|
||||
|
||||
static movemodel_t *model = &trapez;
|
||||
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
double time_tick = TIME_TICK_DEFAULT;
|
||||
|
||||
// difference of time from first call, using nanoseconds
|
||||
double nanot(){
|
||||
static struct timespec *start = NULL;
|
||||
struct timespec now;
|
||||
if(!start){
|
||||
start = MALLOC(struct timespec, 1);
|
||||
if(!start) return -1.;
|
||||
if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
|
||||
}
|
||||
if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
|
||||
//DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
|
||||
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
||||
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
||||
return sd + nd;
|
||||
}
|
||||
|
||||
static void chkminmax(double *min, double *max){
|
||||
if(*min <= *max) return;
|
||||
double t = *min;
|
||||
*min = *max;
|
||||
*max = t;
|
||||
}
|
||||
|
||||
movemodel_t *init_moving(limits_t *l){
|
||||
if(!l) return FALSE;
|
||||
if(!model->init_limits) return NULL;
|
||||
moveparam_t *max = &l->max, *min = &l->min;
|
||||
if(min->speed < 0.) min->speed = -min->speed;
|
||||
if(max->speed < 0.) max->speed = -max->speed;
|
||||
if(min->accel < 0.) min->accel = -min->accel;
|
||||
if(max->accel < 0.) max->accel = -max->accel;
|
||||
chkminmax(&min->coord, &max->coord);
|
||||
chkminmax(&min->speed, &max->speed);
|
||||
chkminmax(&min->accel, &max->accel);
|
||||
if(!model->init_limits(l)) return NULL;
|
||||
return model;
|
||||
}
|
||||
|
||||
int move_to(moveparam_t *target, double t){
|
||||
if(!target || !model) return FALSE;
|
||||
DBG("MOVE to %g at speed %g", target->coord, target->speed);
|
||||
// only positive velocity
|
||||
if(target->speed < 0.) target->speed = -target->speed;
|
||||
// don't mind about acceleration - user cannot set it now
|
||||
return model->calculate(target, t);
|
||||
}
|
||||
|
||||
int init_coordtol(double tolerance){
|
||||
if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
|
||||
coord_tolerance = tolerance;
|
||||
return TRUE;
|
||||
}
|
||||
int init_timetick(double tick){
|
||||
if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
|
||||
time_tick = tick;
|
||||
return TRUE;
|
||||
}
|
||||
62
PID_test/moving.h
Normal file
62
PID_test/moving.h
Normal file
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// tolerance, time ticks
|
||||
#define COORD_TOLERANCE_DEFAULT (0.01)
|
||||
#define COORD_TOLERANCE_MIN (0.0001)
|
||||
#define COORD_TOLERANCE_MAX (10.)
|
||||
#define TIME_TICK_DEFAULT (0.0001)
|
||||
#define TIME_TICK_MIN (1e-9)
|
||||
#define TIME_TICK_MAX (10.)
|
||||
|
||||
typedef enum{
|
||||
ST_STOP, // stopped
|
||||
ST_MOVE, // moving
|
||||
ST_AMOUNT
|
||||
} movestate_t;
|
||||
|
||||
typedef struct{ // all values could be both as positive and negative
|
||||
double coord;
|
||||
double speed;
|
||||
double accel;
|
||||
} moveparam_t;
|
||||
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
} limits_t;
|
||||
|
||||
typedef struct{
|
||||
int (*init_limits)(limits_t *lim); // init values of limits, jerk
|
||||
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
|
||||
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
|
||||
void (*stop)(double t); // stop by ramp
|
||||
void (*emergency_stop)(double t); // stop with highest acceleration
|
||||
double (*stoppedtime)(); // time when moving will ends
|
||||
} movemodel_t;
|
||||
|
||||
extern double coord_tolerance;
|
||||
|
||||
double nanot();
|
||||
movemodel_t *init_moving(limits_t *l);
|
||||
int init_coordtol(double tolerance);
|
||||
int init_timetick(double tick);
|
||||
int move_to(moveparam_t *target, double t);
|
||||
1
PID_test/moving_model.cflags
Normal file
1
PID_test/moving_model.cflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c17
|
||||
4
PID_test/moving_model.config
Normal file
4
PID_test/moving_model.config
Normal file
@@ -0,0 +1,4 @@
|
||||
// Add predefined macros for your project here. For example:
|
||||
// #define THE_ANSWER 42
|
||||
#define _XOPEN_SOURCE 666
|
||||
#define EBUG
|
||||
1
PID_test/moving_model.creator
Normal file
1
PID_test/moving_model.creator
Normal file
@@ -0,0 +1 @@
|
||||
[General]
|
||||
221
PID_test/moving_model.creator.user
Normal file
221
PID_test/moving_model.creator.user
Normal file
@@ -0,0 +1,221 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">1</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/C-files/mountcontrol.git/moving_model</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
184
PID_test/moving_model.creator.user.7bd84e3
Normal file
184
PID_test/moving_model.creator.user.7bd84e3
Normal file
@@ -0,0 +1,184 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">8</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/moving_model</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
1
PID_test/moving_model.cxxflags
Normal file
1
PID_test/moving_model.cxxflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c++17
|
||||
12
PID_test/moving_model.files
Normal file
12
PID_test/moving_model.files
Normal file
@@ -0,0 +1,12 @@
|
||||
Dramp.c
|
||||
Dramp.h
|
||||
PID.c
|
||||
PID.h
|
||||
Sramp.c
|
||||
Sramp.h
|
||||
Tramp.c
|
||||
Tramp.h
|
||||
main.c
|
||||
moving.c
|
||||
moving.h
|
||||
moving_private.h
|
||||
0
PID_test/moving_model.includes
Normal file
0
PID_test/moving_model.includes
Normal file
26
PID_test/moving_private.h
Normal file
26
PID_test/moving_private.h
Normal file
@@ -0,0 +1,26 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
extern double coord_tolerance;
|
||||
extern double time_tick;
|
||||
|
||||
|
||||
4
PID_test/plot
Executable file
4
PID_test/plot
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
pause mouse
|
||||
8
PID_test/plot.cont
Executable file
8
PID_test/plot.cont
Executable file
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
#set term pdf
|
||||
#set output "output.pdf"
|
||||
while(1){
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
pause 1
|
||||
}
|
||||
6
PID_test/plot_jpg
Executable file
6
PID_test/plot_jpg
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set terminal jpeg size 1000,500
|
||||
set output "all.jpg"
|
||||
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
5
PID_test/plot_pdf
Executable file
5
PID_test/plot_pdf
Executable file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set term pdf
|
||||
set output "output.pdf"
|
||||
plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader
|
||||
4
PID_test/plotacc
Executable file
4
PID_test/plotacc
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot 'coords' using 1:5 with lines title columnheader
|
||||
pause mouse
|
||||
4
PID_test/ploterr
Executable file
4
PID_test/ploterr
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
pause mouse
|
||||
6
PID_test/ploterr.cont
Executable file
6
PID_test/ploterr.cont
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
while(1){
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
pause 1
|
||||
}
|
||||
5
PID_test/ploterr_jpg
Executable file
5
PID_test/ploterr_jpg
Executable file
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set term jpeg size 1000,500
|
||||
set output "error.jpg"
|
||||
plot 'coords' using 1:6 with lines title columnheader
|
||||
3
TODO
Normal file
3
TODO
Normal file
@@ -0,0 +1,3 @@
|
||||
fix encoders opening for several tries
|
||||
encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request)
|
||||
Read HW config even in model mode
|
||||
14
examples/CMakeLists.txt
Normal file
14
examples/CMakeLists.txt
Normal file
@@ -0,0 +1,14 @@
|
||||
project(examples)
|
||||
|
||||
# common includes & library
|
||||
include_directories(../)
|
||||
link_libraries(sidservo usefull_macros -lm)
|
||||
|
||||
# exe list
|
||||
add_executable(goto goto.c dump.c conf.c)
|
||||
add_executable(dump dumpmoving.c dump.c conf.c)
|
||||
add_executable(dump_s dumpmoving_scmd.c dump.c conf.c)
|
||||
add_executable(dumpswing dumpswing.c dump.c conf.c)
|
||||
add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c)
|
||||
add_executable(SSIIconf SSIIconf.c conf.c)
|
||||
add_executable(slewNtrack dumpmoving_dragNtrack.c dump.c conf.c)
|
||||
28
examples/Readme.md
Normal file
28
examples/Readme.md
Normal file
@@ -0,0 +1,28 @@
|
||||
Some examples of usage of libsidservo
|
||||
=====================================
|
||||
|
||||
## Auxiliary files
|
||||
|
||||
*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes
|
||||
|
||||
*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero.
|
||||
|
||||
*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero.
|
||||
|
||||
*simpleconv.h*
|
||||
|
||||
|
||||
## Examples
|
||||
|
||||
*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y".
|
||||
|
||||
*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command.
|
||||
|
||||
*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis.
|
||||
|
||||
*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands).
|
||||
|
||||
*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands.
|
||||
|
||||
*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller
|
||||
|
||||
163
examples/SSIIconf.c
Normal file
163
examples/SSIIconf.c
Normal file
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int helpargs;
|
||||
int writeconf;
|
||||
char *conffile;
|
||||
char *hwconffile;
|
||||
} parameters;
|
||||
|
||||
static hardware_configuration_t HW = {0};
|
||||
|
||||
static parameters G = {
|
||||
.conffile = "servo.conf",
|
||||
};
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"help-opts", NO_ARGS, NULL, 'H', arg_int, APTR(&G.helpargs), "configuration help"},
|
||||
{"serconf", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "serial configuration file name"},
|
||||
{"hwconf", NEED_ARG, NULL, 'i', arg_string, APTR(&G.hwconffile),"SSII configuration file name"},
|
||||
{"writeconf", NO_ARGS, NULL, 0, arg_int, APTR(&G.writeconf), "write configuration (BE CAREFUL!)"},
|
||||
end_option
|
||||
};
|
||||
|
||||
static sl_option_t confopts[] = {
|
||||
{"Xaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Xconf.accel), "X Default Acceleration, rad/s^2"},
|
||||
{"Yaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Yconf.accel), "Y Default Acceleration, rad/s^2"},
|
||||
end_option
|
||||
};
|
||||
|
||||
static void dumpaxis(char axis, axis_config_t *c){
|
||||
#define STRUCTPAR(p) (c)->p
|
||||
#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0)
|
||||
#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
||||
DUMPD(accel);
|
||||
DUMPD(backlash);
|
||||
DUMPD(errlimit);
|
||||
DUMP(propgain);
|
||||
DUMP(intgain);
|
||||
DUMP(derivgain);
|
||||
DUMP(outplimit);
|
||||
DUMP(currlimit);
|
||||
DUMP(intlimit);
|
||||
DUMP(motor_stepsperrev);
|
||||
DUMP(axis_stepsperrev);
|
||||
#undef DUMP
|
||||
#undef DUMPD
|
||||
}
|
||||
|
||||
static void dumpxbits(xbits_t *c){
|
||||
#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
|
||||
DUMPBIT(motrev);
|
||||
DUMPBIT(motpolarity);
|
||||
DUMPBIT(encrev);
|
||||
DUMPBIT(dragtrack);
|
||||
DUMPBIT(trackplat);
|
||||
DUMPBIT(handpaden);
|
||||
DUMPBIT(newpad);
|
||||
DUMPBIT(guidemode);
|
||||
#undef DUMPBIT
|
||||
}
|
||||
|
||||
static void dumpybits(ybits_t *c){
|
||||
#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
|
||||
DUMPBIT(motrev);
|
||||
DUMPBIT(motpolarity);
|
||||
DUMPBIT(encrev);
|
||||
DUMPBIT(slewtrack);
|
||||
DUMPBIT(digin_sens);
|
||||
printf("Ydigin=%d\n", c->digin);
|
||||
#undef DUMPBIT
|
||||
}
|
||||
|
||||
static void dumpHWconf(){
|
||||
#undef STRUCTPAR
|
||||
#define STRUCTPAR(p) (HW).p
|
||||
#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
|
||||
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
||||
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
|
||||
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
|
||||
green("X axis configuration:\n");
|
||||
dumpaxis('X', &HW.Xconf);
|
||||
green("X bits:\n");
|
||||
dumpxbits(&HW.xbits);
|
||||
green("Y axis configuration:\n");
|
||||
dumpaxis('Y', &HW.Yconf);
|
||||
green("Y bits:\n");
|
||||
dumpybits(&HW.ybits);
|
||||
green("Other:\n");
|
||||
printf("address=%d\n", HW.address);
|
||||
DUMP(eqrate);
|
||||
DUMP(eqadj);
|
||||
DUMP(trackgoal);
|
||||
DUMPD(latitude);
|
||||
DUMPU32(Xsetpr);
|
||||
DUMPU32(Ysetpr);
|
||||
DUMPU32(Xmetpr);
|
||||
DUMPU32(Ymetpr);
|
||||
DUMPD(Xslewrate);
|
||||
DUMPD(Yslewrate);
|
||||
DUMPD(Xpanrate);
|
||||
DUMPD(Ypanrate);
|
||||
DUMPD(Xguiderate);
|
||||
DUMPD(Yguiderate);
|
||||
DUMPU32(baudrate);
|
||||
DUMPD(locsdeg);
|
||||
DUMPD(locsspeed);
|
||||
DUMPD(backlspd);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help)
|
||||
sl_showhelp(-1, cmdlnopts);
|
||||
if(G.helpargs)
|
||||
sl_showhelp(-1, confopts);
|
||||
conf_t *sconf = readServoConf(G.conffile);
|
||||
if(!sconf){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
|
||||
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
|
||||
/*
|
||||
char *c = sl_print_opts(confopts, TRUE);
|
||||
green("Got configuration:\n");
|
||||
printf("%s\n", c);
|
||||
FREE(c);
|
||||
*/
|
||||
dumpHWconf();
|
||||
/*
|
||||
if(G.hwconffile && G.writeconf){
|
||||
;
|
||||
}*/
|
||||
Mount.quit();
|
||||
return 0;
|
||||
}
|
||||
128
examples/conf.c
Normal file
128
examples/conf.c
Normal file
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
|
||||
static conf_t Config = {
|
||||
.MountDevPath = "/dev/ttyUSB0",
|
||||
.MountDevSpeed = 19200,
|
||||
.EncoderXDevPath = "/dev/encoder_X0",
|
||||
.EncoderYDevPath = "/dev/encoder_Y0",
|
||||
.EncoderDevSpeed = 153000,
|
||||
.MountReqInterval = 0.1,
|
||||
.EncoderReqInterval = 0.001,
|
||||
.SepEncoder = 2,
|
||||
.EncoderSpeedInterval = 0.05,
|
||||
.EncodersDisagreement = 1e-5, // 2''
|
||||
.PIDMaxDt = 1.,
|
||||
.PIDRefreshDt = 0.1,
|
||||
.PIDCycleDt = 5.,
|
||||
.XPIDC.P = 0.5,
|
||||
.XPIDC.I = 0.1,
|
||||
.XPIDC.D = 0.2,
|
||||
.XPIDV.P = 0.09,
|
||||
.XPIDV.I = 0.0,
|
||||
.XPIDV.D = 0.05,
|
||||
.YPIDC.P = 0.5,
|
||||
.YPIDC.I = 0.1,
|
||||
.YPIDC.D = 0.2,
|
||||
.YPIDV.P = 0.09,
|
||||
.YPIDV.I = 0.0,
|
||||
.YPIDV.D = 0.05,
|
||||
.MaxPointingErr = 0.13962634,
|
||||
.MaxFinePointingErr = 0.026179939,
|
||||
.MaxGuidingErr = 4.8481368e-7,
|
||||
};
|
||||
|
||||
static sl_option_t opts[] = {
|
||||
{"MountDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.MountDevPath), "path to mount device"},
|
||||
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
|
||||
{"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"},
|
||||
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
|
||||
{"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
|
||||
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
|
||||
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
|
||||
{"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
|
||||
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
|
||||
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
|
||||
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
|
||||
{"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
|
||||
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
|
||||
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
|
||||
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
|
||||
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
|
||||
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
|
||||
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
|
||||
{"YPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.P), "P of Y PID (coordinate driven)"},
|
||||
{"YPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.I), "I of Y PID (coordinate driven)"},
|
||||
{"YPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.D), "D of Y PID (coordinate driven)"},
|
||||
{"XPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.P), "P of X PID (velocity driven)"},
|
||||
{"XPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.I), "I of X PID (velocity driven)"},
|
||||
{"XPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.D), "D of X PID (velocity driven)"},
|
||||
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
|
||||
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
|
||||
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
|
||||
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
|
||||
{"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
|
||||
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
|
||||
{"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"},
|
||||
{"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"},
|
||||
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
|
||||
end_option
|
||||
};
|
||||
|
||||
conf_t *readServoConf(const char *filename){
|
||||
if(!filename) filename = DEFCONFFILE;
|
||||
int n = sl_conf_readopts(filename, opts);
|
||||
if(n < 0){
|
||||
WARNX("Can't read file %s", filename);
|
||||
return NULL;
|
||||
}
|
||||
if(n == 0){
|
||||
WARNX("Got ZERO parameters from %s", filename);
|
||||
return NULL;
|
||||
}
|
||||
return &Config;
|
||||
}
|
||||
|
||||
void dumpConf(){
|
||||
char *c = sl_print_opts(opts, TRUE);
|
||||
printf("Current configuration:\n%s\n", c);
|
||||
FREE(c);
|
||||
}
|
||||
|
||||
void confHelp(){
|
||||
sl_conf_showhelp(-1, opts);
|
||||
}
|
||||
|
||||
const char* errcodes[MCC_E_AMOUNT] = {
|
||||
[MCC_E_OK] = "OK",
|
||||
[MCC_E_FATAL] = "Fatal error",
|
||||
[MCC_E_BADFORMAT] = "Wrong data format",
|
||||
[MCC_E_ENCODERDEV] = "Encoder error",
|
||||
[MCC_E_MOUNTDEV] = "Mount error",
|
||||
[MCC_E_FAILED] = "Failed to run"
|
||||
};
|
||||
// return string with error code
|
||||
const char *EcodeStr(mcc_errcodes_t e){
|
||||
if(e >= MCC_E_AMOUNT) return "Wrong error code";
|
||||
return errcodes[e];
|
||||
}
|
||||
28
examples/conf.h
Normal file
28
examples/conf.h
Normal file
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
#define DEFCONFFILE "servo.conf"
|
||||
|
||||
void confHelp();
|
||||
conf_t *readServoConf(const char *filename);
|
||||
void dumpConf();
|
||||
const char *EcodeStr(mcc_errcodes_t e);
|
||||
197
examples/dump.c
Normal file
197
examples/dump.c
Normal file
@@ -0,0 +1,197 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// logging of mount position
|
||||
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "dump.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
// starting dump time (to conform different logs)
|
||||
static struct timespec dumpT0 = {0};
|
||||
|
||||
#if 0
|
||||
// amount of elements used for encoders' data filtering
|
||||
#define NFILT (10)
|
||||
|
||||
static double filterK[NFILT];
|
||||
static double lastvals[2][NFILT] = {0};
|
||||
static int need2buildFilter = 1;
|
||||
|
||||
static void buildFilter(){
|
||||
filterK[NFILT-1] = 1.;
|
||||
double sum = 1.;
|
||||
for(int i = NFILT-2; i > -1; --i){
|
||||
filterK[i] = (filterK[i+1] + 1.) * 1.1;
|
||||
sum += filterK[i];
|
||||
}
|
||||
for(int i = 0; i < NFILT; ++i) filterK[i] /= sum;
|
||||
}
|
||||
|
||||
static double filter(double val, int idx){
|
||||
if(need2buildFilter){
|
||||
buildFilter();
|
||||
need2buildFilter = 0;
|
||||
}
|
||||
static int ctr[2] = {0};
|
||||
for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1];
|
||||
lastvals[idx][0] = val;
|
||||
double r = 0.;
|
||||
if(ctr[idx] < NFILT){
|
||||
++ctr[idx];
|
||||
return val;
|
||||
}
|
||||
for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i];
|
||||
return r;
|
||||
}
|
||||
#endif
|
||||
|
||||
// return starting time of dump
|
||||
void dumpt0(struct timespec *t){
|
||||
if(t) *t = dumpT0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief logmnt - log mount data into file
|
||||
* @param fcoords - file to dump
|
||||
* @param m - mount data
|
||||
*/
|
||||
void logmnt(FILE *fcoords, mountdata_t *m){
|
||||
if(!fcoords) return;
|
||||
//DBG("LOG %s", m ? "data" : "header");
|
||||
if(!m){ // write header
|
||||
fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
|
||||
return;
|
||||
}else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
|
||||
// write data
|
||||
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
|
||||
Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
|
||||
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
|
||||
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
|
||||
m->millis);
|
||||
fflush(fcoords);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief dumpmoving - dump conf while moving
|
||||
* @param fcoords - dump file
|
||||
* @param t - max waiting time
|
||||
* @param N - number of cycles to wait while motors aren't moving
|
||||
*/
|
||||
void dumpmoving(FILE *fcoords, double t, int N){
|
||||
if(!fcoords) return;
|
||||
mountdata_t mdata;
|
||||
DBG("Start dump");
|
||||
int ntries = 0;
|
||||
for(; ntries < 10; ++ntries){
|
||||
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
||||
}
|
||||
if(ntries == 10){
|
||||
WARNX("Can't get mount data");
|
||||
LOGWARN("Can't get mount data");
|
||||
}
|
||||
uint32_t mdmillis = mdata.millis;
|
||||
struct timespec encXt = mdata.encXposition.t;
|
||||
int ctr = -1;
|
||||
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
|
||||
double t0 = Mount.timeFromStart();
|
||||
while(Mount.timeFromStart() - t0 < t && ctr < N){
|
||||
usleep(1000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
//double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
|
||||
struct timespec msrt = mdata.encXposition.t;
|
||||
if(msrt.tv_nsec == encXt.tv_nsec) continue;
|
||||
encXt = msrt;
|
||||
if(fcoords) logmnt(fcoords, &mdata);
|
||||
if(mdata.millis == mdmillis) continue;
|
||||
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
|
||||
mdmillis = mdata.millis;
|
||||
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
||||
xlast = mdata.motXposition.val;
|
||||
ylast = mdata.motYposition.val;
|
||||
ctr = 0;
|
||||
}else ++ctr;
|
||||
}
|
||||
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief waitmoving - wait until moving by both axiss stops at least for N cycles
|
||||
* @param N - amount of stopped cycles
|
||||
*/
|
||||
void waitmoving(int N){
|
||||
mountdata_t mdata;
|
||||
int ctr = -1;
|
||||
uint32_t millis = 0;
|
||||
//double xlast = 0., ylast = 0.;
|
||||
DBG("Wait moving for %d stopped times", N);
|
||||
while(ctr < N){
|
||||
usleep(10000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
if(mdata.millis == millis) continue;
|
||||
millis = mdata.millis;
|
||||
if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
|
||||
else ++ctr;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief getPos - get current position
|
||||
* @param mot (o) - motor position (or NULL)
|
||||
* @param Y (o) - encoder position (or NULL)
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int getPos(coordval_pair_t *mot, coordval_pair_t *enc){
|
||||
mountdata_t mdata = {0};
|
||||
int errcnt = 0;
|
||||
do{
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
|
||||
else{
|
||||
errcnt = 0;
|
||||
if(mdata.millis) break;
|
||||
}
|
||||
}while(errcnt < 10);
|
||||
if(errcnt >= 10){
|
||||
WARNX("Can't read mount status");
|
||||
return FALSE;
|
||||
}
|
||||
if(mot){
|
||||
mot->X = mdata.motXposition;
|
||||
mot->Y = mdata.motYposition;
|
||||
}
|
||||
if(enc){
|
||||
enc->X = mdata.encXposition;
|
||||
enc->Y = mdata.encYposition;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// check current position and go to 0 if non-zero
|
||||
void chk0(int ncycles){
|
||||
coordval_pair_t M;
|
||||
if(!getPos(&M, NULL)) signals(2);
|
||||
if(M.X.val || M.Y.val){
|
||||
WARNX("Mount position isn't @ zero; moving");
|
||||
coordpair_t zero = {0., 0.};
|
||||
Mount.moveTo(&zero);
|
||||
waitmoving(ncycles);
|
||||
green("Now mount @ zero\n");
|
||||
}
|
||||
}
|
||||
30
examples/dump.h
Normal file
30
examples/dump.h
Normal file
@@ -0,0 +1,30 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
void logmnt(FILE *fcoords, mountdata_t *m);
|
||||
void dumpmoving(FILE *fcoords, double t, int N);
|
||||
void waitmoving(int N);
|
||||
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
|
||||
void chk0(int ncycles);
|
||||
void dumpt0(struct timespec *t);
|
||||
105
examples/dumpmoving.c
Normal file
105
examples/dumpmoving.c
Normal file
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// dump telescope moving using simplest goto command
|
||||
|
||||
#include <math.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int verbose;
|
||||
int Ncycles;
|
||||
char *logfile;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
} parameters;
|
||||
|
||||
static parameters G = {
|
||||
.Ncycles = 40,
|
||||
};
|
||||
static FILE *fcoords = NULL;
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"},
|
||||
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
|
||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||
end_option
|
||||
};
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
LOGERR("Exit with status %d", sig);
|
||||
Mount.quit();
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help) sl_showhelp(-1, cmdlnopts);
|
||||
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
|
||||
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
|
||||
if(G.logfile) OPENLOG(G.logfile, lvl, 1);
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
confHelp();
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
logmnt(fcoords, NULL);
|
||||
time_t curtime = time(NULL);
|
||||
LOGMSG("Started @ %s", ctime(&curtime));
|
||||
LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed);
|
||||
LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed);
|
||||
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices");
|
||||
coordval_pair_t M;
|
||||
if(!getPos(&M, NULL)) ERRX("Can't get current position");
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val};
|
||||
if(MCC_E_OK != Mount.moveTo(&tag))
|
||||
ERRX("Can't move to 45, 45");
|
||||
dumpmoving(fcoords, 30., G.Ncycles);
|
||||
tag.X = M.X.val; tag.Y = M.Y.val;
|
||||
Mount.moveTo(&tag);
|
||||
dumpmoving(fcoords, 30., G.Ncycles);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
228
examples/dumpmoving_dragNtrack.c
Normal file
228
examples/dumpmoving_dragNtrack.c
Normal file
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// move telescope to target using short command and force it to track mode
|
||||
// also do some corrections while moving
|
||||
|
||||
#include <math.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
// Original XXI=6827
|
||||
// XXD=136546
|
||||
// XXB=4915666
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int Ncycles;
|
||||
double reqint;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
} parameters;
|
||||
|
||||
static parameters G = {
|
||||
.Ncycles = 40,
|
||||
.reqint = -1.,
|
||||
};
|
||||
|
||||
static FILE *fcoords = NULL;
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
|
||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||
end_option
|
||||
};
|
||||
|
||||
static mcc_errcodes_t return2zero();
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
return2zero();
|
||||
sleep(5);
|
||||
Mount.quit();
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
// dump thread
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// return TRUE if motor position is reached +- 0.01 degrees
|
||||
#define XYcount (DEG2RAD(0.3))
|
||||
// tag in degrees!
|
||||
static int Wait(double tag, int isX){
|
||||
mountdata_t mdata;
|
||||
red("Wait for %g degrees\n", tag);
|
||||
tag = DEG2RAD(tag);
|
||||
int errcnt = 0;
|
||||
uint32_t millis = 0;
|
||||
double curpos = 0.;
|
||||
double t0 = sl_dtime();
|
||||
do{
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
|
||||
else{
|
||||
errcnt = 0;
|
||||
if(mdata.millis == millis) continue;
|
||||
millis = mdata.millis;
|
||||
if(isX) curpos = mdata.motXposition.val;
|
||||
else curpos = mdata.motYposition.val;
|
||||
}
|
||||
double t = sl_dtime();
|
||||
if(t - t0 > 1.){
|
||||
t0 = t;
|
||||
printf("\t\tCurrent MOT X/Y: %g / %g deg\n", RAD2DEG(mdata.motXposition.val),
|
||||
RAD2DEG(mdata.motYposition.val));
|
||||
}
|
||||
}while(fabs(curpos - tag) > XYcount && errcnt < 10);
|
||||
if(errcnt >= 10){
|
||||
WARNX("Too much errors");
|
||||
return FALSE;
|
||||
}
|
||||
green("%s reached position %g degrees\n", (isX) ? "X" : "Y", RAD2DEG(tag));
|
||||
fflush(stdout);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// previous GOTO coords/speeds for `mkcorr`
|
||||
static coordpair_t lastTag = {0}, lastSpeed = {0};
|
||||
|
||||
// slew to given position and start tracking
|
||||
// pos/speed in deg and deg/s
|
||||
static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){
|
||||
short_command_t cmd = {0};
|
||||
DBG("Try to move to (%g, %g) with speed (%g, %g)",
|
||||
target->X, target->Y, speed->X, speed->Y);
|
||||
cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
|
||||
cmd.Xspeed = DEG2RAD(speed->X);
|
||||
cmd.Yspeed = DEG2RAD(speed->Y);
|
||||
lastTag = *target;
|
||||
lastSpeed = *speed;
|
||||
/*cmd.xychange = 1;
|
||||
cmd.XBits = 108;
|
||||
cmd.YBits = 28;*/
|
||||
return Mount.shortCmd(&cmd);
|
||||
}
|
||||
|
||||
static mcc_errcodes_t return2zero(){
|
||||
short_command_t cmd = {0};
|
||||
DBG("Try to move to zero");
|
||||
cmd.Xmot = 0.; cmd.Ymot = 0.;
|
||||
coordpair_t maxspd;
|
||||
if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
|
||||
cmd.Xspeed = maxspd.X;
|
||||
cmd.Yspeed = maxspd.Y;
|
||||
/*cmd.xychange = 1;
|
||||
cmd.XBits = 100;
|
||||
cmd.YBits = 20;*/
|
||||
return Mount.shortCmd(&cmd);
|
||||
}
|
||||
|
||||
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
|
||||
long_command_t cmd = {0};
|
||||
cmd.Xspeed = DEG2RAD(lastSpeed.X);
|
||||
cmd.Yspeed = DEG2RAD(lastSpeed.Y);
|
||||
cmd.Xmot = DEG2RAD(lastTag.X);
|
||||
cmd.Ymot = DEG2RAD(lastTag.Y);
|
||||
cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
|
||||
cmd.Xatime = time->X; cmd.Yatime = time->Y;
|
||||
return Mount.longCmd(&cmd);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help) sl_showhelp(-1, cmdlnopts);
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
|
||||
if(MCC_E_OK != Mount.init(Config)){
|
||||
WARNX("Can't init devices");
|
||||
return 1;
|
||||
}
|
||||
//if(!getPos(&M, NULL)) ERRX("Can't get current position");
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
// move to X=40 degr with different speeds
|
||||
pthread_t dthr;
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
// move to 10/10
|
||||
coordpair_t coords, speeds, adders, tadd;
|
||||
coords = (coordpair_t){.X = 10., .Y = 20.};
|
||||
speeds = (coordpair_t){.X = 1., .Y = 2.};
|
||||
adders = (coordpair_t){.X = 0.01, .Y = 0.01};
|
||||
tadd = (coordpair_t){.X = 1., .Y = 2.};
|
||||
green("Goto\n");
|
||||
if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
|
||||
DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
|
||||
green("Waiting X==4\n");
|
||||
Wait(4., 1);
|
||||
// now we are at point by Y but still moving by X; make small correction by X/Y into '+'
|
||||
green("Mkcorr 1\n");
|
||||
if(MCC_E_OK != mkcorr(&adders, &tadd)) ERRX("Can't make corr");
|
||||
green("Waiting X==6\n");
|
||||
Wait(6., 1);
|
||||
green("Goto more\n");
|
||||
coords = (coordpair_t){.X = 20., .Y = 30.};
|
||||
if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
|
||||
DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
|
||||
green("Waiting Y==14\n");
|
||||
Wait(14., 0);
|
||||
// now we are @ point, make the same small correction again
|
||||
green("Mkcorr 2\n");
|
||||
if(MCC_E_OK != mkcorr(&coords, &speeds)) ERRX("Can't make corr");
|
||||
// wait for 5 seconds
|
||||
green("Wait for 5 seconds\n");
|
||||
sleep(5);
|
||||
// return to zero and wait
|
||||
green("Return 2 zero and wait\n");
|
||||
if(MCC_E_OK != return2zero()) ERRX("Can't return");
|
||||
Wait(0., 0);
|
||||
Wait(0., 1);
|
||||
// wait moving ends
|
||||
pthread_join(dthr, NULL);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
177
examples/dumpmoving_scmd.c
Normal file
177
examples/dumpmoving_scmd.c
Normal file
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// dump telescope moving using short binary commands
|
||||
|
||||
#include <math.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int Ncycles;
|
||||
int relative;
|
||||
double reqint;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
char *axis;
|
||||
} parameters;
|
||||
|
||||
static parameters G = {
|
||||
.Ncycles = 40,
|
||||
.reqint = -1.,
|
||||
.axis = "X",
|
||||
};
|
||||
|
||||
static FILE *fcoords = NULL;
|
||||
|
||||
static coordval_pair_t M;
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
|
||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"},
|
||||
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
|
||||
end_option
|
||||
};
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
Mount.quit();
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
// dump thread
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// return TRUE if motor position is reached +- 0.01 degrees
|
||||
#define XYcount (DEG2RAD(0.01))
|
||||
static int Wait(double tag){
|
||||
mountdata_t mdata;
|
||||
red("Wait for %g degrees\n", RAD2DEG(tag));
|
||||
int errcnt = 0;
|
||||
double sign = 0.;
|
||||
uint32_t millis = 0;
|
||||
double curpos = 0.;
|
||||
do{
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
|
||||
else{
|
||||
errcnt = 0;
|
||||
if(mdata.millis == millis) continue;
|
||||
millis = mdata.millis;
|
||||
if(*G.axis == 'X') curpos = mdata.motXposition.val;
|
||||
else curpos = mdata.motYposition.val;
|
||||
if(sign == 0.) sign = (curpos > tag) ? 1. : -1.;
|
||||
//printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos),
|
||||
// RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.);
|
||||
}
|
||||
}while(sign*(curpos - tag) > XYcount && errcnt < 10);
|
||||
if(errcnt >= 10){
|
||||
WARNX("Too much errors");
|
||||
return FALSE;
|
||||
}
|
||||
green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
|
||||
fflush(stdout);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// move X/Y to 40 degr with given speed until given coord
|
||||
static void move(double target, double limit, double speed){
|
||||
green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
|
||||
short_command_t cmd = {0};
|
||||
if(*G.axis == 'X' || *G.axis == 'B'){
|
||||
cmd.Xmot = DEG2RAD(target) + M.X.val;
|
||||
cmd.Xspeed = DEG2RAD(speed);
|
||||
limit = DEG2RAD(limit) + M.X.val;
|
||||
}
|
||||
if(*G.axis == 'Y' || *G.axis == 'B'){
|
||||
cmd.Ymot = DEG2RAD(target) + M.Y.val;
|
||||
cmd.Yspeed = DEG2RAD(speed);
|
||||
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val;
|
||||
}
|
||||
if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
|
||||
if(!Wait(limit)) signals(9);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help) sl_showhelp(-1, cmdlnopts);
|
||||
if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){
|
||||
WARNX("\"Axis\" should be X, Y or B");
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
|
||||
if(MCC_E_OK != Mount.init(Config)){
|
||||
WARNX("Can't init devices");
|
||||
return 1;
|
||||
}
|
||||
if(!getPos(&M, NULL)) ERRX("Can't get current position");
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
// move to X=40 degr with different speeds
|
||||
pthread_t dthr;
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
// goto 30' with 5'/s
|
||||
move(10., 30./60., 5./60.);
|
||||
// goto 1' with 10'/s
|
||||
move(10., 1., 10./60.);
|
||||
// goto 3degr with 15'/s
|
||||
move(10., 3., 15./60.);
|
||||
// and go back with 7deg/s
|
||||
move(0., 0., 7.);
|
||||
// be sure to move @ starting position
|
||||
coordpair_t tag = {.X = M.X.val, .Y = M.Y.val};
|
||||
Mount.moveTo(&tag);
|
||||
// wait moving ends
|
||||
pthread_join(dthr, NULL);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
190
examples/dumpswing.c
Normal file
190
examples/dumpswing.c
Normal file
@@ -0,0 +1,190 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min)
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int Ncycles;
|
||||
int Nswings;
|
||||
double period;
|
||||
double amplitude;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
char *axis;
|
||||
} parameters;
|
||||
|
||||
static parameters G = {
|
||||
.Ncycles = 20,
|
||||
.axis = "X",
|
||||
.Nswings = 10,
|
||||
.period = 1.,
|
||||
.amplitude = 5.,
|
||||
};
|
||||
static FILE *fcoords = NULL;
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 20)"},
|
||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
|
||||
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
|
||||
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
|
||||
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
|
||||
end_option
|
||||
};
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
Mount.quit();
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
// dump thread
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// wait until mount is stopped within 5 cycles or until time reached t
|
||||
void waithalf(double t){
|
||||
mountdata_t mdata;
|
||||
int ctr = -1;
|
||||
uint32_t millis = 0;
|
||||
double xlast = 0., ylast = 0.;
|
||||
while(ctr < 5){
|
||||
if(Mount.timeFromStart() >= t) return;
|
||||
usleep(1000);
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
|
||||
if(mdata.millis == millis) continue;
|
||||
millis = mdata.millis;
|
||||
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
|
||||
//DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
||||
xlast = mdata.motXposition.val;
|
||||
ylast = mdata.motYposition.val;
|
||||
ctr = 0;
|
||||
}else{
|
||||
//DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
|
||||
++ctr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help) sl_showhelp(-1, cmdlnopts);
|
||||
if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){
|
||||
WARNX("\"Axis\" should be X or Y");
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w"))){
|
||||
WARNX("Can't open %s", G.coordsoutput);
|
||||
return 1;
|
||||
}
|
||||
}else fcoords = stdout;
|
||||
if(G.Ncycles < 2){
|
||||
WARNX("Ncycles should be >2");
|
||||
return 1;
|
||||
}
|
||||
double absamp = fabs(G.amplitude);
|
||||
if(absamp < 0.01 || absamp > 45.){
|
||||
WARNX("Amplitude should be from 0.01 to 45 degrees");
|
||||
return 1;
|
||||
}
|
||||
if(G.period < 0.1 || G.period > 900.){
|
||||
WARNX("Period should be from 0.1 to 900s");
|
||||
return 1;
|
||||
}
|
||||
if(G.Nswings < 1){
|
||||
WARNX("Nswings should be more than 0");
|
||||
return 1;
|
||||
}
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
mcc_errcodes_t e = Mount.init(Config);
|
||||
if(e != MCC_E_OK){
|
||||
WARNX("Can't init devices");
|
||||
return 1;
|
||||
}
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
pthread_t dthr;
|
||||
chk0(G.Ncycles);
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
G.period /= 2.; // pause between commands
|
||||
double tagX, tagY;
|
||||
if(*G.axis == 'X'){
|
||||
tagX = DEG2RAD(G.amplitude); tagY = 0.;
|
||||
}else{
|
||||
tagX = 0.; tagY = DEG2RAD(G.amplitude);
|
||||
}
|
||||
double t = Mount.timeFromStart(), t0 = t;
|
||||
coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
|
||||
double divide = 2.;
|
||||
for(int i = 0; i < G.Nswings; ++i){
|
||||
Mount.moveTo(&tag);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
t += G.period / divide;
|
||||
divide = 1.;
|
||||
waithalf(t);
|
||||
DBG("Moved to +, t=%g", t-t0);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
Mount.moveTo(&rtag);
|
||||
t += G.period;
|
||||
waithalf(t);
|
||||
DBG("Moved to -, t=%g", t-t0);
|
||||
DBG("CMD: %g", Mount.timeFromStart()-t0);
|
||||
}
|
||||
green("Move to zero @ %g\n", Mount.timeFromStart());
|
||||
tag = (coordpair_t){0};
|
||||
// be sure to move @ 0,0
|
||||
if(MCC_E_OK != Mount.moveTo(&tag)){
|
||||
Mount.emergStop();
|
||||
Mount.moveTo(&tag);
|
||||
}
|
||||
// wait moving ends
|
||||
pthread_join(dthr, NULL);
|
||||
#undef SCMD
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
145
examples/goto.c
Normal file
145
examples/goto.c
Normal file
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// move telescope to given MOTOR position in degrees
|
||||
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int Ncycles;
|
||||
int wait;
|
||||
int relative;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
double X;
|
||||
double Y;
|
||||
} parameters;
|
||||
|
||||
static parameters G = {
|
||||
.Ncycles = 10,
|
||||
.X = NAN,
|
||||
.Y = NAN,
|
||||
};
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles of waiting in stopped state (default: 40)"},
|
||||
{"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"},
|
||||
{"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"},
|
||||
{"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
|
||||
{"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"},
|
||||
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||
end_option
|
||||
};
|
||||
|
||||
static FILE* fcoords = NULL;
|
||||
static pthread_t dthr;
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
DBG("Quit");
|
||||
Mount.quit();
|
||||
DBG("close");
|
||||
if(fcoords) fclose(fcoords);
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
// dump thread
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int main(int _U_ argc, char _U_ **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help)
|
||||
sl_showhelp(-1, cmdlnopts);
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
|
||||
coordval_pair_t M, E;
|
||||
if(!getPos(&M, &E)) ERRX("Can't get current position");
|
||||
printf("Current time: %.10f\n", Mount.timeFromStart());
|
||||
if(G.coordsoutput){
|
||||
if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
|
||||
G.wait = 1;
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
}
|
||||
M.X.val = RAD2DEG(M.X.val);
|
||||
M.Y.val = RAD2DEG(M.Y.val);
|
||||
printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val,
|
||||
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
|
||||
if(isnan(G.X) && isnan(G.Y)) goto out;
|
||||
coordpair_t tag;
|
||||
if(isnan(G.X)){
|
||||
if(G.relative) G.X = 0.;
|
||||
else G.X = M.X.val;
|
||||
}
|
||||
if(isnan(G.Y)){
|
||||
if(G.relative) G.Y = 0.;
|
||||
else G.Y = M.Y.val;
|
||||
}
|
||||
if(G.relative){
|
||||
G.X += M.X.val;
|
||||
G.Y += M.Y.val;
|
||||
}
|
||||
printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
|
||||
tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
|
||||
mcc_errcodes_t e = Mount.moveTo(&tag);
|
||||
if(MCC_E_OK != e){
|
||||
WARNX("Cant go to given coordinates: %s\n", EcodeStr(e));
|
||||
goto out;
|
||||
}
|
||||
if(G.wait){
|
||||
sleep(1);
|
||||
waitmoving(G.Ncycles);
|
||||
if(!getPos(&M, NULL)) WARNX("Can't get current position");
|
||||
else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
|
||||
}
|
||||
out:
|
||||
DBG("JOIN");
|
||||
if(G.coordsoutput) pthread_join(dthr, NULL);
|
||||
DBG("QUIT");
|
||||
if(G.wait){
|
||||
usleep(250000); // pause to refresh coordinates
|
||||
if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val),
|
||||
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
|
||||
Mount.quit();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
188
examples/scmd_traectory.c
Normal file
188
examples/scmd_traectory.c
Normal file
@@ -0,0 +1,188 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
#include "traectories.h"
|
||||
|
||||
// calculate some traectory and try to run over it
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int dumpconf;
|
||||
int Ncycles; // n cycles to wait stop
|
||||
double reqint; // requests interval (seconds)
|
||||
double Xmax; // maximal X to stop
|
||||
double Ymax; // maximal Y to stop
|
||||
double tmax; // maximal time of emulation
|
||||
double X0; // starting point of traectory (-30..30 degr)
|
||||
double Y0; // -//-
|
||||
char *coordsoutput; // dump file
|
||||
char *errlog; // log with position errors
|
||||
char *tfn; // traectory function name
|
||||
char *conffile;
|
||||
} parameters;
|
||||
|
||||
static conf_t *Config = NULL;
|
||||
static FILE *fcoords = NULL, *errlog = NULL;
|
||||
static pthread_t dthr;
|
||||
static parameters G = {
|
||||
.Ncycles = 40,
|
||||
.reqint = 0.1,
|
||||
.tfn = "sincos",
|
||||
.Xmax = 45.,
|
||||
.Ymax = 45.,
|
||||
.tmax = 300., // 5 minutes
|
||||
.X0 = 10.,
|
||||
.Y0 = 10.,
|
||||
};
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
|
||||
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
|
||||
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
|
||||
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
|
||||
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
|
||||
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y coordinate for traectory (default: 45 degrees)"},
|
||||
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
|
||||
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
|
||||
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
|
||||
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
|
||||
{"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"},
|
||||
end_option
|
||||
};
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
Mount.stop();
|
||||
sleep(1);
|
||||
Mount.quit();
|
||||
if(fcoords) fclose(fcoords);
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// calculate
|
||||
static void runtraectory(traectory_fn tfn){
|
||||
if(!tfn) return;
|
||||
coordval_pair_t telXY;
|
||||
coordval_pair_t target;
|
||||
coordpair_t traectXY;
|
||||
double tlast = 0., tstart = Mount.timeFromStart();
|
||||
long tlastXnsec = 0, tlastYnsec = 0;
|
||||
struct timespec tcur, t0 = {0};
|
||||
dumpt0(&t0);
|
||||
while(1){
|
||||
if(!telpos(&telXY)){
|
||||
WARNX("No next telescope position");
|
||||
return;
|
||||
}
|
||||
if(!Mount.currentT(&tcur)) continue;
|
||||
if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
|
||||
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
|
||||
tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
|
||||
double t = Mount.timeFromStart();
|
||||
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
|
||||
if(!traectory_point(&traectXY, t)) break;
|
||||
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
|
||||
target.X.t = target.Y.t = tcur;
|
||||
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
|
||||
else{
|
||||
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
||||
DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
|
||||
if(errlog)
|
||||
fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
|
||||
}
|
||||
if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
|
||||
while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
|
||||
tlast = t;
|
||||
}
|
||||
WARNX("No next traectory point or emulation ends");
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help) sl_showhelp(-1, cmdlnopts);
|
||||
if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees");
|
||||
if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees");
|
||||
// convert to radians
|
||||
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
|
||||
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
|
||||
ERRX("X0 and Y0 should be -30..30 degrees");
|
||||
if(G.errlog){
|
||||
if(!(errlog = fopen(G.errlog, "w")))
|
||||
ERRX("Can't open error log %s", G.errlog);
|
||||
else
|
||||
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
Config = readServoConf(G.conffile);
|
||||
if(!Config || G.dumpconf){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
Config->MountReqInterval = G.reqint;
|
||||
traectory_fn tfn = traectory_by_name(G.tfn);
|
||||
if(!tfn){
|
||||
WARNX("Bad traectory name %s, should be one of", G.tfn);
|
||||
print_tr_names();
|
||||
return 1;
|
||||
}
|
||||
coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
|
||||
if(!init_traectory(tfn, &c)){
|
||||
ERRX("Can't init traectory");
|
||||
return 1;
|
||||
}
|
||||
mcc_errcodes_t e = Mount.init(Config);
|
||||
if(e != MCC_E_OK){
|
||||
WARNX("Can't init devices");
|
||||
return 1;
|
||||
}
|
||||
signal(SIGTERM, signals); // kill (-15) - quit
|
||||
signal(SIGHUP, SIG_IGN); // hup - ignore
|
||||
signal(SIGINT, signals); // ctrl+C - quit
|
||||
signal(SIGQUIT, signals); // ctrl+\ - quit
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
chk0(G.Ncycles);
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
;
|
||||
runtraectory(tfn);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
25
examples/servo.conf
Normal file
25
examples/servo.conf
Normal file
@@ -0,0 +1,25 @@
|
||||
# Current configuration
|
||||
MountDevPath=/dev/ttyUSB0
|
||||
MountDevSpeed=19200
|
||||
EncoderDevPath=(null)
|
||||
EncoderDevSpeed=1000000
|
||||
MountReqInterval=0.1
|
||||
EncoderReqInterval=0.001
|
||||
SepEncoder=2
|
||||
EncoderXDevPath=/dev/encoder_X0
|
||||
EncoderYDevPath=/dev/encoder_Y0
|
||||
EncoderSpeedInterval=0.05
|
||||
RunModel=1
|
||||
XPIDCP=0.8
|
||||
XPIDCI=0.0
|
||||
XPIDCD=0.0
|
||||
YPIDCP=0.5
|
||||
YPIDCI=0.0
|
||||
YPIDCD=0.0
|
||||
XPIDVP=0.2
|
||||
XPIDVI=0.1
|
||||
XPIDVD=0.0
|
||||
YPIDVP=0.2
|
||||
YPIDVI=0.1
|
||||
YPIDVD=0.0
|
||||
|
||||
29
examples/simpleconv.h
Normal file
29
examples/simpleconv.h
Normal file
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// simple conversion macros
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#define DEG2RAD(d) ((d)/180.*M_PI)
|
||||
#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
|
||||
#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
|
||||
#define RAD2DEG(r) ((r)/M_PI*180.)
|
||||
#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
|
||||
#define RAD2AMIN(r) ((r)/M_PI*180.*60.)
|
||||
|
||||
136
examples/traectories.c
Normal file
136
examples/traectories.c
Normal file
@@ -0,0 +1,136 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// some simplest traectories
|
||||
// all traectories runs increasing X and Y from starting point
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "simpleconv.h"
|
||||
#include "traectories.h"
|
||||
|
||||
static traectory_fn cur_traectory = NULL;
|
||||
// starting point of traectory
|
||||
static coordpair_t XYstart = {0};
|
||||
static double tstart = 0.;
|
||||
|
||||
/**
|
||||
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
|
||||
* @param f - function calculating next point
|
||||
* @param XY0 - starting point
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int init_traectory(traectory_fn f, coordpair_t *XY0){
|
||||
if(!f || !XY0) return FALSE;
|
||||
cur_traectory = f;
|
||||
XYstart = *XY0;
|
||||
tstart = Mount.timeFromStart();
|
||||
mountdata_t mdata;
|
||||
int ntries = 0;
|
||||
for(; ntries < 10; ++ntries){
|
||||
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
||||
}
|
||||
if(ntries == 10) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief traectory_point - get traectory point for given time
|
||||
* @param nextpt (o) - next point coordinates
|
||||
* @param t - UNIX-time of event
|
||||
* @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
|
||||
*/
|
||||
int traectory_point(coordpair_t *nextpt, double t){
|
||||
if(t < 0. || !cur_traectory) return FALSE;
|
||||
coordpair_t pt;
|
||||
if(!cur_traectory(&pt, t)) return FALSE;
|
||||
if(nextpt) *nextpt = pt;
|
||||
if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// current telescope position according to starting motor coordinates
|
||||
// @return FALSE if failed to get current coordinates
|
||||
int telpos(coordval_pair_t *curpos){
|
||||
mountdata_t mdata;
|
||||
int ntries = 0;
|
||||
for(; ntries < 10; ++ntries){
|
||||
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
|
||||
}
|
||||
if(ntries == 10) return FALSE;
|
||||
coordval_pair_t pt;
|
||||
//DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
|
||||
pt.X.val = mdata.encXposition.val;
|
||||
pt.Y.val = mdata.encYposition.val;
|
||||
pt.X.t = mdata.encXposition.t;
|
||||
pt.Y.t = mdata.encYposition.t;
|
||||
if(curpos) *curpos = pt;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// X=X0+1'/s, Y=Y0+15''/s
|
||||
int Linear(coordpair_t *nextpt, double t){
|
||||
coordpair_t pt;
|
||||
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
|
||||
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
|
||||
if(nextpt) *nextpt = pt;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
|
||||
int SinCos(coordpair_t *nextpt, double t){
|
||||
coordpair_t pt;
|
||||
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
|
||||
pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI);
|
||||
if(nextpt) *nextpt = pt;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
typedef struct{
|
||||
traectory_fn f;
|
||||
const char *name;
|
||||
const char *help;
|
||||
} tr_names;
|
||||
|
||||
static tr_names names[] = {
|
||||
{Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
|
||||
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
|
||||
{NULL, NULL, NULL}
|
||||
};
|
||||
|
||||
traectory_fn traectory_by_name(const char *name){
|
||||
traectory_fn f = NULL;
|
||||
for(int i = 0; ; ++i){
|
||||
if(!names[i].f) break;
|
||||
if(strcmp(names[i].name, name) == 0){
|
||||
f = names[i].f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return f;
|
||||
}
|
||||
|
||||
// print all acceptable traectories names with help
|
||||
void print_tr_names(){
|
||||
for(int i = 0; ; ++i){
|
||||
if(!names[i].f) break;
|
||||
printf("%s: %s\n", names[i].name, names[i].help);
|
||||
}
|
||||
}
|
||||
32
examples/traectories.h
Normal file
32
examples/traectories.h
Normal file
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
// traectory
|
||||
typedef int (*traectory_fn)(coordpair_t *, double);
|
||||
|
||||
int init_traectory(traectory_fn f, coordpair_t *XY0);
|
||||
traectory_fn traectory_by_name(const char *name);
|
||||
void print_tr_names();
|
||||
int traectory_point(coordpair_t *nextpt, double t);
|
||||
int telpos(coordval_pair_t *curpos);
|
||||
int Linear(coordpair_t *nextpt, double t);
|
||||
int SinCos(coordpair_t *nextpt, double t);
|
||||
1
libsidservo.cflags
Normal file
1
libsidservo.cflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c17
|
||||
7
libsidservo.config
Normal file
7
libsidservo.config
Normal file
@@ -0,0 +1,7 @@
|
||||
// Add predefined macros for your project here. For example:
|
||||
// #define THE_ANSWER 42
|
||||
#define EBUG
|
||||
#define _POSIX_C_SOURCE 11111111
|
||||
#define PACKAGE_VERSION "0.0.1"
|
||||
#define _XOPEN_SOURCE 666
|
||||
#define _DEFAULT_SOURCE
|
||||
1
libsidservo.creator
Normal file
1
libsidservo.creator
Normal file
@@ -0,0 +1 @@
|
||||
[General]
|
||||
218
libsidservo.creator.user
Normal file
218
libsidservo.creator.user
Normal file
@@ -0,0 +1,218 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 18.0.0, 2026-03-11T12:36:26. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
165
libsidservo.creator.user.7bd84e3
Normal file
165
libsidservo.creator.user.7bd84e3
Normal file
@@ -0,0 +1,165 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 4.10.1, 2020-05-19T00:12:06. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="int">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuelist type="QVariantList" key="ClangCodeModel.CustomCommandLineKey"/>
|
||||
<value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/C-sources/erfa</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="GenericProjectManager.GenericMakeStep.Clean">false</value>
|
||||
<value type="QString" key="GenericProjectManager.GenericMakeStep.MakeArguments"></value>
|
||||
<value type="QString" key="GenericProjectManager.GenericMakeStep.MakeCommand"></value>
|
||||
<value type="bool" key="GenericProjectManager.GenericMakeStep.OverrideMakeflags">false</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="GenericProjectManager.GenericMakeStep.Clean">false</value>
|
||||
<value type="QString" key="GenericProjectManager.GenericMakeStep.MakeArguments"></value>
|
||||
<value type="QString" key="GenericProjectManager.GenericMakeStep.MakeCommand"></value>
|
||||
<value type="bool" key="GenericProjectManager.GenericMakeStep.OverrideMakeflags">false</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Конфигурация развёртывания</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Особая программа</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="RunConfiguration.Arguments"></value>
|
||||
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
<value type="QString" key="RunConfiguration.WorkingDirectory"></value>
|
||||
<value type="QString" key="RunConfiguration.WorkingDirectory.default"></value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="int">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
184
libsidservo.creator.user.cf63021
Normal file
184
libsidservo.creator.user.cf63021
Normal file
@@ -0,0 +1,184 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 15.0.0, 2025-01-27T14:56:12. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">4</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
1
libsidservo.cxxflags
Normal file
1
libsidservo.cxxflags
Normal file
@@ -0,0 +1 @@
|
||||
-std=c++17
|
||||
30
libsidservo.files
Normal file
30
libsidservo.files
Normal file
@@ -0,0 +1,30 @@
|
||||
CMakeLists.txt
|
||||
PID.c
|
||||
PID.h
|
||||
examples/SSIIconf.c
|
||||
examples/conf.c
|
||||
examples/conf.h
|
||||
examples/dump.c
|
||||
examples/dump.h
|
||||
examples/dumpmoving.c
|
||||
examples/dumpmoving_dragNtrack.c
|
||||
examples/dumpmoving_scmd.c
|
||||
examples/dumpswing.c
|
||||
examples/goto.c
|
||||
examples/scmd_traectory.c
|
||||
examples/simpleconv.h
|
||||
main.c
|
||||
sidservo.h
|
||||
serial.c
|
||||
examples/CMakeLists.txt
|
||||
examples/traectories.c
|
||||
examples/traectories.h
|
||||
main.h
|
||||
movingmodel.c
|
||||
movingmodel.h
|
||||
polltest/main.c
|
||||
ramp.c
|
||||
ramp.h
|
||||
serial.h
|
||||
ssii.c
|
||||
ssii.h
|
||||
2
libsidservo.includes
Normal file
2
libsidservo.includes
Normal file
@@ -0,0 +1,2 @@
|
||||
.
|
||||
..
|
||||
621
main.c
Normal file
621
main.c
Normal file
@@ -0,0 +1,621 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* main functions to fill struct `mount_t`
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <strings.h>
|
||||
#include <time.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "movingmodel.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
#include "PID.h"
|
||||
|
||||
// adder for monotonic time by realtime: inited any call of init()
|
||||
static struct timespec timeadder = {0}, // adder of CLOCK_REALTIME to CLOCK_MONOTONIC
|
||||
t0 = {0}, // curtime() for initstarttime() call
|
||||
starttime = {0}; // starting time by monotonic (for timefromstart())
|
||||
|
||||
conf_t Conf = {0};
|
||||
// parameters for model
|
||||
static movemodel_t *Xmodel, *Ymodel;
|
||||
// limits for model and/or real mount (in latter case data should be read from mount on init)
|
||||
// radians, rad/sec, rad/sec^2
|
||||
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||
limits_t
|
||||
Xlimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
|
||||
Ylimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
|
||||
;
|
||||
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
||||
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig);
|
||||
|
||||
/**
|
||||
* @brief curtime - monotonic time from first run
|
||||
* @param t - struct timespec by CLOCK_MONOTONIC but with setpoint by CLOCK_REALTIME on observations start
|
||||
* @return TRUE if all OK
|
||||
* FIXME: double -> struct timespec; on init: init t0 by CLOCK_REALTIME
|
||||
*/
|
||||
int curtime(struct timespec *t){
|
||||
struct timespec now;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &now)) return FALSE;
|
||||
now.tv_sec += timeadder.tv_sec;
|
||||
now.tv_nsec += timeadder.tv_nsec;
|
||||
if(now.tv_nsec > 999999999L){
|
||||
++now.tv_sec;
|
||||
now.tv_nsec -= 1000000000L;
|
||||
}
|
||||
if(t) *t = now;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// init starttime; @return TRUE if all OK
|
||||
static int initstarttime(){
|
||||
struct timespec start;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &starttime)) return FALSE;
|
||||
if(clock_gettime(CLOCK_REALTIME, &start)) return FALSE;
|
||||
timeadder.tv_sec = start.tv_sec - starttime.tv_sec;
|
||||
timeadder.tv_nsec = start.tv_nsec - starttime.tv_nsec;
|
||||
if(timeadder.tv_nsec < 0){
|
||||
--timeadder.tv_sec;
|
||||
timeadder.tv_nsec += 1000000000L;
|
||||
}
|
||||
curtime(&t0);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return difference (in seconds) between time1 and time0
|
||||
double timediff(const struct timespec *time1, const struct timespec *time0){
|
||||
if(!time1 || !time0) return -1.;
|
||||
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
|
||||
}
|
||||
// difference between given time and last initstarttime() call
|
||||
double timediff0(const struct timespec *time1){
|
||||
return timediff(time1, &t0);
|
||||
}
|
||||
// time from last initstarttime() call
|
||||
double timefromstart(){
|
||||
struct timespec now;
|
||||
if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
|
||||
return (now.tv_sec - starttime.tv_sec) + (now.tv_nsec - starttime.tv_nsec) / 1e9;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief quit - close all opened and return to default state
|
||||
* TODO: close serial devices even in "model" mode
|
||||
*/
|
||||
static void quit(){
|
||||
if(Conf.RunModel) return;
|
||||
for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
|
||||
DBG("Close all serial devices");
|
||||
closeSerial();
|
||||
DBG("Exit");
|
||||
}
|
||||
|
||||
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst){
|
||||
if(!c || !Xmodel || !Ymodel) return;
|
||||
double tnow = timefromstart();
|
||||
moveparam_t Xp, Yp;
|
||||
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
|
||||
//DBG("Xstate = %d", Xst);
|
||||
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
|
||||
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
|
||||
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
|
||||
c->X = Xp.coord;
|
||||
c->Y = Yp.coord;
|
||||
if(xst) *xst = Xst;
|
||||
if(yst) *yst = Yst;
|
||||
}
|
||||
|
||||
/**
|
||||
* less square calculations of speed
|
||||
*/
|
||||
less_square_t *LS_init(size_t Ndata){
|
||||
if(Ndata < 5){
|
||||
DBG("Ndata=%zd - TOO SMALL", Ndata);
|
||||
return NULL;
|
||||
}
|
||||
DBG("Init less squares: %zd", Ndata);
|
||||
less_square_t *l = calloc(1, sizeof(less_square_t));
|
||||
l->x = calloc(Ndata, sizeof(double));
|
||||
l->t2 = calloc(Ndata, sizeof(double));
|
||||
l->t = calloc(Ndata, sizeof(double));
|
||||
l->xt = calloc(Ndata, sizeof(double));
|
||||
l->arraysz = Ndata;
|
||||
return l;
|
||||
}
|
||||
void LS_delete(less_square_t **l){
|
||||
if(!l || !*l) return;
|
||||
free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
|
||||
free(*l);
|
||||
*l = NULL;
|
||||
}
|
||||
// add next data portion and calculate current slope
|
||||
double LS_calc_slope(less_square_t *l, double x, double t){
|
||||
if(!l) return 0.;
|
||||
size_t idx = l->idx;
|
||||
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
|
||||
/*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
|
||||
l->xsum, l->tsum, l->t2sum, l->xtsum);*/
|
||||
double t2 = t * t, xt = x * t;
|
||||
l->x[idx] = x; l->t2[idx] = t2;
|
||||
l->t[idx] = t; l->xt[idx] = xt;
|
||||
++idx;
|
||||
l->idx = (idx >= l->arraysz) ? 0 : idx;
|
||||
l->xsum += x - oldx;
|
||||
l->t2sum += t2 - oldt2;
|
||||
l->tsum += t - oldt;
|
||||
l->xtsum += xt - oldxt;
|
||||
double n = (double)l->arraysz;
|
||||
double denominator = n * l->t2sum - l->tsum * l->tsum;
|
||||
if(fabs(denominator) < 1e-7) return 0.;
|
||||
double numerator = n * l->xtsum - l->xsum * l->tsum;
|
||||
//DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
|
||||
// point: (sum_x - slope * sum_t) / n;
|
||||
return (numerator / denominator);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief init - open serial devices and do other job
|
||||
* @param c - initial configuration
|
||||
* @return error code
|
||||
*/
|
||||
static mcc_errcodes_t init(conf_t *c){
|
||||
FNAME();
|
||||
if(!c) return MCC_E_BADFORMAT;
|
||||
if(!initstarttime()) return MCC_E_FAILED;
|
||||
Conf = *c;
|
||||
mcc_errcodes_t ret = MCC_E_OK;
|
||||
Xmodel = model_init(&Xlimits);
|
||||
Ymodel = model_init(&Ylimits);
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
if(Conf.RunModel){
|
||||
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
|
||||
DBG("Define mount device path and speed");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}else if(!openMount()){
|
||||
DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
|
||||
ret = MCC_E_MOUNTDEV;
|
||||
}
|
||||
if(Conf.SepEncoder){
|
||||
if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
|
||||
DBG("Define encoder device path");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}else if(!openEncoder()){
|
||||
DBG("Can't open encoder device");
|
||||
ret = MCC_E_ENCODERDEV;
|
||||
}
|
||||
}
|
||||
// TODO: read hardware configuration on init
|
||||
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
|
||||
DBG("Wrong speed interval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
|
||||
if(ret != MCC_E_OK) return ret;
|
||||
// read HW config to update constants
|
||||
hardware_configuration_t HW;
|
||||
if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
|
||||
// make a pause for actual encoder's values
|
||||
double t0 = timefromstart();
|
||||
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
|
||||
mcc_errcodes_t e = updateMotorPos();
|
||||
// and refresh data after updating
|
||||
DBG("Wait for next mount reading");
|
||||
t0 = timefromstart();
|
||||
while(timefromstart() - t0 < Conf.MountReqInterval * 3.) usleep(1000);
|
||||
return e;
|
||||
}
|
||||
|
||||
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
|
||||
// TODO fix to real limits!!!
|
||||
static int chkX(double X){
|
||||
if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkY(double Y){
|
||||
if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkXs(double s){
|
||||
if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkYs(double s){
|
||||
if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// set SLEWING state if axis was stopped
|
||||
static void setslewingstate(){
|
||||
//FNAME();
|
||||
mountdata_t d;
|
||||
if(MCC_E_OK == getMD(&d)){
|
||||
axis_status_t newx = d.Xstate, newy = d.Ystate;
|
||||
//DBG("old state: %d/%d", d.Xstate, d.Ystate);
|
||||
if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
|
||||
if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
|
||||
if(newx != d.Xstate || newy != d.Ystate){
|
||||
DBG("Started moving -> slew");
|
||||
setStat(newx, newy);
|
||||
}
|
||||
}else DBG("CAN't GET MOUNT DATA!");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move2 - simple move to given point and stop
|
||||
* @param X - new X coordinate (radians: -pi..pi) or NULL
|
||||
* @param Y - new Y coordinate (radians: -pi..pi) or NULL
|
||||
* @return error code
|
||||
*/
|
||||
static mcc_errcodes_t move2(const coordpair_t *target){
|
||||
if(!target) return MCC_E_BADFORMAT;
|
||||
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||
short_command_t cmd = {0};
|
||||
DBG("x,y: %g, %g", target->X, target->Y);
|
||||
cmd.Xmot = target->X;
|
||||
cmd.Ymot = target->Y;
|
||||
cmd.Xspeed = Xlimits.max.speed;
|
||||
cmd.Yspeed = Ylimits.max.speed;
|
||||
/*mcc_errcodes_t r = shortcmd(&cmd);
|
||||
if(r != MCC_E_OK) return r;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;*/
|
||||
return shortcmd(&cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief setspeed - set maximal speed over axis by text command
|
||||
* @param X (i) - max speed or NULL
|
||||
* @param Y (i) - -//-
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
|
||||
if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel) return MCC_E_FAILED;
|
||||
int32_t spd = X_RS2MOTSPD(tagspeed->X);
|
||||
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
|
||||
spd = Y_RS2MOTSPD(tagspeed->Y);
|
||||
if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move2s - move to target with given max speed
|
||||
* @param target (i) - target or NULL
|
||||
* @param speed (i) - speed or NULL
|
||||
* @return
|
||||
*/
|
||||
static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){
|
||||
if(!target || !speed) return MCC_E_BADFORMAT;
|
||||
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
||||
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
|
||||
// updateMotorPos() here can make a problem; TODO: remove?
|
||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||
short_command_t cmd = {0};
|
||||
cmd.Xmot = target->X;
|
||||
cmd.Ymot = target->Y;
|
||||
cmd.Xspeed = speed->X;
|
||||
cmd.Yspeed = speed->Y;
|
||||
mcc_errcodes_t r = shortcmd(&cmd);
|
||||
if(r != MCC_E_OK) return r;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief emstop - emergency stop
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t emstop(){
|
||||
FNAME();
|
||||
if(Conf.RunModel){
|
||||
double curt = timefromstart();
|
||||
Xmodel->emergency_stop(Xmodel, curt);
|
||||
Ymodel->emergency_stop(Ymodel, curt);
|
||||
return MCC_E_OK;
|
||||
}
|
||||
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
// normal stop
|
||||
static mcc_errcodes_t stop(){
|
||||
FNAME();
|
||||
if(Conf.RunModel){
|
||||
double curt = timefromstart();
|
||||
Xmodel->stop(Xmodel, curt);
|
||||
Ymodel->stop(Ymodel,curt);
|
||||
return MCC_E_OK;
|
||||
}
|
||||
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief shortcmd - send and receive short binary command
|
||||
* @param cmd (io) - command
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t shortcmd(short_command_t *cmd){
|
||||
if(!cmd) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel){
|
||||
double curt = timefromstart();
|
||||
moveparam_t param = {0};
|
||||
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
|
||||
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
}
|
||||
SSscmd s = {0};
|
||||
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
|
||||
s.Xmot = X_RAD2MOT(cmd->Xmot);
|
||||
s.Ymot = Y_RAD2MOT(cmd->Ymot);
|
||||
s.Xspeed = X_RS2MOTSPD(cmd->Xspeed);
|
||||
s.Yspeed = Y_RS2MOTSPD(cmd->Yspeed);
|
||||
s.xychange = cmd->xychange;
|
||||
s.XBits = cmd->XBits;
|
||||
s.YBits = cmd->YBits;
|
||||
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
|
||||
if(!cmdS(&s)) return MCC_E_FAILED;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief longcmd - send and receive long binary command
|
||||
* @param cmd (io) - command
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t longcmd(long_command_t *cmd){
|
||||
if(!cmd) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel){
|
||||
double curt = timefromstart();
|
||||
moveparam_t param = {0};
|
||||
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
|
||||
if(!model_move2(Xmodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
|
||||
if(!model_move2(Ymodel, ¶m, curt)) return MCC_E_FAILED;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
}
|
||||
SSlcmd l = {0};
|
||||
l.Xmot = X_RAD2MOT(cmd->Xmot);
|
||||
l.Ymot = Y_RAD2MOT(cmd->Ymot);
|
||||
l.Xspeed = X_RS2MOTSPD(cmd->Xspeed);
|
||||
l.Yspeed = Y_RS2MOTSPD(cmd->Yspeed);
|
||||
l.Xadder = X_RS2MOTSPD(cmd->Xadder);
|
||||
l.Yadder = Y_RS2MOTSPD(cmd->Yadder);
|
||||
l.Xatime = S2ADDER(cmd->Xatime);
|
||||
l.Yatime = S2ADDER(cmd->Yatime);
|
||||
if(!cmdL(&l)) return MCC_E_FAILED;
|
||||
setslewingstate();
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
||||
if(!hwConfig) return MCC_E_BADFORMAT;
|
||||
if(Conf.RunModel) return MCC_E_FAILED;
|
||||
SSconfig config;
|
||||
DBG("Read HW configuration");
|
||||
if(!cmdC(&config, FALSE)) return MCC_E_FAILED;
|
||||
// Convert acceleration (ticks per loop^2 to rad/s^2)
|
||||
hwConfig->Xconf.accel = X_MOTACC2RS(config.Xconf.accel);
|
||||
hwConfig->Yconf.accel = Y_MOTACC2RS(config.Yconf.accel);
|
||||
// Convert backlash (ticks to radians)
|
||||
hwConfig->Xconf.backlash = X_MOT2RAD(config.Xconf.backlash);
|
||||
hwConfig->Yconf.backlash = Y_MOT2RAD(config.Yconf.backlash);
|
||||
// Convert error limit (ticks to radians)
|
||||
hwConfig->Xconf.errlimit = X_MOT2RAD(config.Xconf.errlimit);
|
||||
hwConfig->Yconf.errlimit = Y_MOT2RAD(config.Yconf.errlimit);
|
||||
// Proportional, integral, and derivative gains are unitless, so no conversion needed
|
||||
hwConfig->Xconf.propgain = (double)config.Xconf.propgain;
|
||||
hwConfig->Yconf.propgain = (double)config.Yconf.propgain;
|
||||
hwConfig->Xconf.intgain = (double)config.Xconf.intgain;
|
||||
hwConfig->Yconf.intgain = (double)config.Yconf.intgain;
|
||||
hwConfig->Xconf.derivgain = (double)config.Xconf.derivgain;
|
||||
hwConfig->Yconf.derivgain = (double)config.Yconf.derivgain;
|
||||
// Output limit is a percentage (0-100)
|
||||
hwConfig->Xconf.outplimit = (double)config.Xconf.outplimit / 255.0 * 100.0;
|
||||
hwConfig->Yconf.outplimit = (double)config.Yconf.outplimit / 255.0 * 100.0;
|
||||
// Current limit in amps
|
||||
hwConfig->Xconf.currlimit = (double)config.Xconf.currlimit / 100.0;
|
||||
hwConfig->Yconf.currlimit = (double)config.Yconf.currlimit / 100.0;
|
||||
// Integral limit is unitless
|
||||
hwConfig->Xconf.intlimit = (double)config.Xconf.intlimit;
|
||||
hwConfig->Yconf.intlimit = (double)config.Yconf.intlimit;
|
||||
// Copy XBits and YBits (no conversion needed)
|
||||
hwConfig->xbits = config.xbits;
|
||||
hwConfig->ybits = config.ybits;
|
||||
// Copy address
|
||||
hwConfig->address = config.address;
|
||||
|
||||
// TODO: What to do with eqrate, eqadj and trackgoal?
|
||||
config.latitude = __bswap_16(config.latitude);
|
||||
// Convert latitude (degrees * 100 to radians)
|
||||
hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
|
||||
// Copy ticks per revolution
|
||||
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
|
||||
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
|
||||
hwConfig->Xmetpr = __bswap_32(config.Xmetpr); // as documentation said, real ticks are 4 times less
|
||||
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
|
||||
// Convert slew rates (ticks per loop to rad/s)
|
||||
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
||||
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
||||
// Convert pan rates (ticks per loop to rad/s)
|
||||
hwConfig->Xpanrate = X_MOTSPD2RS(config.Xpanrate);
|
||||
hwConfig->Ypanrate = Y_MOTSPD2RS(config.Ypanrate);
|
||||
// Convert guide rates (ticks per loop to rad/s)
|
||||
hwConfig->Xguiderate = X_MOTSPD2RS(config.Xguiderate);
|
||||
hwConfig->Yguiderate = Y_MOTSPD2RS(config.Yguiderate);
|
||||
// copy baudrate
|
||||
hwConfig->baudrate = (uint32_t) config.baudrate;
|
||||
// Convert local search degrees (degrees * 100 to radians)
|
||||
hwConfig->locsdeg = (double)config.locsdeg / 100.0 * M_PI / 180.0;
|
||||
// Convert local search speed (arcsec per second to rad/s)
|
||||
hwConfig->locsspeed = (double)config.locsspeed * M_PI / (180.0 * 3600.0);
|
||||
// Convert backlash speed (ticks per loop to rad/s)
|
||||
hwConfig->backlspd = X_MOTSPD2RS(config.backlspd);
|
||||
// now read text commands
|
||||
int64_t i64;
|
||||
double Xticks, Yticks;
|
||||
DBG("SERIAL");
|
||||
// motor's encoder ticks per rev
|
||||
if(!SSgetint(CMD_MEPRX, &i64)) return MCC_E_FAILED;
|
||||
Xticks = ((double) i64); // divide by 4 as these values stored ???
|
||||
if(!SSgetint(CMD_MEPRY, &i64)) return MCC_E_FAILED;
|
||||
Yticks = ((double) i64);
|
||||
X_ENC_ZERO = Conf.XEncZero;
|
||||
Y_ENC_ZERO = Conf.YEncZero;
|
||||
DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
|
||||
X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
|
||||
Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
|
||||
DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
|
||||
// axis encoder ticks per rev
|
||||
if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
|
||||
Xticks = (double) i64;
|
||||
if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
|
||||
Yticks = (double) i64;
|
||||
DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
|
||||
X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
|
||||
Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
|
||||
DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
||||
SSconfig config;
|
||||
if(Conf.RunModel) return MCC_E_FAILED;
|
||||
// Convert acceleration (rad/s^2 to ticks per loop^2)
|
||||
config.Xconf.accel = X_RS2MOTACC(hwConfig->Xconf.accel);
|
||||
config.Yconf.accel = Y_RS2MOTACC(hwConfig->Yconf.accel);
|
||||
// Convert backlash (radians to ticks)
|
||||
config.Xconf.backlash = X_RAD2MOT(hwConfig->Xconf.backlash);
|
||||
config.Yconf.backlash = Y_RAD2MOT(hwConfig->Yconf.backlash);
|
||||
// Convert error limit (radians to ticks)
|
||||
config.Xconf.errlimit = X_RAD2MOT(hwConfig->Xconf.errlimit);
|
||||
config.Yconf.errlimit = Y_RAD2MOT(hwConfig->Yconf.errlimit);
|
||||
// Proportional, integral, and derivative gains are unitless, so no conversion needed
|
||||
config.Xconf.propgain = (uint16_t)hwConfig->Xconf.propgain;
|
||||
config.Yconf.propgain = (uint16_t)hwConfig->Yconf.propgain;
|
||||
config.Xconf.intgain = (uint16_t)hwConfig->Xconf.intgain;
|
||||
config.Yconf.intgain = (uint16_t)hwConfig->Yconf.intgain;
|
||||
config.Xconf.derivgain = (uint16_t)hwConfig->Xconf.derivgain;
|
||||
config.Yconf.derivgain = (uint16_t)hwConfig->Yconf.derivgain;
|
||||
// Output limit is a percentage (0-100), so convert back to 0-255
|
||||
config.Xconf.outplimit = (uint8_t)(hwConfig->Xconf.outplimit / 100.0 * 255.0);
|
||||
config.Yconf.outplimit = (uint8_t)(hwConfig->Yconf.outplimit / 100.0 * 255.0);
|
||||
// Current limit is in amps (convert back to *100)
|
||||
config.Xconf.currlimit = (uint16_t)(hwConfig->Xconf.currlimit * 100.0);
|
||||
config.Yconf.currlimit = (uint16_t)(hwConfig->Yconf.currlimit * 100.0);
|
||||
// Integral limit is unitless, so no conversion needed
|
||||
config.Xconf.intlimit = (uint16_t)hwConfig->Xconf.intlimit;
|
||||
config.Yconf.intlimit = (uint16_t)hwConfig->Yconf.intlimit;
|
||||
// Copy XBits and YBits (no conversion needed)
|
||||
config.xbits = hwConfig->xbits;
|
||||
config.ybits = hwConfig->ybits;
|
||||
// Convert latitude (radians to degrees * 100)
|
||||
config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
|
||||
// Convert slew rates (rad/s to ticks per loop)
|
||||
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
|
||||
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
|
||||
// Convert pan rates (rad/s to ticks per loop)
|
||||
config.Xpanrate = X_RS2MOTSPD(hwConfig->Xpanrate);
|
||||
config.Ypanrate = Y_RS2MOTSPD(hwConfig->Ypanrate);
|
||||
// Convert guide rates (rad/s to ticks per loop)
|
||||
config.Xguiderate = X_RS2MOTSPD(hwConfig->Xguiderate);
|
||||
config.Yguiderate = Y_RS2MOTSPD(hwConfig->Yguiderate);
|
||||
// Convert local search degrees (radians to degrees * 100)
|
||||
config.locsdeg = (uint32_t)(hwConfig->locsdeg * 180.0 / M_PI * 100.0);
|
||||
// Convert local search speed (rad/s to arcsec per second)
|
||||
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
|
||||
// Convert backlash speed (rad/s to ticks per loop)
|
||||
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
|
||||
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
|
||||
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
|
||||
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
|
||||
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
|
||||
// todo - also write text params
|
||||
// TODO - next
|
||||
(void) config;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// getters of max/min speed and acceleration
|
||||
mcc_errcodes_t maxspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.max.speed;
|
||||
v->Y = Ylimits.max.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t minspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.min.speed;
|
||||
v->Y = Ylimits.min.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t acceleration(coordpair_t *a){
|
||||
if(!a) return MCC_E_BADFORMAT;
|
||||
a->X = Xlimits.max.accel;
|
||||
a->Y = Ylimits.max.accel;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// init mount class
|
||||
mount_t Mount = {
|
||||
.init = init,
|
||||
.quit = quit,
|
||||
.getMountData = getMD,
|
||||
.moveTo = move2,
|
||||
.moveWspeed = move2s,
|
||||
.setSpeed = setspeed,
|
||||
.emergStop = emstop,
|
||||
.stop = stop,
|
||||
.shortCmd = shortcmd,
|
||||
.longCmd = longcmd,
|
||||
.getHWconfig = get_hwconf,
|
||||
.saveHWconfig = write_hwconf,
|
||||
.currentT = curtime,
|
||||
.timeFromStart = timefromstart,
|
||||
.timeDiff = timediff,
|
||||
.timeDiff0 = timediff0,
|
||||
.correctTo = correct2,
|
||||
.getMaxSpeed = maxspeed,
|
||||
.getMinSpeed = minspeed,
|
||||
.getAcceleration = acceleration,
|
||||
};
|
||||
|
||||
81
main.h
Normal file
81
main.h
Normal file
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Almost all here used for debug purposes
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "movingmodel.h"
|
||||
#include "sidservo.h"
|
||||
|
||||
extern conf_t Conf;
|
||||
extern limits_t Xlimits, Ylimits;
|
||||
int curtime(struct timespec *t);
|
||||
double timediff(const struct timespec *time1, const struct timespec *time0);
|
||||
double timediff0(const struct timespec *time1);
|
||||
double timefromstart();
|
||||
void getModData(coordpair_t *c, movestate_t *xst, movestate_t *yst);
|
||||
typedef struct{
|
||||
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
|
||||
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
|
||||
size_t idx; // index of current data in array
|
||||
size_t arraysz; // size of arrays
|
||||
} less_square_t;
|
||||
|
||||
less_square_t *LS_init(size_t Ndata);
|
||||
void LS_delete(less_square_t **ls);
|
||||
double LS_calc_slope(less_square_t *l, double x, double t);
|
||||
|
||||
// unused arguments of functions
|
||||
#define _U_ __attribute__((__unused__))
|
||||
// weak functions
|
||||
#define WEAK __attribute__ ((weak))
|
||||
|
||||
#ifndef DBL_EPSILON
|
||||
#define DBL_EPSILON (2.2204460492503131e-16)
|
||||
#endif
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE (0)
|
||||
#endif
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE (1)
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef EBUG
|
||||
#include <stdio.h>
|
||||
#define COLOR_RED "\033[1;31;40m"
|
||||
#define COLOR_GREEN "\033[1;32;40m"
|
||||
#define COLOR_OLD "\033[0;0;0m"
|
||||
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
||||
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
||||
#define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
|
||||
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
|
||||
fprintf(stderr, __VA_ARGS__); \
|
||||
fprintf(stderr, "\n");} while(0)
|
||||
|
||||
#else // EBUG
|
||||
#define FNAME()
|
||||
#define DBG(...)
|
||||
#endif // EBUG
|
||||
46
movingfilter.c-
Normal file
46
movingfilter.c-
Normal file
@@ -0,0 +1,46 @@
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#define NFILT (5)
|
||||
|
||||
static double filterK[NFILT];
|
||||
|
||||
static void buildFilter(){
|
||||
filterK[NFILT-1] = 1.;
|
||||
double sum = 1.;
|
||||
for(int i = NFILT-2; i > -1; --i){
|
||||
filterK[i] = (filterK[i+1] + 1.) * 1.1;
|
||||
sum += filterK[i];
|
||||
}
|
||||
for(int i = 0; i < NFILT; ++i){
|
||||
filterK[i] /= sum;
|
||||
fprintf(stderr, "%d: %g\n", i, filterK[i]);
|
||||
}
|
||||
}
|
||||
|
||||
static double filter(double val){
|
||||
static int ctr = 0;
|
||||
static double lastvals[NFILT] = {0.};
|
||||
for(int i = NFILT-1; i > 0; --i) lastvals[i] = lastvals[i-1];
|
||||
lastvals[0] = val;
|
||||
double r = 0.;
|
||||
if(ctr < NFILT){
|
||||
++ctr;
|
||||
return val;
|
||||
}
|
||||
for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
buildFilter();
|
||||
printf("Signal\tNoiced\tFiltered\n");
|
||||
for(int i = 0; i < 100; ++i){
|
||||
double di = (double)i;
|
||||
double sig = di * di / 1e5 + sin(i * M_PI / 1500.);
|
||||
double noiced = sig + 0.1 * (drand48() - 0.5);
|
||||
printf("%.3f\t%.3f\t%.3f\n", sig, noiced, filter(noiced));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
73
movingmodel.c
Normal file
73
movingmodel.c
Normal file
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "movingmodel.h"
|
||||
#include "ramp.h"
|
||||
|
||||
extern movemodel_t trapez;
|
||||
|
||||
static void chkminmax(double *min, double *max){
|
||||
if(*min <= *max) return;
|
||||
double t = *min;
|
||||
*min = *max;
|
||||
*max = t;
|
||||
}
|
||||
|
||||
movemodel_t *model_init(limits_t *l){
|
||||
if(!l) return FALSE;
|
||||
movemodel_t *m = calloc(1, sizeof(movemodel_t));
|
||||
// we can't use memcpy or assign as Times/Params would be common for all
|
||||
*m = trapez;
|
||||
m->Times = calloc(STAGE_AMOUNT, sizeof(double));
|
||||
m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
|
||||
moveparam_t *max = &l->max, *min = &l->min;
|
||||
if(min->speed < 0.) min->speed = -min->speed;
|
||||
if(max->speed < 0.) max->speed = -max->speed;
|
||||
if(min->accel < 0.) min->accel = -min->accel;
|
||||
if(max->accel < 0.) max->accel = -max->accel;
|
||||
chkminmax(&min->coord, &max->coord);
|
||||
chkminmax(&min->speed, &max->speed);
|
||||
chkminmax(&min->accel, &max->accel);
|
||||
m->Min = l->min;
|
||||
m->Max = l->max;
|
||||
m->movingstage = STAGE_STOPPED;
|
||||
m->state = ST_STOP;
|
||||
pthread_mutex_init(&m->mutex, NULL);
|
||||
DBG("model inited");
|
||||
return m;
|
||||
}
|
||||
|
||||
int model_move2(movemodel_t *model, moveparam_t *target, double t){
|
||||
if(!target || !model) return FALSE;
|
||||
DBG("MOVE to %g (deg) at speed %g (deg/s)", target->coord/M_PI*180., target->speed/M_PI*180.);
|
||||
// only positive velocity
|
||||
if(target->speed < 0.) target->speed = -target->speed;
|
||||
if(fabs(target->speed) < model->Min.speed){
|
||||
DBG("STOP");
|
||||
model->stop(model, t);
|
||||
return TRUE;
|
||||
}
|
||||
// don't mind about acceleration - user cannot set it now
|
||||
return model->calculate(model, target, t);
|
||||
}
|
||||
76
movingmodel.h
Normal file
76
movingmodel.h
Normal file
@@ -0,0 +1,76 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <pthread.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
// tolerance, time ticks
|
||||
#define COORD_TOLERANCE_DEFAULT (1e-8)
|
||||
#define COORD_TOLERANCE_MIN (1e-12)
|
||||
#define COORD_TOLERANCE_MAX (10.)
|
||||
#define TIME_TICK_DEFAULT (0.0001)
|
||||
#define TIME_TICK_MIN (1e-9)
|
||||
#define TIME_TICK_MAX (10.)
|
||||
|
||||
typedef enum{
|
||||
ST_STOP, // stopped
|
||||
ST_MOVE, // moving
|
||||
ST_AMOUNT
|
||||
} movestate_t;
|
||||
|
||||
typedef struct{
|
||||
double coord;
|
||||
double speed;
|
||||
double accel;
|
||||
} moveparam_t;
|
||||
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
//double acceleration;
|
||||
} limits_t;
|
||||
|
||||
typedef enum{
|
||||
STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed
|
||||
STAGE_MAXSPEED, // go with target speed
|
||||
STAGE_DECEL, // go from target speed to zero
|
||||
STAGE_STOPPED, // stop
|
||||
STAGE_AMOUNT
|
||||
} movingstage_t;
|
||||
|
||||
typedef struct movemodel{
|
||||
moveparam_t Min;
|
||||
moveparam_t Max;
|
||||
movingstage_t movingstage;
|
||||
movestate_t state;
|
||||
double *Times;
|
||||
moveparam_t *Params;
|
||||
moveparam_t curparams; // init values of limits, jerk
|
||||
int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||
movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t
|
||||
movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state
|
||||
void (*stop)(struct movemodel *m, double t); // stop by ramp
|
||||
void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration
|
||||
double (*stoppedtime)(struct movemodel *m); // time when moving will ends
|
||||
pthread_mutex_t mutex;
|
||||
} movemodel_t;
|
||||
|
||||
movemodel_t *model_init(limits_t *l);
|
||||
int model_move2(movemodel_t *model, moveparam_t *target, double t);
|
||||
197
polltest/main.c
Normal file
197
polltest/main.c
Normal file
@@ -0,0 +1,197 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
// suppose that we ONLY poll data
|
||||
#define XYBUFSZ (128)
|
||||
|
||||
struct{
|
||||
int help;
|
||||
char *Xpath;
|
||||
char *Ypath;
|
||||
double dt;
|
||||
} G = {
|
||||
.Xpath = "/dev/encoder_X0",
|
||||
.Ypath = "/dev/encoder_Y0",
|
||||
.dt = 0.001,
|
||||
};
|
||||
|
||||
sl_option_t options[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"Xpath", NEED_ARG, NULL, 'X', arg_string, APTR(&G.Xpath), "path to X encoder"},
|
||||
{"Ypath", NEED_ARG, NULL, 'Y', arg_string, APTR(&G.Ypath), "path to Y encoder"},
|
||||
{"dt", NEED_ARG, NULL, 'd', arg_double, APTR(&G.dt), "request interval (1e-4..10s)"},
|
||||
};
|
||||
|
||||
typedef struct{
|
||||
char buf[XYBUFSZ+1];
|
||||
int len;
|
||||
} buf_t;
|
||||
|
||||
static int Xfd = -1, Yfd = -1;
|
||||
|
||||
void signals(int sig){
|
||||
if(sig){
|
||||
signal(sig, SIG_IGN);
|
||||
DBG("Get signal %d, quit.\n", sig);
|
||||
}
|
||||
DBG("close");
|
||||
if(Xfd > 0){ close(Xfd); Xfd = -1; }
|
||||
if(Yfd > 0){ close(Yfd); Yfd = -1; }
|
||||
exit(sig);
|
||||
}
|
||||
|
||||
static int op(const char *nm){
|
||||
int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK);
|
||||
if(fd < 0) ERR("Can't open %s", nm);
|
||||
struct termios2 tty;
|
||||
if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings");
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
|
||||
tty.c_ispeed = 1000000;
|
||||
tty.c_ospeed = 1000000;
|
||||
if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings");
|
||||
// try to set exclusive
|
||||
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
|
||||
return fd;
|
||||
}
|
||||
|
||||
// write to buffer next data portion; return FALSE in case of error
|
||||
static int readstrings(buf_t *buf, int fd){
|
||||
FNAME();
|
||||
if(!buf){WARNX("Empty buffer"); return FALSE;}
|
||||
int L = XYBUFSZ - buf->len;
|
||||
if(L == 0){
|
||||
DBG("buffer overfull!", buf->len);
|
||||
char *lastn = strrchr(buf->buf, '\n');
|
||||
if(lastn){
|
||||
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
|
||||
++lastn;
|
||||
buf->len = XYBUFSZ - (lastn - buf->buf);
|
||||
DBG("Memmove %d", buf->len);
|
||||
memmove(lastn, buf->buf, buf->len);
|
||||
buf->buf[buf->len] = 0;
|
||||
}else buf->len = 0;
|
||||
L = XYBUFSZ - buf->len;
|
||||
}
|
||||
int got = read(fd, &buf->buf[buf->len], L);
|
||||
if(got < 0){
|
||||
WARN("read()");
|
||||
return FALSE;
|
||||
}else if(got == 0){ DBG("NO data"); return TRUE; }
|
||||
buf->len += got;
|
||||
buf->buf[buf->len] = 0;
|
||||
DBG("buf[%d]: %s", buf->len, buf->buf);
|
||||
return TRUE;
|
||||
}
|
||||
// return TRUE if got, FALSE if no data found
|
||||
static int getdata(buf_t *buf, long *out){
|
||||
if(!buf || buf->len < 1) return FALSE;
|
||||
// read record between last '\n' and previous (or start of string)
|
||||
char *last = &buf->buf[buf->len - 1];
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
if(*last != '\n') return FALSE;
|
||||
*last = 0;
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
char *prev = strrchr(buf->buf, '\n');
|
||||
if(!prev) prev = buf->buf;
|
||||
else{
|
||||
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
|
||||
++prev; // after last '\n'
|
||||
}
|
||||
if(out) *out = atol(prev);
|
||||
// clear buffer
|
||||
buf->len = 0;
|
||||
return TRUE;
|
||||
}
|
||||
// try to write '\n' asking new data portion; return FALSE if failed
|
||||
static int asknext(int fd){
|
||||
FNAME();
|
||||
if(fd < 0) return FALSE;
|
||||
int i = 0;
|
||||
for(; i < 5; ++i){
|
||||
int l = write(fd, "\n", 1);
|
||||
//DBG("l=%d", l);
|
||||
if(1 == l) return TRUE;
|
||||
usleep(100);
|
||||
}
|
||||
DBG("5 tries... failed!");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
buf_t xbuf, ybuf;
|
||||
long xlast, ylast;
|
||||
double xtlast, ytlast;
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, options);
|
||||
if(G.help) sl_showhelp(-1, options);
|
||||
if(G.dt < 1e-4) ERRX("dx too small");
|
||||
if(G.dt > 10.) ERRX("dx too big");
|
||||
Xfd = op(G.Xpath);
|
||||
Yfd = op(G.Ypath);
|
||||
struct pollfd pfds[2];
|
||||
pfds[0].fd = Xfd; pfds[0].events = POLLIN;
|
||||
pfds[1].fd = Yfd; pfds[1].events = POLLIN;
|
||||
double t0x, t0y, tstart;
|
||||
asknext(Xfd); asknext(Yfd);
|
||||
t0x = t0y = tstart = sl_dtime();
|
||||
DBG("Start");
|
||||
do{ // main cycle
|
||||
if(poll(pfds, 2, 0) < 0){
|
||||
WARN("poll()");
|
||||
break;
|
||||
}
|
||||
if(pfds[0].revents && POLLIN){
|
||||
DBG("got X");
|
||||
if(!readstrings(&xbuf, Xfd)) break;
|
||||
}
|
||||
if(pfds[1].revents && POLLIN){
|
||||
DBG("got Y");
|
||||
if(!readstrings(&ybuf, Yfd)) break;
|
||||
}
|
||||
double curt = sl_dtime();
|
||||
if(getdata(&xbuf, &xlast)) xtlast = curt;
|
||||
if(curt - t0x >= G.dt){ // get last records
|
||||
if(curt - xtlast < 1.5*G.dt)
|
||||
printf("%-14.4fX=%ld\n", xtlast-tstart, xlast);
|
||||
if(!asknext(Xfd)) break;
|
||||
t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt;
|
||||
}
|
||||
curt = sl_dtime();
|
||||
if(getdata(&ybuf, &ylast)) ytlast = curt;
|
||||
if(curt - t0y >= G.dt){ // get last records
|
||||
if(curt - ytlast < 1.5*G.dt)
|
||||
printf("%-14.4fY=%ld\n", ytlast-tstart, ylast);
|
||||
if(!asknext(Yfd)) break;
|
||||
t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt;
|
||||
}
|
||||
}while(Xfd > 0 && Yfd > 0);
|
||||
DBG("OOps: disconnected");
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
259
ramp.c
Normal file
259
ramp.c
Normal file
@@ -0,0 +1,259 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// simplest trapezioidal ramp
|
||||
|
||||
#include <math.h>
|
||||
#include <strings.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "ramp.h"
|
||||
|
||||
#ifdef EBUG
|
||||
#undef DBG
|
||||
#define DBG(...)
|
||||
#undef FNAME
|
||||
#define FNAME()
|
||||
#endif
|
||||
|
||||
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
|
||||
static void emstop(movemodel_t *m, double _U_ t){
|
||||
FNAME();
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
m->curparams.accel = 0.;
|
||||
m->curparams.speed = 0.;
|
||||
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
|
||||
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
|
||||
m->state = ST_STOP;
|
||||
m->movingstage = STAGE_STOPPED;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
}
|
||||
|
||||
static void stop(movemodel_t *m, double t){
|
||||
FNAME();
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
|
||||
m->movingstage = STAGE_DECEL;
|
||||
m->state = ST_MOVE;
|
||||
m->Times[STAGE_DECEL] = t;
|
||||
m->Params[STAGE_DECEL].speed = m->curparams.speed;
|
||||
if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
|
||||
else m->Params[STAGE_DECEL].accel = m->Max.accel;
|
||||
m->Params[STAGE_DECEL].coord = m->curparams.coord;
|
||||
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
|
||||
m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
|
||||
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
|
||||
double dt = m->Times[STAGE_STOPPED] - t;
|
||||
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
|
||||
m->Params[STAGE_DECEL].accel * dt * dt / 2.;
|
||||
ret:
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
}
|
||||
|
||||
// inner part of `calc`, could be called recoursively for hard case
|
||||
static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
|
||||
// signs
|
||||
double sign_a01 = 0., sign_a23 = 0., sign_vset = 0.; // accelerations on stages ACCEL and DECEL, speed on maxspeed stage
|
||||
// times
|
||||
double dt01 = 0., dt12 = 0., dt23 = 0.;
|
||||
// absolute speed at stage 23 (or in that point); absolute max acceleration
|
||||
double abs_vset = x->speed, abs_a = m->Max.accel;
|
||||
// absolute target movement
|
||||
double abs_Dx = fabs(x->coord - m->curparams.coord);
|
||||
if(m->state == ST_STOP && abs_Dx < coord_tolerance){
|
||||
DBG("Movement too small -> stay at place");
|
||||
return;
|
||||
}
|
||||
// signs of Dx and current speed
|
||||
double sign_Dx = (x->coord > m->curparams.coord) ? 1. : -1.;
|
||||
double v0 = m->curparams.speed;
|
||||
double sign_v0 = v0 < 0. ? -1 : 1., abs_v0 = fabs(v0);
|
||||
if(v0 == 0.) sign_v0 = 0.;
|
||||
// preliminary calculations (vset and dependent values could be changed)
|
||||
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
|
||||
double abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
|
||||
dt23 = abs_vset / abs_a;
|
||||
double abs_dx_stop = abs_v0 * abs_v0 / 2. / abs_a;
|
||||
if(sign_Dx * sign_v0 >= 0. && abs_dx_stop < abs_Dx){ // we shouldn't change speed direction
|
||||
if(fabs(abs_dx_stop - abs_Dx) <= coord_tolerance){ // simplest case: just stop
|
||||
//DBG("Simplest case: stop");
|
||||
dt01 = dt12 = 0.;
|
||||
sign_a23 = -sign_v0;
|
||||
dt23 = abs_v0 / abs_a;
|
||||
}else if(abs_vset < abs_v0){ // move with smaller speed than now: very simple case
|
||||
//DBG("Move with smaller speed");
|
||||
sign_a01 = sign_a23 = -sign_v0;
|
||||
sign_vset = sign_v0;
|
||||
double abs_dx01 = abs_v0 * dt01 - abs_a * dt01 * dt01 / 2.;
|
||||
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
|
||||
dt12 = abs_dx12 / abs_vset;
|
||||
}else{// move with larget speed
|
||||
//DBG("Move with larger speed");
|
||||
double abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
|
||||
if(abs_Dx < abs_dx01 + abs_dx23){ // recalculate target speed and other
|
||||
abs_vset = sqrt(abs_a * abs_Dx + abs_v0 * abs_v0 / 2.);
|
||||
dt01 = fabs(abs_v0 - abs_vset) / abs_a;
|
||||
abs_dx01 = abs_v0 * dt01 + abs_a * dt01 * dt01 / 2.;
|
||||
dt23 = abs_vset / abs_a;
|
||||
abs_dx23 = abs_vset * abs_vset / 2. / abs_a;
|
||||
DBG("Can't reach target speed %g, take %g instead", x->speed, abs_vset);
|
||||
}
|
||||
sign_a01 = sign_Dx; // sign_v0 could be ZERO!!!
|
||||
sign_a23 = -sign_Dx;
|
||||
sign_vset = sign_Dx;
|
||||
double abs_dx12 = abs_Dx - abs_dx01 - abs_dx23;
|
||||
dt12 = abs_dx12 / abs_vset;
|
||||
}
|
||||
}else{
|
||||
// if we are here, we have the worst case: change speed direction
|
||||
// DBG("Hardest case: change speed direction");
|
||||
// now we should calculate coordinate at which model stops and biuld new trapezium from that point
|
||||
double x0 = m->curparams.coord, v0 = m->curparams.speed;
|
||||
double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
|
||||
m->state = ST_STOP;
|
||||
m->curparams.accel = 0.; m->curparams.coord = xstop; m->curparams.speed = 0.;
|
||||
unlockedcalc(m, x, tstop); // calculate new ramp
|
||||
// and change started conditions
|
||||
m->curparams.coord = x0; m->curparams.speed = v0;
|
||||
m->Times[STAGE_ACCEL] = t;
|
||||
m->Params[STAGE_ACCEL].coord = x0;
|
||||
m->Params[STAGE_ACCEL].speed = v0;
|
||||
// DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
|
||||
return;
|
||||
}
|
||||
m->state = ST_MOVE;
|
||||
m->movingstage = STAGE_ACCEL;
|
||||
// some knot parameters
|
||||
double a01 = sign_a01 * abs_a, a23 = sign_a23 * abs_a;
|
||||
double v1, v2, x0, x1, x2;
|
||||
v2 = v1 = sign_vset * abs_vset;
|
||||
x0 = m->curparams.coord;
|
||||
x1 = x0 + v0 * dt01 + a01 * dt01 * dt01 / 2.;
|
||||
x2 = x1 + v1 * dt12;
|
||||
// fill knot parameters
|
||||
moveparam_t *p = &m->Params[STAGE_ACCEL]; // 0-1 - change started speed
|
||||
p->accel = a01;
|
||||
p->speed = m->curparams.speed;
|
||||
p->coord = x0;
|
||||
m->Times[STAGE_ACCEL] = t;
|
||||
p = &m->Params[STAGE_MAXSPEED]; // 1-2 - constant speed
|
||||
p->accel = 0.;
|
||||
p->speed = v1;
|
||||
p->coord = x1;
|
||||
m->Times[STAGE_MAXSPEED] = m->Times[STAGE_ACCEL] + dt01;
|
||||
p = &m->Params[STAGE_DECEL]; // 2-3 - decrease speed
|
||||
p->accel = a23;
|
||||
p->speed = v2;
|
||||
p->coord = x2;
|
||||
m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
|
||||
p = &m->Params[STAGE_STOPPED]; // 3 - stop at target
|
||||
p->accel = p->speed = 0.;
|
||||
p->coord = x->coord;
|
||||
m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief calc - moving calculation
|
||||
* @param x - using max speed (>0!!!) and coordinate
|
||||
* @param t - current time value
|
||||
* @return FALSE if can't move with given parameters
|
||||
*/
|
||||
static int calc(movemodel_t *m, moveparam_t *x, double t) {
|
||||
//DBG("target coord/speed: %g/%g; current: %g/%g", x->coord, x->speed, m->curparams.coord, m->curparams.speed);
|
||||
if (!x || !m) return FALSE;
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
int ret = FALSE;
|
||||
// Validate input parameters
|
||||
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
|
||||
DBG("Wrong coordinate [%g, %g]", m->Min.coord, m->Max.coord);
|
||||
goto ret;
|
||||
}
|
||||
if(x->speed < m->Min.speed || x->speed > m->Max.speed){
|
||||
DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
|
||||
goto ret;
|
||||
}
|
||||
ret = TRUE; // now there's no chanses to make error
|
||||
unlockedcalc(m, x, t);
|
||||
// Debug output
|
||||
/*for(int i = 0; i < STAGE_AMOUNT; i++){
|
||||
DBG("Stage %d: t=%.6f, coord=%.6f, speed=%.6f, accel=%.6f",
|
||||
i, m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
|
||||
}*/
|
||||
ret:
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(m->state == ST_STOP) goto ret;
|
||||
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
|
||||
if(m->Times[s] <= t){ // check time for current stage
|
||||
m->movingstage = s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(m->movingstage == STAGE_STOPPED){
|
||||
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
/* DBG("REACHED STOPping stage @ t=%g", t);
|
||||
for(int s = STAGE_STOPPED; s >= 0; --s){
|
||||
DBG("T[%d]=%g, ", s, m->Times[s]);
|
||||
}*/
|
||||
emstop(m, t);
|
||||
goto ret;
|
||||
}
|
||||
// calculate current parameters
|
||||
double dt = t - m->Times[m->movingstage];
|
||||
double a = m->Params[m->movingstage].accel;
|
||||
double v0 = m->Params[m->movingstage].speed;
|
||||
double x0 = m->Params[m->movingstage].coord;
|
||||
m->curparams.accel = a;
|
||||
m->curparams.speed = v0 + a * dt;
|
||||
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
|
||||
ret:
|
||||
if(next) *next = m->curparams;
|
||||
movestate_t st = m->state;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return st;
|
||||
}
|
||||
|
||||
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
if(cur) *cur = m->curparams;
|
||||
movestate_t st = m->state;
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return st;
|
||||
}
|
||||
|
||||
static double gettstop(movemodel_t *m){
|
||||
pthread_mutex_lock(&m->mutex);
|
||||
double r = m->Times[STAGE_STOPPED];
|
||||
pthread_mutex_unlock(&m->mutex);
|
||||
return r;
|
||||
}
|
||||
|
||||
movemodel_t trapez = {
|
||||
.stop = stop,
|
||||
.emergency_stop = emstop,
|
||||
.get_state = getst,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stoppedtime = gettstop,
|
||||
};
|
||||
23
ramp.h
Normal file
23
ramp.h
Normal file
@@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "movingmodel.h"
|
||||
|
||||
extern movemodel_t trapez;
|
||||
884
serial.c
Normal file
884
serial.c
Normal file
@@ -0,0 +1,884 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <asm-generic/termbits.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "movingmodel.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
// serial devices FD
|
||||
static int encfd[2] = {-1, -1}, mntfd = -1;
|
||||
// main mount data
|
||||
static mountdata_t mountdata = {0};
|
||||
// last encoders time and last encoders data - for speed measurement
|
||||
//static coordval_t lastXenc = {0}, lastYenc = {0};
|
||||
|
||||
// mutexes for RW operations with mount device and data
|
||||
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
|
||||
datamutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
// encoders thread and mount thread
|
||||
static pthread_t encthread, mntthread;
|
||||
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
|
||||
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
|
||||
// encoders raw data
|
||||
typedef struct __attribute__((packed)){
|
||||
uint8_t magick;
|
||||
int32_t encY;
|
||||
int32_t encX;
|
||||
uint8_t CRC[4];
|
||||
} enc_t;
|
||||
|
||||
// calculate current X/Y speeds
|
||||
void getXspeed(){
|
||||
static less_square_t *ls = NULL;
|
||||
if(!ls){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
double dt = timediff0(&mountdata.encXposition.t);
|
||||
double speed = LS_calc_slope(ls, mountdata.encXposition.val, dt);
|
||||
if(fabs(speed) < 1.5 * Xlimits.max.speed){
|
||||
mountdata.encXspeed.val = speed;
|
||||
mountdata.encXspeed.t = mountdata.encXposition.t;
|
||||
}
|
||||
}
|
||||
void getYspeed(){
|
||||
static less_square_t *ls = NULL;
|
||||
if(!ls){
|
||||
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
|
||||
if(!ls) return;
|
||||
}
|
||||
double dt = timediff0(&mountdata.encYposition.t);
|
||||
double speed = LS_calc_slope(ls, mountdata.encYposition.val, dt);
|
||||
if(fabs(speed) < 1.5 * Ylimits.max.speed){
|
||||
mountdata.encYspeed.val = speed;
|
||||
mountdata.encYspeed.t = mountdata.encYposition.t;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief parse_encbuf - check encoder buffer (for encoder data based on SSII proto) and fill fresh data
|
||||
* @param databuf - input buffer with 13 bytes of data
|
||||
* @param t - time when databuf[0] got
|
||||
*/
|
||||
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
|
||||
if(!t) return;
|
||||
enc_t *edata = (enc_t*) databuf;
|
||||
/*
|
||||
#ifdef EBUG
|
||||
DBG("ENCBUF:");
|
||||
for(int i = 0; i < ENC_DATALEN; ++i) printf("%02X ", databuf[i]);
|
||||
printf("\n");
|
||||
#endif
|
||||
*/
|
||||
if(edata->magick != ENC_MAGICK){
|
||||
DBG("No magick");
|
||||
return;
|
||||
}
|
||||
if(edata->CRC[3]){
|
||||
DBG("No 0 @ end: 0x%02x", edata->CRC[3]);
|
||||
return;
|
||||
}
|
||||
uint32_t POS_SUM = 0;
|
||||
for(int i = 1; i < 9; ++i) POS_SUM += databuf[i];
|
||||
uint8_t x = POS_SUM >> 8;
|
||||
if(edata->CRC[0] != x){
|
||||
DBG("CRC[0] = 0x%02x, need 0x%02x", edata->CRC[0], x);
|
||||
return;
|
||||
}
|
||||
uint8_t y = ((0xFFFF - POS_SUM) & 0xFF) - x;
|
||||
if(edata->CRC[1] != y){
|
||||
DBG("CRC[1] = 0x%02x, need 0x%02x", edata->CRC[1], y);
|
||||
return;
|
||||
}
|
||||
y = (0xFFFF - POS_SUM) >> 8;
|
||||
if(edata->CRC[2] != y){
|
||||
DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
|
||||
return;
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encXposition.val = Xenc2rad(edata->encX);
|
||||
mountdata.encYposition.val = Yenc2rad(edata->encY);
|
||||
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
|
||||
mountdata.encXposition.t = *t;
|
||||
mountdata.encYposition.t = *t;
|
||||
getXspeed(); getYspeed();
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
|
||||
}
|
||||
|
||||
#if 0
|
||||
/**
|
||||
* @brief getencval - get uint64_t data from encoder
|
||||
* @param fd - encoder fd
|
||||
* @param val - value read
|
||||
* @param t - measurement time
|
||||
* @return amount of data read or 0 if problem
|
||||
*/
|
||||
static int getencval(int fd, double *val, struct timespec *t){
|
||||
if(fd < 0){
|
||||
DBG("Encoder fd < 0!");
|
||||
return FALSE;
|
||||
}
|
||||
char buf[128];
|
||||
int got = 0, Lmax = 127;
|
||||
double t0 = timefromstart();
|
||||
//DBG("start: %.6g", t0);
|
||||
do{
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(fd, &rfds);
|
||||
struct timeval tv = encRtmout;
|
||||
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval){
|
||||
//DBG("select()==0 - timeout, %.6g", timefromstart());
|
||||
break;
|
||||
}
|
||||
if(retval < 0){
|
||||
if(errno == EINTR){
|
||||
DBG("EINTR");
|
||||
continue;
|
||||
}
|
||||
DBG("select() < 0");
|
||||
return 0;
|
||||
}
|
||||
if(FD_ISSET(fd, &rfds)){
|
||||
ssize_t l = read(fd, &buf[got], Lmax);
|
||||
if(l < 1){
|
||||
DBG("read() < 0");
|
||||
return 0; // disconnected ??
|
||||
}
|
||||
got += l; Lmax -= l;
|
||||
buf[got] = 0;
|
||||
} else continue;
|
||||
if(buf[got-1] == '\n') break; // got EOL as last symbol
|
||||
}while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
|
||||
if(got == 0){
|
||||
//DBG("No data from encoder, tfs=%.6g", timefromstart());
|
||||
return 0;
|
||||
}
|
||||
char *estr = strrchr(buf, '\n');
|
||||
if(!estr){
|
||||
DBG("No EOL");
|
||||
return 0;
|
||||
}
|
||||
*estr = 0;
|
||||
char *bgn = strrchr(buf, '\n');
|
||||
if(bgn) ++bgn;
|
||||
else bgn = buf;
|
||||
char *eptr;
|
||||
long data = strtol(bgn, &eptr, 10);
|
||||
if(eptr != estr){
|
||||
DBG("NAN");
|
||||
return 0; // wrong number
|
||||
}
|
||||
if(val) *val = (double) data;
|
||||
if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
|
||||
return got;
|
||||
}
|
||||
#endif
|
||||
|
||||
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
|
||||
static int getencbyte(){
|
||||
if(encfd[0] < 0) return -1;
|
||||
uint8_t byte = 0;
|
||||
fd_set rfds;
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(encfd[0], &rfds);
|
||||
struct timeval tv = encRtmout;
|
||||
int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval) break;
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
return -1;
|
||||
}
|
||||
if(FD_ISSET(encfd[0], &rfds)){
|
||||
ssize_t l = read(encfd[0], &byte, 1);
|
||||
if(l != 1) return -2; // disconnected ??
|
||||
break;
|
||||
} else return -1;
|
||||
}while(1);
|
||||
return (int)byte;
|
||||
}
|
||||
// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
|
||||
static int getmntbyte(){
|
||||
if(mntfd < 0) return -1;
|
||||
uint8_t byte;
|
||||
fd_set rfds;
|
||||
/* ssize_t l = read(mntfd, &byte, 1);
|
||||
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
||||
if(l == 0) return -1;
|
||||
if(l != 1) return -2; // disconnected ??
|
||||
return (int) byte;*/
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(mntfd, &rfds);
|
||||
struct timeval tv = mntRtmout;
|
||||
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
DBG("Error in select()");
|
||||
return -1;
|
||||
}
|
||||
//DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds));
|
||||
if(FD_ISSET(mntfd, &rfds)){
|
||||
ssize_t l = read(mntfd, &byte, 1);
|
||||
//DBG("MNT read=%zd byte=0x%X", l, byte);
|
||||
if(l != 1){
|
||||
DBG("Mount disconnected?");
|
||||
return -2; // disconnected ??
|
||||
}
|
||||
break;
|
||||
} else return -1;
|
||||
}while(1);
|
||||
return (int)byte;
|
||||
}
|
||||
// clear data from input buffer
|
||||
static void clrmntbuf(){
|
||||
if(mntfd < 0) return;
|
||||
uint8_t byte;
|
||||
fd_set rfds;
|
||||
do{
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(mntfd, &rfds);
|
||||
struct timeval tv = {.tv_sec=0, .tv_usec=10};
|
||||
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(retval < 0){
|
||||
if(errno == EINTR) continue;
|
||||
DBG("Error in select()");
|
||||
break;
|
||||
}
|
||||
if(FD_ISSET(mntfd, &rfds)){
|
||||
ssize_t l = read(mntfd, &byte, 1);
|
||||
if(l != 1) break;
|
||||
} else break;
|
||||
}while(1);
|
||||
}
|
||||
|
||||
// main encoder thread (for separate encoder): read next data and make parsing
|
||||
static void *encoderthread1(void _U_ *u){
|
||||
if(Conf.SepEncoder != 1) return NULL;
|
||||
uint8_t databuf[ENC_DATALEN];
|
||||
int wridx = 0, errctr = 0;
|
||||
struct timespec tcur;
|
||||
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
|
||||
int b = getencbyte();
|
||||
if(b == -2) ++errctr;
|
||||
if(b < 0) continue;
|
||||
errctr = 0;
|
||||
// DBG("Got byte from Encoder: 0x%02X", b);
|
||||
if(wridx == 0){
|
||||
if((uint8_t)b == ENC_MAGICK){
|
||||
// DBG("Got magic -> start filling packet");
|
||||
databuf[wridx++] = (uint8_t) b;
|
||||
}
|
||||
continue;
|
||||
}else databuf[wridx++] = (uint8_t) b;
|
||||
if(wridx == ENC_DATALEN){
|
||||
if(curtime(&tcur)){
|
||||
parse_encbuf(databuf, &tcur);
|
||||
wridx = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(encfd[0] > -1){
|
||||
close(encfd[0]);
|
||||
encfd[0] = -1;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
#define XYBUFSZ (128)
|
||||
typedef struct{
|
||||
char buf[XYBUFSZ+1];
|
||||
int len;
|
||||
} buf_t;
|
||||
|
||||
// write to buffer next data portion; return FALSE in case of error
|
||||
static int readstrings(buf_t *buf, int fd){
|
||||
if(!buf){DBG("Empty buffer"); return FALSE;}
|
||||
int L = XYBUFSZ - buf->len;
|
||||
if(L < 0){
|
||||
DBG("buf not initialized!");
|
||||
buf->len = 0;
|
||||
}
|
||||
if(L == 0){
|
||||
DBG("buffer overfull: %d!", buf->len);
|
||||
char *lastn = strrchr(buf->buf, '\n');
|
||||
if(lastn){
|
||||
fprintf(stderr, "BUFOVR: _%s_", buf->buf);
|
||||
++lastn;
|
||||
buf->len = XYBUFSZ - (lastn - buf->buf);
|
||||
DBG("Memmove %d", buf->len);
|
||||
memmove(lastn, buf->buf, buf->len);
|
||||
buf->buf[buf->len] = 0;
|
||||
}else buf->len = 0;
|
||||
L = XYBUFSZ - buf->len;
|
||||
}
|
||||
//DBG("read %d bytes from %d", L, fd);
|
||||
int got = read(fd, &buf->buf[buf->len], L);
|
||||
if(got < 0){
|
||||
DBG("read()");
|
||||
return FALSE;
|
||||
}else if(got == 0){ DBG("NO data"); return TRUE; }
|
||||
buf->len += got;
|
||||
buf->buf[buf->len] = 0;
|
||||
//DBG("buf[%d]: %s", buf->len, buf->buf);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return TRUE if got, FALSE if no data found
|
||||
static int getdata(buf_t *buf, long *out){
|
||||
if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE;
|
||||
// read record between last '\n' and previous (or start of string)
|
||||
char *last = &buf->buf[buf->len - 1];
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
if(*last != '\n') return FALSE;
|
||||
*last = 0;
|
||||
//DBG("buf: _%s_", buf->buf);
|
||||
char *prev = strrchr(buf->buf, '\n');
|
||||
if(!prev) prev = buf->buf;
|
||||
else{
|
||||
fprintf(stderr, "MORETHANONE: _%s_", buf->buf);
|
||||
++prev; // after last '\n'
|
||||
}
|
||||
if(out) *out = atol(prev);
|
||||
// clear buffer
|
||||
buf->len = 0;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// try to write '\n' asking new data portion; return FALSE if failed
|
||||
static int asknext(int fd){
|
||||
//FNAME();
|
||||
if(fd < 0) return FALSE;
|
||||
int i = 0;
|
||||
for(; i < 5; ++i){
|
||||
int l = write(fd, "\n", 1);
|
||||
//DBG("l=%d", l);
|
||||
if(1 == l) return TRUE;
|
||||
usleep(100);
|
||||
}
|
||||
DBG("5 tries... failed!");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// main encoder thread for separate encoders as USB devices /dev/encoder_X0 and /dev/encoder_Y0
|
||||
static void *encoderthread2(void _U_ *u){
|
||||
if(Conf.SepEncoder != 2) return NULL;
|
||||
DBG("Thread started");
|
||||
struct pollfd pfds[2];
|
||||
pfds[0].fd = encfd[0]; pfds[0].events = POLLIN;
|
||||
pfds[1].fd = encfd[1]; pfds[1].events = POLLIN;
|
||||
double t0[2], tstart;
|
||||
buf_t strbuf[2] = {0};
|
||||
long msrlast[2]; // last encoder data
|
||||
double mtlast[2]; // last measurement time
|
||||
asknext(encfd[0]); asknext(encfd[1]);
|
||||
t0[0] = t0[1] = tstart = timefromstart();
|
||||
int errctr = 0;
|
||||
do{ // main cycle
|
||||
if(poll(pfds, 2, 0) < 0){
|
||||
DBG("poll()");
|
||||
break;
|
||||
}
|
||||
int got = 0;
|
||||
for(int i = 0; i < 2; ++i){
|
||||
if(pfds[i].revents && POLLIN){
|
||||
if(!readstrings(&strbuf[i], encfd[i])){
|
||||
++errctr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
double curt = timefromstart();
|
||||
if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
|
||||
if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
|
||||
if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
|
||||
pthread_mutex_lock(&datamutex);
|
||||
if(i == 0){
|
||||
mountdata.encXposition.val = Xenc2rad((double)msrlast[i]);
|
||||
curtime(&mountdata.encXposition.t);
|
||||
/*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
|
||||
msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
|
||||
X_ENC_ZERO, X_ENC_STEPSPERREV);*/
|
||||
getXspeed();
|
||||
}else{
|
||||
mountdata.encYposition.val = Yenc2rad((double)msrlast[i]);
|
||||
curtime(&mountdata.encYposition.t);
|
||||
getYspeed();
|
||||
}
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
}
|
||||
if(!asknext(encfd[i])){
|
||||
++errctr;
|
||||
break;
|
||||
}
|
||||
t0[i] = (curt - t0[i] < 2.*Conf.EncoderReqInterval) ? t0[i] + Conf.EncoderReqInterval : curt;
|
||||
++got;
|
||||
}
|
||||
}
|
||||
if(got == 2) errctr = 0;
|
||||
}while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR);
|
||||
DBG("\n\nEXIT: ERRCTR=%d", errctr);
|
||||
for(int i = 0; i < 2; ++i){
|
||||
if(encfd[i] > -1){
|
||||
close(encfd[i]);
|
||||
encfd[i] = -1;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
data_t *cmd2dat(const char *cmd){
|
||||
if(!cmd) return NULL;
|
||||
data_t *d = calloc(1, sizeof(data_t));
|
||||
if(!d) return NULL;
|
||||
d->buf = (uint8_t*)strdup(cmd);
|
||||
d->len = strlen(cmd);
|
||||
d->maxlen = d->len + 1;
|
||||
return d;
|
||||
}
|
||||
void data_free(data_t **x){
|
||||
if(!x || !*x) return;
|
||||
free((*x)->buf);
|
||||
free(*x);
|
||||
*x = NULL;
|
||||
}
|
||||
|
||||
static void chkModStopped(double *prev, double cur, int *nstopped, axis_status_t *stat){
|
||||
if(!prev || !nstopped || !stat) return;
|
||||
if(isnan(*prev)){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("START");
|
||||
}else if(*stat != AXIS_STOPPED){
|
||||
if(fabs(*prev - cur) < DBL_EPSILON && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("AXIS stopped; prev=%g, cur=%g; nstopped=%d", *prev/M_PI*180., cur/M_PI*180., *nstopped);
|
||||
}
|
||||
}else if(*prev != cur){
|
||||
DBG("AXIS moving");
|
||||
*nstopped = 0;
|
||||
}
|
||||
*prev = cur;
|
||||
}
|
||||
|
||||
// main mount thread
|
||||
static void *mountthread(void _U_ *u){
|
||||
int errctr = 0;
|
||||
uint8_t buf[2*sizeof(SSstat)];
|
||||
SSstat *status = (SSstat*) buf;
|
||||
bzero(&mountdata, sizeof(mountdata));
|
||||
double t0 = timefromstart(), tstart = t0, tcur = t0;
|
||||
double oldmt = -100.; // old `millis measurement` time
|
||||
static uint32_t oldmillis = 0;
|
||||
if(Conf.RunModel){
|
||||
double Xprev = NAN, Yprev = NAN; // previous coordinates
|
||||
int xcnt = 0, ycnt = 0;
|
||||
while(1){
|
||||
coordpair_t c;
|
||||
movestate_t xst, yst;
|
||||
// now change data
|
||||
getModData(&c, &xst, &yst);
|
||||
struct timespec tnow;
|
||||
if(!curtime(&tnow) || (tcur = timefromstart()) < 0.) continue;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.encXposition.t = mountdata.encYposition.t = tnow;
|
||||
mountdata.encXposition.val = c.X + (drand48() - 0.5)*1e-6; // .2arcsec error
|
||||
mountdata.encYposition.val = c.Y + (drand48() - 0.5)*1e-6;
|
||||
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
|
||||
if(tcur - oldmt > Conf.MountReqInterval){
|
||||
oldmillis = mountdata.millis = (uint32_t)((tcur - tstart) * 1e3);
|
||||
mountdata.motYposition.t = mountdata.motXposition.t = tnow;
|
||||
if(xst == ST_MOVE)
|
||||
mountdata.motXposition.val = c.X + (c.X - mountdata.motXposition.val)*(drand48() - 0.5)/100.;
|
||||
//else
|
||||
// mountdata.motXposition.val = c.X;
|
||||
if(yst == ST_MOVE)
|
||||
mountdata.motYposition.val = c.Y + (c.Y - mountdata.motYposition.val)*(drand48() - 0.5)/100.;
|
||||
//else
|
||||
// mountdata.motYposition.val = c.Y;
|
||||
oldmt = tcur;
|
||||
}else mountdata.millis = oldmillis;
|
||||
chkModStopped(&Xprev, c.X, &xcnt, &mountdata.Xstate);
|
||||
chkModStopped(&Yprev, c.Y, &ycnt, &mountdata.Ystate);
|
||||
getXspeed(); getYspeed();
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(50);
|
||||
t0 = timefromstart();
|
||||
}
|
||||
}
|
||||
// data to get
|
||||
data_t d = {.buf = buf, .maxlen = sizeof(buf)};
|
||||
// cmd to send
|
||||
data_t *cmd_getstat = cmd2dat(CMD_GETSTAT);
|
||||
if(!cmd_getstat) goto failed;
|
||||
while(mntfd > -1 && errctr < MAX_ERR_CTR){
|
||||
// read data to status
|
||||
struct timespec tcur;
|
||||
if(!curtime(&tcur)) continue;
|
||||
// 80 milliseconds to get answer on GETSTAT
|
||||
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
|
||||
#ifdef EBUG
|
||||
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
|
||||
for(size_t i = 0; i < d.len; ++i) printf("%02X ", d.buf[i]);
|
||||
printf("\n");
|
||||
#endif
|
||||
++errctr; continue;
|
||||
}
|
||||
if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){
|
||||
DBG("BAD checksum of SSstat, need %d", status->checksum);
|
||||
++errctr; continue;
|
||||
}
|
||||
errctr = 0;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
// now change data
|
||||
SSconvstat(status, &mountdata, &tcur);
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
// allow writing & getters
|
||||
do{
|
||||
usleep(500);
|
||||
}while(timefromstart() - t0 < Conf.MountReqInterval);
|
||||
t0 = timefromstart();
|
||||
}
|
||||
data_free(&cmd_getstat);
|
||||
failed:
|
||||
if(mntfd > -1){
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// open device and return its FD or -1
|
||||
static int ttyopen(const char *path, speed_t speed){
|
||||
int fd = -1;
|
||||
struct termios2 tty;
|
||||
DBG("Try to open %s @ %d", path, speed);
|
||||
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
|
||||
DBG("Can't open device %s: %s", path, strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
if(ioctl(fd, TCGETS2, &tty)){
|
||||
DBG("Can't read TTY settings");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
|
||||
tty.c_iflag = 0; // don't do any changes in input stream
|
||||
tty.c_oflag = 0; // don't do any changes in output stream
|
||||
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
|
||||
tty.c_ispeed = speed;
|
||||
tty.c_ospeed = speed;
|
||||
//tty.c_cc[VMIN] = 0; // non-canonical mode
|
||||
//tty.c_cc[VTIME] = 5;
|
||||
if(ioctl(fd, TCSETS2, &tty)){
|
||||
DBG("Can't set TTY settings");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
|
||||
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
|
||||
// try to set exclusive
|
||||
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
|
||||
return fd;
|
||||
}
|
||||
|
||||
// return FALSE if failed
|
||||
int openEncoder(){
|
||||
if(Conf.RunModel) return TRUE;
|
||||
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
|
||||
if(Conf.SepEncoder == 1){ // only one device
|
||||
DBG("One device");
|
||||
if(encfd[0] > -1) close(encfd[0]);
|
||||
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
|
||||
if(encfd[0] < 0) return FALSE;
|
||||
encRtmout.tv_sec = 0;
|
||||
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes
|
||||
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
|
||||
close(encfd[0]);
|
||||
encfd[0] = -1;
|
||||
return FALSE;
|
||||
}
|
||||
}else if(Conf.SepEncoder == 2){
|
||||
DBG("Two devices!");
|
||||
const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath};
|
||||
for(int i = 0; i < 2; ++i){
|
||||
if(encfd[i] > -1) close(encfd[i]);
|
||||
encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
|
||||
if(encfd[i] < 0) return FALSE;
|
||||
}
|
||||
encRtmout.tv_sec = 0;
|
||||
encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
|
||||
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
|
||||
for(int i = 0; i < 2; ++i){
|
||||
close(encfd[i]);
|
||||
encfd[i] = -1;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
}else return FALSE;
|
||||
DBG("Encoder opened, thread started");
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// return FALSE if failed
|
||||
int openMount(){
|
||||
if(Conf.RunModel) goto create_thread;
|
||||
if(mntfd > -1) close(mntfd);
|
||||
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
|
||||
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
|
||||
if(mntfd < 0) return FALSE;
|
||||
DBG("mntfd=%d", mntfd);
|
||||
// clear buffer
|
||||
while(getmntbyte() > -1);
|
||||
/*int g = write(mntfd, "XXS\r", 4);
|
||||
DBG("Written %d", g);
|
||||
uint8_t buf[100];
|
||||
do{
|
||||
ssize_t l = read(mntfd, buf, 100);
|
||||
DBG("got %zd", l);
|
||||
}while(1);*/
|
||||
mntRtmout.tv_sec = 0;
|
||||
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
|
||||
create_thread:
|
||||
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
|
||||
DBG("Can't create mount thread");
|
||||
if(!Conf.RunModel){
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
DBG("Mount opened, thread started");
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// close all opened serial devices and quit threads
|
||||
void closeSerial(){
|
||||
// TODO: close devices in "model" mode too!
|
||||
if(Conf.RunModel) return;
|
||||
if(mntfd > -1){
|
||||
DBG("Cancel mount thread");
|
||||
pthread_cancel(mntthread);
|
||||
DBG("join mount thread");
|
||||
pthread_join(mntthread, NULL);
|
||||
DBG("close mount fd");
|
||||
close(mntfd);
|
||||
mntfd = -1;
|
||||
}
|
||||
if(encfd[0] > -1){
|
||||
DBG("Cancel encoder thread");
|
||||
pthread_cancel(encthread);
|
||||
DBG("join encoder thread");
|
||||
pthread_join(encthread, NULL);
|
||||
DBG("close encoder's fd");
|
||||
close(encfd[0]);
|
||||
encfd[0] = -1;
|
||||
if(Conf.SepEncoder == 2 && encfd[1] > -1){
|
||||
close(encfd[1]);
|
||||
encfd[1] = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// get fresh encoder information
|
||||
mcc_errcodes_t getMD(mountdata_t *d){
|
||||
if(!d) return MCC_E_BADFORMAT;
|
||||
pthread_mutex_lock(&datamutex);
|
||||
*d = mountdata;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val);
|
||||
//DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL));
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
void setStat(axis_status_t Xstate, axis_status_t Ystate){
|
||||
DBG("set x/y state to %d/%d", Xstate, Ystate);
|
||||
pthread_mutex_lock(&datamutex);
|
||||
mountdata.Xstate = Xstate;
|
||||
mountdata.Ystate = Ystate;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
}
|
||||
|
||||
// write-read without locking mutex (to be used inside other functions)
|
||||
static int wr(const data_t *out, data_t *in, int needeol){
|
||||
if((!out && !in) || mntfd < 0){
|
||||
DBG("Wrong arguments or no mount fd");
|
||||
return FALSE;
|
||||
}
|
||||
clrmntbuf();
|
||||
if(out){
|
||||
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
|
||||
DBG("written bytes not equal to need");
|
||||
return FALSE;
|
||||
}
|
||||
if(needeol){
|
||||
int g = write(mntfd, "\r", 1); // add EOL
|
||||
(void) g;
|
||||
}
|
||||
usleep(50000); // add little pause so that the idiot has time to swallow
|
||||
}
|
||||
if(!in) return TRUE;
|
||||
in->len = 0;
|
||||
for(size_t i = 0; i < in->maxlen; ++i){
|
||||
int b = getmntbyte();
|
||||
if(b < 0) break; // nothing to read -> go out
|
||||
in->buf[in->len++] = (uint8_t) b;
|
||||
}
|
||||
while(getmntbyte() > -1);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief MountWriteRead - write and read @ once (or only read/write)
|
||||
* @param out (o) - data to write or NULL if not need
|
||||
* @param in (i) - data to read or NULL if not need
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int MountWriteRead(const data_t *out, data_t *in){
|
||||
if(Conf.RunModel) return -1;
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
int ret = wr(out, in, 1);
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
// send binary data - without EOL
|
||||
int MountWriteReadRaw(const data_t *out, data_t *in){
|
||||
if(Conf.RunModel) return -1;
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
int ret = wr(out, in, 0);
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef EBUG
|
||||
static void logscmd(SSscmd *c){
|
||||
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
|
||||
printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits);
|
||||
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSscmd)-2)) printf("Checksum failed\n");
|
||||
else printf("Checksum OK\n");
|
||||
}
|
||||
static void loglcmd(SSlcmd *c){
|
||||
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
|
||||
printf("Xadder=%d, Yadder=%d, Xatime=%d, Yatime=%d\n", c->Xadder, c->Yadder, c->Xatime, c->Yatime);
|
||||
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSlcmd)-2)) printf("Checksum failed\n");
|
||||
else printf("Checksum OK\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
// send short/long binary command; return FALSE if failed
|
||||
static int bincmd(uint8_t *cmd, int len){
|
||||
if(Conf.RunModel) return FALSE;
|
||||
static data_t *dscmd = NULL, *dlcmd = NULL;
|
||||
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
|
||||
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
|
||||
int ret = FALSE;
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
// dummy buffer to clear trash in input
|
||||
//char ans[300];
|
||||
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||
if(len == sizeof(SSscmd)){
|
||||
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Short command");
|
||||
#ifdef EBUG
|
||||
logscmd((SSscmd*)cmd);
|
||||
#endif
|
||||
if(!wr(dscmd, NULL, 1)) goto rtn;
|
||||
}else if(len == sizeof(SSlcmd)){
|
||||
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||
DBG("Long command");
|
||||
#ifdef EBUG
|
||||
loglcmd((SSlcmd*)cmd);
|
||||
#endif
|
||||
if(!wr(dlcmd, NULL, 1)) goto rtn;
|
||||
}else{
|
||||
goto rtn;
|
||||
}
|
||||
data_t d;
|
||||
d.buf = cmd;
|
||||
d.len = d.maxlen = len;
|
||||
ret = wr(&d, NULL, 0);
|
||||
DBG("%s", ret ? "SUCCESS" : "FAIL");
|
||||
rtn:
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// short, long and config text-binary commands
|
||||
// return TRUE if OK
|
||||
int cmdS(SSscmd *cmd){
|
||||
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
|
||||
}
|
||||
int cmdL(SSlcmd *cmd){
|
||||
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
|
||||
}
|
||||
// rw == 1 to write, 0 to read
|
||||
int cmdC(SSconfig *conf, int rw){
|
||||
if(Conf.RunModel) return FALSE;
|
||||
static data_t *wcmd = NULL, *rcmd = NULL;
|
||||
int ret = FALSE;
|
||||
// dummy buffer to clear trash in input
|
||||
char ans[300];
|
||||
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||
if(!wcmd) wcmd = cmd2dat(CMD_PROGFLASH);
|
||||
if(!rcmd) rcmd = cmd2dat(CMD_DUMPFLASH);
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
if(rw){ // write
|
||||
if(!wr(wcmd, &a, 1)) goto rtn;
|
||||
}else{ // read
|
||||
data_t d;
|
||||
d.buf = (uint8_t *) conf;
|
||||
d.len = 0; d.maxlen = 0;
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("write command: %s", ret ? "TRUE" : "FALSE");
|
||||
if(!ret) goto rtn;
|
||||
// make a huge pause for stupid SSII
|
||||
usleep(100000);
|
||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
||||
if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
|
||||
// simplest checksum
|
||||
uint16_t sum = 0;
|
||||
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
||||
if(sum != conf->checksum){
|
||||
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
||||
ret = FALSE;
|
||||
goto rtn;
|
||||
}
|
||||
}
|
||||
rtn:
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
44
serial.h
Normal file
44
serial.h
Normal file
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sidservo.h"
|
||||
#include "ssii.h"
|
||||
|
||||
// magick starting sequence
|
||||
#define ENC_MAGICK (204)
|
||||
// encoder data sequence length
|
||||
#define ENC_DATALEN (13)
|
||||
// max error counter (when read() returns -1)
|
||||
#define MAX_ERR_CTR (100)
|
||||
|
||||
data_t *cmd2dat(const char *cmd);
|
||||
void data_free(data_t **x);
|
||||
int openEncoder();
|
||||
int openMount();
|
||||
void closeSerial();
|
||||
mcc_errcodes_t getMD(mountdata_t *d);
|
||||
void setStat(axis_status_t Xstate, axis_status_t Ystate);
|
||||
int MountWriteRead(const data_t *out, data_t *in);
|
||||
int MountWriteReadRaw(const data_t *out, data_t *in);
|
||||
int cmdS(SSscmd *cmd);
|
||||
int cmdL(SSlcmd *cmd);
|
||||
int cmdC(SSconfig *conf, int rw);
|
||||
void getXspeed();
|
||||
void getYspeed();
|
||||
261
sidservo.h
Normal file
261
sidservo.h
Normal file
@@ -0,0 +1,261 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* This file contains all need for external usage
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
// minimal serial speed of mount device
|
||||
#define MOUNT_BAUDRATE_MIN (1200)
|
||||
// max speed interval, seconds
|
||||
#define MCC_CONF_MAX_SPEEDINT (2.)
|
||||
// minimal speed interval in parts of EncoderReqInterval
|
||||
#define MCC_CONF_MIN_SPEEDC (3.)
|
||||
|
||||
|
||||
// error codes
|
||||
typedef enum{
|
||||
MCC_E_OK = 0, // all OK
|
||||
MCC_E_FATAL, // some fatal error
|
||||
MCC_E_BADFORMAT, // wrong arguments of function
|
||||
MCC_E_ENCODERDEV, // encoder device error or can't open
|
||||
MCC_E_MOUNTDEV, // mount device error or can't open
|
||||
MCC_E_FAILED, // failed to run command - protocol error
|
||||
MCC_E_AMOUNT // Just amount of errors
|
||||
} mcc_errcodes_t;
|
||||
|
||||
typedef struct{
|
||||
double P, I, D;
|
||||
} PIDpar_t;
|
||||
|
||||
typedef struct{
|
||||
char* MountDevPath; // path to mount device
|
||||
int MountDevSpeed; // serial speed
|
||||
char* EncoderDevPath; // path to encoder device
|
||||
int EncoderDevSpeed; // serial speed
|
||||
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
||||
char* EncoderXDevPath; // paths to new controller devices
|
||||
char* EncoderYDevPath;
|
||||
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
|
||||
double MountReqInterval; // interval between subsequent mount requests (seconds)
|
||||
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
|
||||
double EncoderSpeedInterval; // interval between speed calculations
|
||||
int RunModel; // == 1 if you want to use model instead of real mount
|
||||
double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared)
|
||||
double PIDRefreshDt; // normal PID refresh interval
|
||||
double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps)
|
||||
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
|
||||
PIDpar_t XPIDV;
|
||||
PIDpar_t YPIDC;
|
||||
PIDpar_t YPIDV;
|
||||
double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
|
||||
double MaxFinePointingErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
|
||||
double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
|
||||
int XEncZero; // encoders' zero position
|
||||
int YEncZero;
|
||||
} conf_t;
|
||||
|
||||
// coordinates/speeds in degrees or d/s: X, Y
|
||||
typedef struct{
|
||||
double X; double Y;
|
||||
} coordpair_t;
|
||||
|
||||
// coordinate/speed and time of last measurement
|
||||
typedef struct{
|
||||
double val;
|
||||
struct timespec t;
|
||||
} coordval_t;
|
||||
|
||||
typedef struct{
|
||||
coordval_t X;
|
||||
coordval_t Y;
|
||||
} coordval_pair_t;
|
||||
|
||||
// data to read/write
|
||||
typedef struct{
|
||||
uint8_t *buf; // data buffer
|
||||
size_t len; // its length
|
||||
size_t maxlen; // maximal buffer size
|
||||
} data_t;
|
||||
|
||||
typedef struct{
|
||||
uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction
|
||||
uint8_t motpolarity :1; // If 1, the motor polarity is reversed
|
||||
uint8_t encrev :1; // If 1, the axis encoder is reversed
|
||||
uint8_t dragtrack :1; // If 1, we are in computerless Drag and Track mode
|
||||
uint8_t trackplat :1; // If 1, we are in the tracking platform mode
|
||||
uint8_t handpaden :1; // If 1, hand paddle is enabled
|
||||
uint8_t newpad :1; // If 1, hand paddle is compatible with New hand paddle, which allows slewing in two directions and guiding
|
||||
uint8_t guidemode :1; // If 1, we are in guide mode. The pan rate is added or subtracted from the current tracking rate
|
||||
} xbits_t;
|
||||
|
||||
typedef struct{
|
||||
uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction
|
||||
uint8_t motpolarity :1; // If 1, the motor polarity is reversed
|
||||
uint8_t encrev :1; // If 1, the axis encoder is reversed
|
||||
/* If 1, we are in computerless Slew and Track mode
|
||||
(no clutches; use handpad to slew; must be in Drag and Track mode too) */
|
||||
uint8_t slewtrack :1;
|
||||
uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
|
||||
uint8_t digin :3; // Digital input from radio handpad receiver
|
||||
} ybits_t;
|
||||
|
||||
typedef struct{
|
||||
xbits_t XBits;
|
||||
ybits_t YBits;
|
||||
uint8_t ExtraBits;
|
||||
uint16_t ain0;
|
||||
uint16_t ain1;
|
||||
} extradata_t;
|
||||
|
||||
typedef enum{
|
||||
AXIS_STOPPED,
|
||||
AXIS_SLEWING,
|
||||
AXIS_POINTING,
|
||||
AXIS_GUIDING,
|
||||
AXIS_ERROR,
|
||||
} axis_status_t;
|
||||
|
||||
typedef struct{
|
||||
axis_status_t Xstate;
|
||||
axis_status_t Ystate;
|
||||
coordval_t motXposition;
|
||||
coordval_t motYposition;
|
||||
coordval_t encXposition;
|
||||
coordval_t encYposition;
|
||||
coordval_t encXspeed; // once per <config> s
|
||||
coordval_t encYspeed;
|
||||
uint8_t keypad;
|
||||
extradata_t extradata;
|
||||
uint32_t millis;
|
||||
double temperature;
|
||||
double voltage;
|
||||
} mountdata_t;
|
||||
|
||||
typedef struct{
|
||||
double Xmot; // 0 X motor position (rad)
|
||||
double Xspeed; // 4 X speed (rad/s)
|
||||
double Ymot; // 8
|
||||
double Yspeed; // 12
|
||||
uint8_t xychange; // 16 change Xbits/Ybits value
|
||||
uint8_t XBits; // 17
|
||||
uint8_t YBits; // 18
|
||||
} short_command_t; // short command
|
||||
|
||||
typedef struct{
|
||||
double Xmot; // 0 X motor position (rad)
|
||||
double Xspeed; // 4 X speed (rad/s)
|
||||
double Ymot; // 8
|
||||
double Yspeed; // 12
|
||||
double Xadder; // 16 - X adder (rad/s)
|
||||
double Yadder; // 20
|
||||
double Xatime; // 24 X adder time, sec
|
||||
double Yatime; // 28
|
||||
} long_command_t; // long command
|
||||
|
||||
// hardware axis configuration
|
||||
typedef struct{
|
||||
double accel; // Default Acceleration, rad/s^2
|
||||
double backlash; // Backlash (???)
|
||||
double errlimit; // Error Limit, rad
|
||||
double propgain; // Proportional Gain (???)
|
||||
double intgain; // Integral Gain (???)
|
||||
double derivgain; // Derivative Gain (???)
|
||||
double outplimit; // Output Limit, percent (0..100)
|
||||
double currlimit; // Current Limit (A)
|
||||
double intlimit; // Integral Limit (???)
|
||||
// these params are taken from mount by text commands (don't save negative values - better save these marks in xybits
|
||||
double motor_stepsperrev;// encoder's steps per revolution: motor and axis
|
||||
double axis_stepsperrev; // negative sign of these values means reverse direction
|
||||
} __attribute__((packed)) axis_config_t;
|
||||
|
||||
// hardware configuration
|
||||
typedef struct{
|
||||
axis_config_t Xconf;
|
||||
xbits_t xbits;
|
||||
axis_config_t Yconf;
|
||||
ybits_t ybits;
|
||||
uint8_t address;
|
||||
double eqrate; // Equatorial Rate (???)
|
||||
double eqadj; // Equatorial UpDown adjust (???)
|
||||
double trackgoal; // Tracking Platform Goal (???)
|
||||
double latitude; // Latitude, rad
|
||||
uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev
|
||||
uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev
|
||||
uint32_t Ymetpr; // Azm Motor Ticks Per Rev
|
||||
uint32_t Xmetpr; // Alt Motor Ticks Per Rev
|
||||
double Xslewrate; // Alt/Dec Slew Rate (rad/s)
|
||||
double Yslewrate; // Azm/RA Slew Rate (rad/s)
|
||||
double Xpanrate; // Alt/Dec Pan Rate (rad/s)
|
||||
double Ypanrate; // Azm/RA Pan Rate (rad/s)
|
||||
double Xguiderate; // Alt/Dec Guide Rate (rad/s)
|
||||
double Yguiderate; // Azm/RA Guide Rate (rad/s)
|
||||
uint32_t baudrate; // Baud Rate (baud)
|
||||
double locsdeg; // Local Search Degrees (rad)
|
||||
double locsspeed; // Local Search Speed (rad/s)
|
||||
double backlspd; // Backlash speed (rad/s)
|
||||
} hardware_configuration_t;
|
||||
|
||||
/* flags for slew function
|
||||
typedef struct{
|
||||
uint32_t slewNguide : 1; // ==1 to guide after slewing
|
||||
} slewflags_t;
|
||||
*/
|
||||
// mount class
|
||||
typedef struct{
|
||||
// TODO: on init/quit clear all XY-bits to default`
|
||||
mcc_errcodes_t (*init)(conf_t *c); // init device
|
||||
void (*quit)(); // deinit
|
||||
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
||||
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
|
||||
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target);
|
||||
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
|
||||
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
|
||||
mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
|
||||
mcc_errcodes_t (*stop)(); // stop
|
||||
mcc_errcodes_t (*emergStop)(); // emergency stop
|
||||
mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command
|
||||
mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command
|
||||
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
||||
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
|
||||
int (*currentT)(struct timespec *t); // current time
|
||||
double (*timeFromStart)(); // amount of seconds from last init
|
||||
double (*timeDiff)(const struct timespec *time1, const struct timespec *time0); // difference of times
|
||||
double (*timeDiff0)(const struct timespec *time1); // difference between current time and last init time
|
||||
mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
|
||||
mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//-
|
||||
mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
|
||||
} mount_t;
|
||||
|
||||
extern mount_t Mount;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
10
sidservo.pc.in
Normal file
10
sidservo.pc.in
Normal file
@@ -0,0 +1,10 @@
|
||||
prefix=@CMAKE_INSTALL_PREFIX@
|
||||
exec_prefix=${prefix}
|
||||
libdir=${exec_prefix}/lib
|
||||
includedir=${prefix}/include
|
||||
|
||||
Name: @PROJ@
|
||||
Description: library for managing SiderealServo II based equatorial mount
|
||||
Version: @VERSION@
|
||||
Libs: -L${libdir} -l@PROJ@
|
||||
Cflags: -I${includedir}
|
||||
229
ssii.c
Normal file
229
ssii.c
Normal file
@@ -0,0 +1,229 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <ctype.h>
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "main.h"
|
||||
#include "serial.h"
|
||||
#include "ssii.h"
|
||||
|
||||
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
|
||||
// defaults until read from controller
|
||||
double X_MOT_STEPSPERREV = 13312000.,
|
||||
Y_MOT_STEPSPERREV = 17578668.,
|
||||
X_ENC_STEPSPERREV = 67108864.,
|
||||
Y_ENC_STEPSPERREV = 67108864.;
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len){
|
||||
uint16_t checksum = 0;
|
||||
for(int i = 0; i < len; i++){
|
||||
//DBG("data[%d]=0x%X", i, *buf);
|
||||
checksum += *buf++;
|
||||
}
|
||||
checksum ^= 0xFF00; // invert high byte
|
||||
//DBG("Checksum of %d bytes: 0x%04x", len, checksum);
|
||||
return checksum;
|
||||
}
|
||||
|
||||
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
|
||||
static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
|
||||
if(*prev == INT32_MAX){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("START");
|
||||
}else if(*stat != AXIS_STOPPED){
|
||||
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
|
||||
*stat = AXIS_STOPPED;
|
||||
DBG("AXIS stopped");
|
||||
}
|
||||
}else if(*prev != cur){
|
||||
DBG("AXIS moving");
|
||||
*nstopped = 0;
|
||||
}
|
||||
*prev = cur;
|
||||
}
|
||||
// check for stopped/pointing states
|
||||
static void ChkStopped(const SSstat *s, mountdata_t *m){
|
||||
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
|
||||
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
|
||||
chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
|
||||
chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SSconvstat - convert stat from SSII format to human
|
||||
* @param s (i) - just read data
|
||||
* @param m (o) - output
|
||||
* @param t - measurement time
|
||||
*/
|
||||
void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
|
||||
if(!s || !m || !t) return;
|
||||
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
||||
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
||||
ChkStopped(s, m);
|
||||
m->motXposition.t = m->motYposition.t = *t;
|
||||
// fill encoder data from here, as there's no separate enc thread
|
||||
if(!Conf.SepEncoder){
|
||||
m->encXposition.val = Xenc2rad(s->Xenc);
|
||||
DBG("encx: %g", m->encXposition.val);
|
||||
m->encYposition.val = Yenc2rad(s->Yenc);
|
||||
m->encXposition.t = m->encYposition.t = *t;
|
||||
getXspeed(); getYspeed();
|
||||
}
|
||||
m->keypad = s->keypad;
|
||||
m->extradata.ExtraBits = s->ExtraBits;
|
||||
m->extradata.ain0 = s->ain0;
|
||||
m->extradata.ain1 = s->ain1;
|
||||
m->extradata.XBits = s->XBits;
|
||||
m->extradata.YBits = s->YBits;
|
||||
m->millis = s->millis;
|
||||
m->voltage = (double)s->voltage / 10.;
|
||||
m->temperature = ((double)s->tF - 32.) * 5. / 9.;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SStextcmd - send simple text command to mount and return answer
|
||||
* @param cmd (i) - command to send
|
||||
* @param answer (o) - answer (or NULL)
|
||||
* @return
|
||||
*/
|
||||
int SStextcmd(const char *cmd, data_t *answer){
|
||||
if(!cmd){
|
||||
DBG("try to send empty command");
|
||||
return FALSE;
|
||||
}
|
||||
data_t d;
|
||||
d.buf = (uint8_t*) cmd;
|
||||
d.len = d.maxlen = strlen(cmd);
|
||||
//DBG("send %zd bytes: %s", d.len, d.buf);
|
||||
return MountWriteRead(&d, answer);
|
||||
}
|
||||
|
||||
// the same as SStextcmd, but not adding EOL - send raw 'cmd'
|
||||
int SSrawcmd(const char *cmd, data_t *answer){
|
||||
if(!cmd){
|
||||
DBG("try to send empty command");
|
||||
return FALSE;
|
||||
}
|
||||
data_t d;
|
||||
d.buf = (uint8_t*) cmd;
|
||||
d.len = d.maxlen = strlen(cmd);
|
||||
//DBG("send %zd bytes: %s", d.len, d.buf);
|
||||
return MountWriteReadRaw(&d, answer);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SSgetint - send text command and return integer answer
|
||||
* @param cmd (i) - command to send
|
||||
* @param ans (o) - intval (INT64_MAX if error)
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int SSgetint(const char *cmd, int64_t *ans){
|
||||
if(!cmd || !ans) return FALSE;
|
||||
uint8_t buf[64];
|
||||
data_t d = {.buf = buf, .len = 0, .maxlen = 64};
|
||||
if(!SStextcmd(cmd, &d)) return FALSE;
|
||||
int64_t retval = INT64_MAX;
|
||||
if(d.len > 1){
|
||||
char *ptr = (char*) buf;
|
||||
size_t i = 0;
|
||||
for(; i < d.len; ++i){
|
||||
if(isdigit(*ptr)) break;
|
||||
++ptr;
|
||||
}
|
||||
if(i < d.len) retval = atol(ptr);
|
||||
}
|
||||
DBG("read int: %" PRIi64, retval);
|
||||
*ans = retval;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SSsetterI - integer setter
|
||||
* @param cmd - command to send
|
||||
* @param ival - value
|
||||
* @return false if failed
|
||||
*/
|
||||
int SSsetterI(const char *cmd, int32_t ival){
|
||||
char buf[128];
|
||||
snprintf(buf, 127, "%s%" PRIi32, cmd, ival);
|
||||
return SStextcmd(buf, NULL);
|
||||
}
|
||||
|
||||
int SSstop(int emerg){
|
||||
int i = 0;
|
||||
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
|
||||
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
|
||||
for(; i < 10; ++i){
|
||||
if(!SStextcmd(cmdx, NULL)) continue;
|
||||
if(SStextcmd(cmdy, NULL)) break;
|
||||
}
|
||||
if(i == 10) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// update motors' positions due to encoders'
|
||||
mcc_errcodes_t updateMotorPos(){
|
||||
mountdata_t md = {0};
|
||||
if(Conf.RunModel) return MCC_E_OK;
|
||||
double t0 = timefromstart(), t = 0.;
|
||||
struct timespec curt;
|
||||
DBG("start @ %g", t0);
|
||||
do{
|
||||
t = timefromstart();
|
||||
if(!curtime(&curt)){
|
||||
usleep(10000);
|
||||
continue;
|
||||
}
|
||||
//DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
|
||||
if(MCC_E_OK == getMD(&md)){
|
||||
if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
|
||||
DBG("Just started? t-t0 = %g!", t - t0);
|
||||
usleep(10000);
|
||||
continue;
|
||||
}
|
||||
if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
|
||||
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
|
||||
mcc_errcodes_t OK = MCC_E_OK;
|
||||
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
|
||||
DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
|
||||
DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
|
||||
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
|
||||
DBG("Xpos sync failed!");
|
||||
OK = MCC_E_FAILED;
|
||||
}else DBG("Xpos sync OK, Dt=%g", t - t0);
|
||||
}
|
||||
if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
|
||||
DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
|
||||
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
|
||||
DBG("Ypos sync failed!");
|
||||
OK = MCC_E_FAILED;
|
||||
}else DBG("Ypos sync OK, Dt=%g", t - t0);
|
||||
}
|
||||
if(MCC_E_OK == OK){
|
||||
DBG("Encoders synced");
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
DBG("NO DATA; dt = %g", t - t0);
|
||||
}while(t - t0 < 2.);
|
||||
return MCC_E_FATAL;
|
||||
}
|
||||
350
ssii.h
Normal file
350
ssii.h
Normal file
@@ -0,0 +1,350 @@
|
||||
/*
|
||||
* This file is part of the SSII project.
|
||||
* Copyright 2022 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* This file contains stuff for sidereal-servo specific protocol
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
/*********** base commands ***********/
|
||||
// get/set X/Y in motsteps
|
||||
#define CMD_MOTX "X"
|
||||
#define CMD_MOTY "Y"
|
||||
// set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags)
|
||||
#define CMD_MOTXYS "S"
|
||||
// reset current motor position to given value (and stop, if moving)
|
||||
#define CMD_MOTXSET "XF"
|
||||
#define CMD_MOTYSET "YF"
|
||||
// acceleration (per each loop, max: 3900)
|
||||
#define CMD_MOTXACCEL "XR"
|
||||
#define CMD_MOTYACCEL "YR"
|
||||
// PID regulator:
|
||||
// P: 0..32767
|
||||
#define CMD_PIDPX "XP"
|
||||
#define CMD_PIDPY "YP"
|
||||
// I: 0..32767
|
||||
#define CMD_PIDIX "XI"
|
||||
#define CMD_PIDIY "YI"
|
||||
// limit of I (doesn't work): 0:24000 (WTF???)
|
||||
#define CMD_PIDILX "XL"
|
||||
#define CMD_PIDILY "YL"
|
||||
// D: 0..32767
|
||||
#define CMD_PIDDX "XD"
|
||||
#define CMD_PIDDY "YD"
|
||||
// current position error
|
||||
#define CMD_POSERRX "XE"
|
||||
#define CMD_POSERRY "YE"
|
||||
// max position error limit (X: E#, Y: e#)
|
||||
#define CMD_POSERRLIMX "XEL"
|
||||
#define CMD_POSERRLIMY "YEL"
|
||||
// current PWM output: 0..255 (or set max PWM out)
|
||||
#define CMD_PWMOUTX "XO"
|
||||
#define CMD_PWMOUTY "YO"
|
||||
// motor current *100 (or set current limit): 0..240
|
||||
#define CMD_MOTCURNTX "XC"
|
||||
#define CMD_MOTCURNTY "YC"
|
||||
// change axis to Manual mode and set the PWM output: -255:255
|
||||
#define CMD_MANUALPWMX "XM"
|
||||
#define CMD_MANUALPWMY "YM"
|
||||
// change axis to Auto mode
|
||||
#define CMD_AUTOX "XA"
|
||||
#define CMD_AUTOY "YA"
|
||||
// get positioin in encoders' ticks or reset it to given value
|
||||
#define CMD_ENCX "XZ"
|
||||
#define CMD_ENCY "YZ"
|
||||
// get/set speed (geter x: S#, getter y: s#)
|
||||
#define CMD_SPEEDX "XS"
|
||||
#define CMD_SPEEDY "YS"
|
||||
// normal stop X/Y
|
||||
#define CMD_STOPX "XN"
|
||||
#define CMD_STOPY "YN"
|
||||
// lower speed -> drag&track or slew&track
|
||||
#define CMD_STOPTRACKX "XNT"
|
||||
#define CMD_STOPTRACKY "YNT"
|
||||
// emergency stop
|
||||
#define CMD_EMSTOPX "XG"
|
||||
#define CMD_EMSTOPY "YG"
|
||||
// get/set X/Ybits
|
||||
#define CMD_BITSX "XB"
|
||||
#define CMD_BITSY "YB"
|
||||
|
||||
/*********** getters/setters without "Y" variant ***********/
|
||||
// get handpad status (decimal)
|
||||
#define CMD_HANDPAD "XK"
|
||||
// get TCPU (deg F)
|
||||
#define CMD_TCPU "XH"
|
||||
// get firmware version *10
|
||||
#define CMD_FIRMVER "XV"
|
||||
// get motor voltage *10
|
||||
#define CMD_MOTVOLTAGE "XJ"
|
||||
// get/set current CPU clock (milliseconds)
|
||||
#define CMD_MILLIS "XY"
|
||||
// reset servo
|
||||
#define CMD_RESET "XQ"
|
||||
// clear to factory defaults
|
||||
#define CMD_CLRDEFAULTS "XU"
|
||||
// save configuration to flash ROM
|
||||
#define CMD_WRITEFLASH "XW"
|
||||
// read config from flash to RAM
|
||||
#define CMD_READFLASH "XT"
|
||||
// write to flash following full config (128 bytes + 2 bytes of checksum)
|
||||
#define CMD_PROGFLASH "FC"
|
||||
// read configuration (-//-)
|
||||
#define CMD_DUMPFLASH "SC"
|
||||
// get serial number
|
||||
#define CMD_SERIAL "YV"
|
||||
|
||||
/*********** extended commands ***********/
|
||||
// get/set latitute
|
||||
#define CMD_LATITUDE "XXL"
|
||||
// getters/setters of motor's encoders per rev
|
||||
#define CMD_MEPRX "XXU"
|
||||
#define CMD_MEPRY "XXV"
|
||||
// -//- axis encoders
|
||||
#define CMD_AEPRX "XXT"
|
||||
#define CMD_AEPRY "XXZ"
|
||||
// get/set slew rate
|
||||
#define CMD_SLEWRATEX "XXA"
|
||||
#define CMD_SLEWRATEY "XXB"
|
||||
// get/set pan rate
|
||||
#define CMD_PANRATEX "XXC"
|
||||
#define CMD_PANRATEY "XXD"
|
||||
// get/set platform tracking rate
|
||||
#define CMD_PLATRATE "XXE"
|
||||
// get/set platform up/down adjuster
|
||||
#define CMD_PLATADJ "XXF"
|
||||
// get/set platform goal
|
||||
#define CMD_PLATGOAL "XXG"
|
||||
// get/set guide rate
|
||||
#define CMD_GUIDERATEX "XXH"
|
||||
#define CMD_GUIDERATEY "XXI"
|
||||
// get/set picservo timeout (seconds)
|
||||
#define CMD_PICTMOUT "XXJ"
|
||||
// get/set digital outputs of radio handpad
|
||||
#define CMD_RADIODIGOUT "XXQ"
|
||||
// get/set argo navis mode
|
||||
#define CMD_ARGONAVIS "XXN"
|
||||
// get/set local search distance
|
||||
#define CMD_LOSCRCHDISTX "XXM"
|
||||
#define CMD_LOSCRCHDISTY "XXO"
|
||||
// get/set backlash
|
||||
#define CMD_BACKLASHX "XXO"
|
||||
#define CMD_BACKLASHY "XXP"
|
||||
|
||||
|
||||
// get binary data of all statistics
|
||||
#define CMD_GETSTAT "XXS"
|
||||
// send short command
|
||||
#define CMD_SHORTCMD "XXR"
|
||||
// send long command
|
||||
#define CMD_LONGCMD "YXR"
|
||||
|
||||
|
||||
/*********** special ***********/
|
||||
// exit ASCII checksum mode
|
||||
#define CMD_EXITACM "YXY0\r\xb8"
|
||||
// controller status:
|
||||
// X# Y# XZ# YZ# XC# YC# V# T# X[AM] Y[AM] K#
|
||||
// X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits
|
||||
#define CMD_GETSTATTEXT "\r"
|
||||
|
||||
// Loop freq
|
||||
#define SITECH_LOOP_FREQUENCY (1953.)
|
||||
|
||||
// amount of consequent same coordinates to detect stop
|
||||
#define MOTOR_STOPPED_CNT (19)
|
||||
|
||||
// replace macros with global variables inited when config read
|
||||
extern int X_ENC_ZERO, Y_ENC_ZERO;
|
||||
extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV;
|
||||
|
||||
// TODO: take it from settings?
|
||||
// steps per revolution (SSI - x4 - for SSI)
|
||||
// -> hwconf.Xconf.mot/enc_stepsperrev
|
||||
//#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||
// 13312000 / 4 = 3328000
|
||||
//#define X_MOT_STEPSPERREV (3328000.)
|
||||
//#define Y_MOT_STEPSPERREV_SSI (17578668.)
|
||||
// 17578668 / 4 = 4394667
|
||||
//#define Y_MOT_STEPSPERREV (4394667.)
|
||||
|
||||
// encoder per revolution
|
||||
//#define X_ENC_STEPSPERREV (67108864.)
|
||||
//#define Y_ENC_STEPSPERREV (67108864.)
|
||||
// encoder zero position
|
||||
// -> conf.XEncZero/YEncZero
|
||||
//#define X_ENC_ZERO (61245239)
|
||||
//#define Y_ENC_ZERO (36999830)
|
||||
// encoder reversed (no: +1) -> sign of ...stepsperrev
|
||||
//#define X_ENC_SIGN (-1.)
|
||||
//#define Y_ENC_SIGN (-1.)
|
||||
|
||||
|
||||
// encoder position to radians and back
|
||||
#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
|
||||
#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
|
||||
#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
|
||||
#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
|
||||
|
||||
// convert angle in radians to +-pi
|
||||
static inline __attribute__((always_inline)) double ang2half(double ang){
|
||||
ang = fmod(ang, 2.*M_PI);
|
||||
if(ang < -M_PI) ang += 2.*M_PI;
|
||||
else if(ang > M_PI) ang -= 2.*M_PI;
|
||||
return ang;
|
||||
}
|
||||
// convert to only positive: 0..2pi
|
||||
static inline __attribute__((always_inline)) double ang2full(double ang){
|
||||
ang = fmod(ang, 2.*M_PI);
|
||||
if(ang < 0.) ang += 2.*M_PI;
|
||||
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
|
||||
return ang;
|
||||
}
|
||||
|
||||
// motor position to radians and back
|
||||
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
|
||||
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
|
||||
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
|
||||
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
|
||||
// motor speed in rad/s and back
|
||||
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
|
||||
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
|
||||
// motor acceleration -//-
|
||||
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
|
||||
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
|
||||
|
||||
// adder time to seconds vice versa
|
||||
#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY))
|
||||
#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY))
|
||||
|
||||
// encoder's tolerance (ticks)
|
||||
#define YencTOL (25.)
|
||||
#define XencTOL (25.)
|
||||
|
||||
|
||||
// all need data in one
|
||||
typedef struct{ // 41 bytes
|
||||
uint8_t ctrlAddr; // 0 a8 + controller address
|
||||
int32_t Xmot; // 1 Dec/HA motor position
|
||||
int32_t Ymot; // 5
|
||||
int32_t Xenc; // 9 Dec/HA encoder position
|
||||
int32_t Yenc; // 13
|
||||
uint8_t keypad; // 17 keypad status
|
||||
xbits_t XBits; // 18
|
||||
ybits_t YBits; // 19
|
||||
uint8_t ExtraBits; // 20
|
||||
uint16_t ain0; // 21 analog inputs
|
||||
uint16_t ain1; // 23
|
||||
uint32_t millis; // 25 milliseconds clock
|
||||
int8_t tF; // 29 temperature (degF)
|
||||
uint8_t voltage; // 30 input voltage *10 (RA worm phase?)
|
||||
uint32_t XLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change
|
||||
uint32_t YLast; // 35 Az/RA motor location at last Az/RA scope encoder location change
|
||||
uint16_t checksum; // 39 checksum, H inverted
|
||||
}__attribute__((packed)) SSstat;
|
||||
|
||||
typedef struct{
|
||||
int32_t Xmot; // 0 X motor position
|
||||
int32_t Xspeed; // 4 X speed
|
||||
int32_t Ymot; // 8
|
||||
int32_t Yspeed; // 12
|
||||
uint8_t xychange; // 16 change Xbits/Ybits value
|
||||
uint8_t XBits; // 17
|
||||
uint8_t YBits; // 18
|
||||
uint16_t checksum; // 19
|
||||
} __attribute__((packed)) SSscmd; // short command
|
||||
|
||||
typedef struct{
|
||||
int32_t Xmot; // 0 X motor position
|
||||
int32_t Xspeed; // 4 X speed
|
||||
int32_t Ymot; // 8
|
||||
int32_t Yspeed; // 12
|
||||
int32_t Xadder; // 16 - X adder
|
||||
int32_t Yadder; // 20
|
||||
int32_t Xatime; // 24 X adder time (1953 == 1s)
|
||||
int32_t Yatime; // 28
|
||||
uint16_t checksum; // 32
|
||||
} __attribute__((packed)) SSlcmd; // long command
|
||||
|
||||
typedef struct{
|
||||
uint32_t accel; // Default Acceleration (0..3900)
|
||||
uint32_t backlash; // Backlash (???)
|
||||
uint16_t errlimit; // Error Limit (0..32767)
|
||||
uint16_t propgain; // Proportional Gain (0..32767)
|
||||
uint16_t intgain; // Integral Gain (0..32767)
|
||||
uint16_t derivgain; // Derivative Gain (0..32767)
|
||||
uint16_t outplimit; // Output Limit, 0xFF = 100.0 (0..255)
|
||||
uint16_t currlimit; // Current Limit * 100 (0..240)
|
||||
uint16_t intlimit; // Integral Limit (0..24000)
|
||||
} __attribute__((packed)) AxeConfig;
|
||||
|
||||
typedef struct{
|
||||
AxeConfig Xconf;
|
||||
xbits_t xbits;
|
||||
uint8_t unused0;
|
||||
AxeConfig Yconf;
|
||||
ybits_t ybits;
|
||||
uint8_t unused1;
|
||||
uint8_t address;
|
||||
uint8_t unused2;
|
||||
uint32_t eqrate; // Equatorial Rate (platform?) (???)
|
||||
int32_t eqadj; // Equatorial UpDown adjust (???)
|
||||
uint32_t trackgoal; // Tracking Platform Goal (???)
|
||||
uint16_t latitude; // Latitude * 100, MSB FIRST!!
|
||||
uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Ymetpr; // Azm Motor Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Xmetpr; // Alt Motor Ticks Per Rev, MSB FIRST!!
|
||||
int32_t Xslewrate; // Alt/Dec Slew Rate (rates are negative in "through the pole" mode!!!)
|
||||
int32_t Yslewrate; // Azm/RA Slew Rate
|
||||
int32_t Xpanrate; // Alt/Dec Pan Rate
|
||||
int32_t Ypanrate; // Azm/RA Pan Rate
|
||||
int32_t Xguiderate; // Alt/Dec Guide Rate
|
||||
int32_t Yguiderate; // Azm/RA Guide Rate
|
||||
uint8_t unknown0; // R/A PEC Auto Sync Enable (if low bit = 1), or PicServo Comm Timeout??
|
||||
uint8_t unused3;
|
||||
uint8_t baudrate; // Baud Rate??
|
||||
uint8_t unused4;
|
||||
uint8_t specmode; // 1 = Enable Argo Navis, 2 = Enable Sky Commander
|
||||
uint8_t unused5;
|
||||
uint32_t locsdeg; // Local Search Degrees * 100
|
||||
uint32_t locsspeed; // Local Search Speed, arcsec per sec (???)
|
||||
uint32_t backlspd; // Backlash speed
|
||||
uint32_t pecticks; // RA/Azm PEC Ticks
|
||||
uint16_t unused6;
|
||||
uint16_t checksum;
|
||||
} __attribute__((packed)) SSconfig;
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timespec *t);
|
||||
int SStextcmd(const char *cmd, data_t *answer);
|
||||
int SSrawcmd(const char *cmd, data_t *answer);
|
||||
int SSgetint(const char *cmd, int64_t *ans);
|
||||
int SSsetterI(const char *cmd, int32_t ival);
|
||||
int SSstop(int emerg);
|
||||
mcc_errcodes_t updateMotorPos();
|
||||
Reference in New Issue
Block a user