From 4dcb7d44440b14f31481d4302aa5a90beda0da85 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Thu, 12 Mar 2026 08:49:36 +0300 Subject: [PATCH] Move the library to the separate repository --- .qtcreator/libsidservo.creator.user | 218 +++++ .qtcreator/libsidservo.creator.user.cf63021 | 218 +++++ CMakeLists.txt | 87 ++ PID.c | 221 +++++ PID.h | 40 + PID_test.deprecated/Dramp.c | 141 +++ PID_test.deprecated/Dramp.h | 23 + PID_test.deprecated/Makefile | 57 ++ PID_test.deprecated/Sramp.c | 27 + PID_test.deprecated/Sramp.h | 23 + PID_test.deprecated/Tramp.c | 223 +++++ PID_test.deprecated/Tramp.h | 23 + PID_test.deprecated/main.c | 243 +++++ PID_test.deprecated/moving.c | 105 +++ PID_test.deprecated/moving.h | 70 ++ PID_test.deprecated/moving_model.cflags | 1 + PID_test.deprecated/moving_model.config | 4 + PID_test.deprecated/moving_model.creator | 1 + PID_test.deprecated/moving_model.creator.user | 221 +++++ .../moving_model.creator.user.7bd84e3 | 184 ++++ PID_test.deprecated/moving_model.cxxflags | 1 + PID_test.deprecated/moving_model.files | 10 + PID_test.deprecated/moving_model.includes | 0 PID_test.deprecated/moving_private.h | 26 + PID_test.deprecated/plot | 4 + PID_test.deprecated/plot.cont | 8 + PID_test.deprecated/plot_jpg | 6 + PID_test.deprecated/plot_pdf | 5 + PID_test.deprecated/plotacc | 4 + PID_test.deprecated/ploterr | 4 + PID_test.deprecated/ploterr.cont | 6 + PID_test.deprecated/ploterr_jpg | 5 + PID_test/Makefile | 57 ++ PID_test/PID.c | 64 ++ PID_test/PID.h | 39 + PID_test/Tramp.c | 223 +++++ PID_test/Tramp.h | 23 + PID_test/main.c | 222 +++++ PID_test/moving.c | 89 ++ PID_test/moving.h | 62 ++ PID_test/moving_model.cflags | 1 + PID_test/moving_model.config | 4 + PID_test/moving_model.creator | 1 + PID_test/moving_model.creator.user | 221 +++++ PID_test/moving_model.creator.user.7bd84e3 | 184 ++++ PID_test/moving_model.cxxflags | 1 + PID_test/moving_model.files | 12 + PID_test/moving_model.includes | 0 PID_test/moving_private.h | 26 + PID_test/plot | 4 + PID_test/plot.cont | 8 + PID_test/plot_jpg | 6 + PID_test/plot_pdf | 5 + PID_test/plotacc | 4 + PID_test/ploterr | 4 + PID_test/ploterr.cont | 6 + PID_test/ploterr_jpg | 5 + TODO | 3 + examples/CMakeLists.txt | 14 + examples/Readme.md | 28 + examples/SSIIconf.c | 163 ++++ examples/conf.c | 128 +++ examples/conf.h | 28 + examples/dump.c | 197 ++++ examples/dump.h | 30 + examples/dumpmoving.c | 105 +++ examples/dumpmoving_dragNtrack.c | 228 +++++ examples/dumpmoving_scmd.c | 177 ++++ examples/dumpswing.c | 190 ++++ examples/goto.c | 145 +++ examples/scmd_traectory.c | 188 ++++ examples/servo.conf | 25 + examples/simpleconv.h | 29 + examples/traectories.c | 136 +++ examples/traectories.h | 32 + libsidservo.cflags | 1 + libsidservo.config | 7 + libsidservo.creator | 1 + libsidservo.creator.user | 218 +++++ libsidservo.creator.user.7bd84e3 | 165 ++++ libsidservo.creator.user.cf63021 | 184 ++++ libsidservo.cxxflags | 1 + libsidservo.files | 30 + libsidservo.includes | 2 + main.c | 621 ++++++++++++ main.h | 81 ++ movingfilter.c- | 46 + movingmodel.c | 73 ++ movingmodel.h | 76 ++ polltest/main.c | 197 ++++ ramp.c | 259 +++++ ramp.h | 23 + serial.c | 884 ++++++++++++++++++ serial.h | 44 + sidservo.h | 261 ++++++ sidservo.pc.in | 10 + ssii.c | 229 +++++ ssii.h | 350 +++++++ 98 files changed, 9089 insertions(+) create mode 100644 .qtcreator/libsidservo.creator.user create mode 100644 .qtcreator/libsidservo.creator.user.cf63021 create mode 100644 CMakeLists.txt create mode 100644 PID.c create mode 100644 PID.h create mode 100644 PID_test.deprecated/Dramp.c create mode 100644 PID_test.deprecated/Dramp.h create mode 100644 PID_test.deprecated/Makefile create mode 100644 PID_test.deprecated/Sramp.c create mode 100644 PID_test.deprecated/Sramp.h create mode 100644 PID_test.deprecated/Tramp.c create mode 100644 PID_test.deprecated/Tramp.h create mode 100644 PID_test.deprecated/main.c create mode 100644 PID_test.deprecated/moving.c create mode 100644 PID_test.deprecated/moving.h create mode 100644 PID_test.deprecated/moving_model.cflags create mode 100644 PID_test.deprecated/moving_model.config create mode 100644 PID_test.deprecated/moving_model.creator create mode 100644 PID_test.deprecated/moving_model.creator.user create mode 100644 PID_test.deprecated/moving_model.creator.user.7bd84e3 create mode 100644 PID_test.deprecated/moving_model.cxxflags create mode 100644 PID_test.deprecated/moving_model.files create mode 100644 PID_test.deprecated/moving_model.includes create mode 100644 PID_test.deprecated/moving_private.h create mode 100755 PID_test.deprecated/plot create mode 100755 PID_test.deprecated/plot.cont create mode 100755 PID_test.deprecated/plot_jpg create mode 100755 PID_test.deprecated/plot_pdf create mode 100755 PID_test.deprecated/plotacc create mode 100755 PID_test.deprecated/ploterr create mode 100755 PID_test.deprecated/ploterr.cont create mode 100755 PID_test.deprecated/ploterr_jpg create mode 100644 PID_test/Makefile create mode 100644 PID_test/PID.c create mode 100644 PID_test/PID.h create mode 100644 PID_test/Tramp.c create mode 100644 PID_test/Tramp.h create mode 100644 PID_test/main.c create mode 100644 PID_test/moving.c create mode 100644 PID_test/moving.h create mode 100644 PID_test/moving_model.cflags create mode 100644 PID_test/moving_model.config create mode 100644 PID_test/moving_model.creator create mode 100644 PID_test/moving_model.creator.user create mode 100644 PID_test/moving_model.creator.user.7bd84e3 create mode 100644 PID_test/moving_model.cxxflags create mode 100644 PID_test/moving_model.files create mode 100644 PID_test/moving_model.includes create mode 100644 PID_test/moving_private.h create mode 100755 PID_test/plot create mode 100755 PID_test/plot.cont create mode 100755 PID_test/plot_jpg create mode 100755 PID_test/plot_pdf create mode 100755 PID_test/plotacc create mode 100755 PID_test/ploterr create mode 100755 PID_test/ploterr.cont create mode 100755 PID_test/ploterr_jpg create mode 100644 TODO create mode 100644 examples/CMakeLists.txt create mode 100644 examples/Readme.md create mode 100644 examples/SSIIconf.c create mode 100644 examples/conf.c create mode 100644 examples/conf.h create mode 100644 examples/dump.c create mode 100644 examples/dump.h create mode 100644 examples/dumpmoving.c create mode 100644 examples/dumpmoving_dragNtrack.c create mode 100644 examples/dumpmoving_scmd.c create mode 100644 examples/dumpswing.c create mode 100644 examples/goto.c create mode 100644 examples/scmd_traectory.c create mode 100644 examples/servo.conf create mode 100644 examples/simpleconv.h create mode 100644 examples/traectories.c create mode 100644 examples/traectories.h create mode 100644 libsidservo.cflags create mode 100644 libsidservo.config create mode 100644 libsidservo.creator create mode 100644 libsidservo.creator.user create mode 100644 libsidservo.creator.user.7bd84e3 create mode 100644 libsidservo.creator.user.cf63021 create mode 100644 libsidservo.cxxflags create mode 100644 libsidservo.files create mode 100644 libsidservo.includes create mode 100644 main.c create mode 100644 main.h create mode 100644 movingfilter.c- create mode 100644 movingmodel.c create mode 100644 movingmodel.h create mode 100644 polltest/main.c create mode 100644 ramp.c create mode 100644 ramp.h create mode 100644 serial.c create mode 100644 serial.h create mode 100644 sidservo.h create mode 100644 sidservo.pc.in create mode 100644 ssii.c create mode 100644 ssii.h diff --git a/.qtcreator/libsidservo.creator.user b/.qtcreator/libsidservo.creator.user new file mode 100644 index 0000000..186d416 --- /dev/null +++ b/.qtcreator/libsidservo.creator.user @@ -0,0 +1,218 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/.qtcreator/libsidservo.creator.user.cf63021 b/.qtcreator/libsidservo.creator.user.cf63021 new file mode 100644 index 0000000..dfa9ebb --- /dev/null +++ b/.qtcreator/libsidservo.creator.user.cf63021 @@ -0,0 +1,218 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bdf2c99 --- /dev/null +++ b/CMakeLists.txt @@ -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() diff --git a/PID.c b/PID.c new file mode 100644 index 0000000..3f642cd --- /dev/null +++ b/PID.c @@ -0,0 +1,221 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include + +#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); +} diff --git a/PID.h b/PID.h new file mode 100644 index 0000000..5ecfad0 --- /dev/null +++ b/PID.h @@ -0,0 +1,40 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include + +#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); diff --git a/PID_test.deprecated/Dramp.c b/PID_test.deprecated/Dramp.c new file mode 100644 index 0000000..09a4f0e --- /dev/null +++ b/PID_test.deprecated/Dramp.c @@ -0,0 +1,141 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include + +#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, +}; diff --git a/PID_test.deprecated/Dramp.h b/PID_test.deprecated/Dramp.h new file mode 100644 index 0000000..e765c77 --- /dev/null +++ b/PID_test.deprecated/Dramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving_private.h" + +extern movemodel_t dumb; diff --git a/PID_test.deprecated/Makefile b/PID_test.deprecated/Makefile new file mode 100644 index 0000000..5e19d5d --- /dev/null +++ b/PID_test.deprecated/Makefile @@ -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 diff --git a/PID_test.deprecated/Sramp.c b/PID_test.deprecated/Sramp.c new file mode 100644 index 0000000..47f03ed --- /dev/null +++ b/PID_test.deprecated/Sramp.c @@ -0,0 +1,27 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// quasi non-jerk s-ramp + +#include +#include + +#include "Sramp.h" + + +movemodel_t s_shaped = { 0 }; diff --git a/PID_test.deprecated/Sramp.h b/PID_test.deprecated/Sramp.h new file mode 100644 index 0000000..3decd8b --- /dev/null +++ b/PID_test.deprecated/Sramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving_private.h" + +extern movemodel_t s_shaped; diff --git a/PID_test.deprecated/Tramp.c b/PID_test.deprecated/Tramp.c new file mode 100644 index 0000000..f164463 --- /dev/null +++ b/PID_test.deprecated/Tramp.c @@ -0,0 +1,223 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// simplest trapezioidal ramp + +#include +#include +#include +#include + +#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, +}; diff --git a/PID_test.deprecated/Tramp.h b/PID_test.deprecated/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/PID_test.deprecated/Tramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving_private.h" + +extern movemodel_t trapez; diff --git a/PID_test.deprecated/main.c b/PID_test.deprecated/main.c new file mode 100644 index 0000000..5a67b97 --- /dev/null +++ b/PID_test.deprecated/main.c @@ -0,0 +1,243 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include + +#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; +} diff --git a/PID_test.deprecated/moving.c b/PID_test.deprecated/moving.c new file mode 100644 index 0000000..d7264c0 --- /dev/null +++ b/PID_test.deprecated/moving.c @@ -0,0 +1,105 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#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; +} diff --git a/PID_test.deprecated/moving.h b/PID_test.deprecated/moving.h new file mode 100644 index 0000000..6854308 --- /dev/null +++ b/PID_test.deprecated/moving.h @@ -0,0 +1,70 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#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); diff --git a/PID_test.deprecated/moving_model.cflags b/PID_test.deprecated/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/PID_test.deprecated/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/PID_test.deprecated/moving_model.config b/PID_test.deprecated/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/PID_test.deprecated/moving_model.config @@ -0,0 +1,4 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/PID_test.deprecated/moving_model.creator b/PID_test.deprecated/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/PID_test.deprecated/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/PID_test.deprecated/moving_model.creator.user b/PID_test.deprecated/moving_model.creator.user new file mode 100644 index 0000000..eaf30ec --- /dev/null +++ b/PID_test.deprecated/moving_model.creator.user @@ -0,0 +1,221 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 1 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 1 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/C-files/mountcontrol.git/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/PID_test.deprecated/moving_model.creator.user.7bd84e3 b/PID_test.deprecated/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/PID_test.deprecated/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/PID_test.deprecated/moving_model.cxxflags b/PID_test.deprecated/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/PID_test.deprecated/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/PID_test.deprecated/moving_model.files b/PID_test.deprecated/moving_model.files new file mode 100644 index 0000000..e522e0e --- /dev/null +++ b/PID_test.deprecated/moving_model.files @@ -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 diff --git a/PID_test.deprecated/moving_model.includes b/PID_test.deprecated/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/PID_test.deprecated/moving_private.h b/PID_test.deprecated/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/PID_test.deprecated/moving_private.h @@ -0,0 +1,26 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving.h" + +extern double coord_tolerance; +extern double time_tick; + + diff --git a/PID_test.deprecated/plot b/PID_test.deprecated/plot new file mode 100755 index 0000000..476e1f3 --- /dev/null +++ b/PID_test.deprecated/plot @@ -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 diff --git a/PID_test.deprecated/plot.cont b/PID_test.deprecated/plot.cont new file mode 100755 index 0000000..16dc0f5 --- /dev/null +++ b/PID_test.deprecated/plot.cont @@ -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 +} \ No newline at end of file diff --git a/PID_test.deprecated/plot_jpg b/PID_test.deprecated/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/PID_test.deprecated/plot_jpg @@ -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 diff --git a/PID_test.deprecated/plot_pdf b/PID_test.deprecated/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/PID_test.deprecated/plot_pdf @@ -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 diff --git a/PID_test.deprecated/plotacc b/PID_test.deprecated/plotacc new file mode 100755 index 0000000..f15faaa --- /dev/null +++ b/PID_test.deprecated/plotacc @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:5 with lines title columnheader +pause mouse diff --git a/PID_test.deprecated/ploterr b/PID_test.deprecated/ploterr new file mode 100755 index 0000000..d2b990d --- /dev/null +++ b/PID_test.deprecated/ploterr @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:6 with lines title columnheader +pause mouse diff --git a/PID_test.deprecated/ploterr.cont b/PID_test.deprecated/ploterr.cont new file mode 100755 index 0000000..95bcd4b --- /dev/null +++ b/PID_test.deprecated/ploterr.cont @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +while(1){ + plot 'coords' using 1:6 with lines title columnheader + pause 1 +} diff --git a/PID_test.deprecated/ploterr_jpg b/PID_test.deprecated/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/PID_test.deprecated/ploterr_jpg @@ -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 diff --git a/PID_test/Makefile b/PID_test/Makefile new file mode 100644 index 0000000..5e19d5d --- /dev/null +++ b/PID_test/Makefile @@ -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 diff --git a/PID_test/PID.c b/PID_test/PID.c new file mode 100644 index 0000000..7774258 --- /dev/null +++ b/PID_test/PID.c @@ -0,0 +1,64 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#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; +} diff --git a/PID_test/PID.h b/PID_test/PID.h new file mode 100644 index 0000000..e60ddd6 --- /dev/null +++ b/PID_test/PID.h @@ -0,0 +1,39 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include + +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); diff --git a/PID_test/Tramp.c b/PID_test/Tramp.c new file mode 100644 index 0000000..f164463 --- /dev/null +++ b/PID_test/Tramp.c @@ -0,0 +1,223 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// simplest trapezioidal ramp + +#include +#include +#include +#include + +#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, +}; diff --git a/PID_test/Tramp.h b/PID_test/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/PID_test/Tramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving_private.h" + +extern movemodel_t trapez; diff --git a/PID_test/main.c b/PID_test/main.c new file mode 100644 index 0000000..d1b3c0f --- /dev/null +++ b/PID_test/main.c @@ -0,0 +1,222 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include + +#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; +} diff --git a/PID_test/moving.c b/PID_test/moving.c new file mode 100644 index 0000000..4b77040 --- /dev/null +++ b/PID_test/moving.c @@ -0,0 +1,89 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#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; +} diff --git a/PID_test/moving.h b/PID_test/moving.h new file mode 100644 index 0000000..eb77824 --- /dev/null +++ b/PID_test/moving.h @@ -0,0 +1,62 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#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); diff --git a/PID_test/moving_model.cflags b/PID_test/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/PID_test/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/PID_test/moving_model.config b/PID_test/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/PID_test/moving_model.config @@ -0,0 +1,4 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/PID_test/moving_model.creator b/PID_test/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/PID_test/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/PID_test/moving_model.creator.user b/PID_test/moving_model.creator.user new file mode 100644 index 0000000..eaf30ec --- /dev/null +++ b/PID_test/moving_model.creator.user @@ -0,0 +1,221 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 1 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 1 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/C-files/mountcontrol.git/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/PID_test/moving_model.creator.user.7bd84e3 b/PID_test/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/PID_test/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/PID_test/moving_model.cxxflags b/PID_test/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/PID_test/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/PID_test/moving_model.files b/PID_test/moving_model.files new file mode 100644 index 0000000..0086d29 --- /dev/null +++ b/PID_test/moving_model.files @@ -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 diff --git a/PID_test/moving_model.includes b/PID_test/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/PID_test/moving_private.h b/PID_test/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/PID_test/moving_private.h @@ -0,0 +1,26 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "moving.h" + +extern double coord_tolerance; +extern double time_tick; + + diff --git a/PID_test/plot b/PID_test/plot new file mode 100755 index 0000000..476e1f3 --- /dev/null +++ b/PID_test/plot @@ -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 diff --git a/PID_test/plot.cont b/PID_test/plot.cont new file mode 100755 index 0000000..16dc0f5 --- /dev/null +++ b/PID_test/plot.cont @@ -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 +} \ No newline at end of file diff --git a/PID_test/plot_jpg b/PID_test/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/PID_test/plot_jpg @@ -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 diff --git a/PID_test/plot_pdf b/PID_test/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/PID_test/plot_pdf @@ -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 diff --git a/PID_test/plotacc b/PID_test/plotacc new file mode 100755 index 0000000..f15faaa --- /dev/null +++ b/PID_test/plotacc @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:5 with lines title columnheader +pause mouse diff --git a/PID_test/ploterr b/PID_test/ploterr new file mode 100755 index 0000000..d2b990d --- /dev/null +++ b/PID_test/ploterr @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:6 with lines title columnheader +pause mouse diff --git a/PID_test/ploterr.cont b/PID_test/ploterr.cont new file mode 100755 index 0000000..95bcd4b --- /dev/null +++ b/PID_test/ploterr.cont @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +while(1){ + plot 'coords' using 1:6 with lines title columnheader + pause 1 +} diff --git a/PID_test/ploterr_jpg b/PID_test/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/PID_test/ploterr_jpg @@ -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 diff --git a/TODO b/TODO new file mode 100644 index 0000000..1d2dcb7 --- /dev/null +++ b/TODO @@ -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 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..5afbbd6 --- /dev/null +++ b/examples/CMakeLists.txt @@ -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) diff --git a/examples/Readme.md b/examples/Readme.md new file mode 100644 index 0000000..6a7e1dc --- /dev/null +++ b/examples/Readme.md @@ -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 + diff --git a/examples/SSIIconf.c b/examples/SSIIconf.c new file mode 100644 index 0000000..7980878 --- /dev/null +++ b/examples/SSIIconf.c @@ -0,0 +1,163 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include + +#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; +} diff --git a/examples/conf.c b/examples/conf.c new file mode 100644 index 0000000..8a77322 --- /dev/null +++ b/examples/conf.c @@ -0,0 +1,128 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include + +#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]; +} diff --git a/examples/conf.h b/examples/conf.h new file mode 100644 index 0000000..062528e --- /dev/null +++ b/examples/conf.h @@ -0,0 +1,28 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#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); diff --git a/examples/dump.c b/examples/dump.c new file mode 100644 index 0000000..de111ed --- /dev/null +++ b/examples/dump.c @@ -0,0 +1,197 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// logging of mount position + +#include + +#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"); + } +} diff --git a/examples/dump.h b/examples/dump.h new file mode 100644 index 0000000..0731f2e --- /dev/null +++ b/examples/dump.h @@ -0,0 +1,30 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include + +#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); diff --git a/examples/dumpmoving.c b/examples/dumpmoving.c new file mode 100644 index 0000000..9b0c111 --- /dev/null +++ b/examples/dumpmoving.c @@ -0,0 +1,105 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// dump telescope moving using simplest goto command + +#include +#include +#include +#include +#include + +#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; +} diff --git a/examples/dumpmoving_dragNtrack.c b/examples/dumpmoving_dragNtrack.c new file mode 100644 index 0000000..07d5b71 --- /dev/null +++ b/examples/dumpmoving_dragNtrack.c @@ -0,0 +1,228 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// move telescope to target using short command and force it to track mode +// also do some corrections while moving + +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/examples/dumpmoving_scmd.c b/examples/dumpmoving_scmd.c new file mode 100644 index 0000000..f16ee4b --- /dev/null +++ b/examples/dumpmoving_scmd.c @@ -0,0 +1,177 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// dump telescope moving using short binary commands + +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/examples/dumpswing.c b/examples/dumpswing.c new file mode 100644 index 0000000..4d4b51f --- /dev/null +++ b/examples/dumpswing.c @@ -0,0 +1,190 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#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; +} diff --git a/examples/goto.c b/examples/goto.c new file mode 100644 index 0000000..b17deab --- /dev/null +++ b/examples/goto.c @@ -0,0 +1,145 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// move telescope to given MOTOR position in degrees + +#include +#include +#include +#include + +#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; +} diff --git a/examples/scmd_traectory.c b/examples/scmd_traectory.c new file mode 100644 index 0000000..67ad655 --- /dev/null +++ b/examples/scmd_traectory.c @@ -0,0 +1,188 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include + +#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; +} diff --git a/examples/servo.conf b/examples/servo.conf new file mode 100644 index 0000000..9e7d3d4 --- /dev/null +++ b/examples/servo.conf @@ -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 + diff --git a/examples/simpleconv.h b/examples/simpleconv.h new file mode 100644 index 0000000..099ff86 --- /dev/null +++ b/examples/simpleconv.h @@ -0,0 +1,29 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// simple conversion macros + +#include + +#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.) + diff --git a/examples/traectories.c b/examples/traectories.c new file mode 100644 index 0000000..d8464af --- /dev/null +++ b/examples/traectories.c @@ -0,0 +1,136 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// some simplest traectories +// all traectories runs increasing X and Y from starting point + +#include +#include +#include + +#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); + } +} diff --git a/examples/traectories.h b/examples/traectories.h new file mode 100644 index 0000000..513eae6 --- /dev/null +++ b/examples/traectories.h @@ -0,0 +1,32 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#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); diff --git a/libsidservo.cflags b/libsidservo.cflags new file mode 100644 index 0000000..85d51b3 --- /dev/null +++ b/libsidservo.cflags @@ -0,0 +1 @@ +-std=c17 diff --git a/libsidservo.config b/libsidservo.config new file mode 100644 index 0000000..2a32b68 --- /dev/null +++ b/libsidservo.config @@ -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 diff --git a/libsidservo.creator b/libsidservo.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/libsidservo.creator @@ -0,0 +1 @@ +[General] diff --git a/libsidservo.creator.user b/libsidservo.creator.user new file mode 100644 index 0000000..186d416 --- /dev/null +++ b/libsidservo.creator.user @@ -0,0 +1,218 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + 0 + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + Version + 22 + + diff --git a/libsidservo.creator.user.7bd84e3 b/libsidservo.creator.user.7bd84e3 new file mode 100644 index 0000000..e594907 --- /dev/null +++ b/libsidservo.creator.user.7bd84e3 @@ -0,0 +1,165 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + false + true + false + + + + ProjectExplorer.Project.PluginSettings + + + true + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/C-sources/erfa + + + + all + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Сборка + + ProjectExplorer.BuildSteps.Build + + + + + clean + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Очистка + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + По умолчанию + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Развёртывание + + ProjectExplorer.BuildSteps.Deploy + + 1 + Конфигурация развёртывания + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + 2 + + + Особая программа + + ProjectExplorer.CustomExecutableRunConfiguration + + 3768 + false + true + false + false + true + + + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/libsidservo.creator.user.cf63021 b/libsidservo.creator.user.cf63021 new file mode 100644 index 0000000..002b8fd --- /dev/null +++ b/libsidservo.creator.user.cf63021 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + false + false + 1 + true + true + 0 + 8 + true + false + 1 + true + true + true + *.md, *.MD, Makefile + true + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 4 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/libsidservo.cxxflags b/libsidservo.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/libsidservo.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/libsidservo.files b/libsidservo.files new file mode 100644 index 0000000..aa9288a --- /dev/null +++ b/libsidservo.files @@ -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 diff --git a/libsidservo.includes b/libsidservo.includes new file mode 100644 index 0000000..d494155 --- /dev/null +++ b/libsidservo.includes @@ -0,0 +1,2 @@ +. +.. diff --git a/main.c b/main.c new file mode 100644 index 0000000..f2d731d --- /dev/null +++ b/main.c @@ -0,0 +1,621 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +/* + * main functions to fill struct `mount_t` + */ + +#include +#include +#include +#include +#include +#include + +#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, +}; + diff --git a/main.h b/main.h new file mode 100644 index 0000000..769d305 --- /dev/null +++ b/main.h @@ -0,0 +1,81 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +/* + * Almost all here used for debug purposes + */ + +#pragma once + +#include + +#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 + #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 diff --git a/movingfilter.c- b/movingfilter.c- new file mode 100644 index 0000000..16001b2 --- /dev/null +++ b/movingfilter.c- @@ -0,0 +1,46 @@ +#include +#include +#include + +#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; +} \ No newline at end of file diff --git a/movingmodel.c b/movingmodel.c new file mode 100644 index 0000000..4326859 --- /dev/null +++ b/movingmodel.c @@ -0,0 +1,73 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include + +#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); +} diff --git a/movingmodel.h b/movingmodel.h new file mode 100644 index 0000000..dfd7ab9 --- /dev/null +++ b/movingmodel.h @@ -0,0 +1,76 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#include + +#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); diff --git a/polltest/main.c b/polltest/main.c new file mode 100644 index 0000000..6ac84db --- /dev/null +++ b/polltest/main.c @@ -0,0 +1,197 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include +#include +#include + +// 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; +} diff --git a/ramp.c b/ramp.c new file mode 100644 index 0000000..b9f4728 --- /dev/null +++ b/ramp.c @@ -0,0 +1,259 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// simplest trapezioidal ramp + +#include +#include + +#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, +}; diff --git a/ramp.h b/ramp.h new file mode 100644 index 0000000..486a367 --- /dev/null +++ b/ramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include "movingmodel.h" + +extern movemodel_t trapez; diff --git a/serial.c b/serial.c new file mode 100644 index 0000000..fc57cd6 --- /dev/null +++ b/serial.c @@ -0,0 +1,884 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/serial.h b/serial.h new file mode 100644 index 0000000..45694b1 --- /dev/null +++ b/serial.h @@ -0,0 +1,44 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#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(); diff --git a/sidservo.h b/sidservo.h new file mode 100644 index 0000000..ed4feab --- /dev/null +++ b/sidservo.h @@ -0,0 +1,261 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +/* + * This file contains all need for external usage + */ + + +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +// 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 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 diff --git a/sidservo.pc.in b/sidservo.pc.in new file mode 100644 index 0000000..431e00d --- /dev/null +++ b/sidservo.pc.in @@ -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} diff --git a/ssii.c b/ssii.c new file mode 100644 index 0000000..6e6f3f9 --- /dev/null +++ b/ssii.c @@ -0,0 +1,229 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#include +#include +#include +#include +#include + +#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; +} diff --git a/ssii.h b/ssii.h new file mode 100644 index 0000000..0748369 --- /dev/null +++ b/ssii.h @@ -0,0 +1,350 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 Edward V. Emelianov . + * + * 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 . + */ + +/* + * This file contains stuff for sidereal-servo specific protocol + */ + +#pragma once + +#include +#include + +#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();