From b8ce24243d3970b935572838e1e89f138d0161dc Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Thu, 12 Mar 2026 10:40:13 +0300 Subject: [PATCH] CMakeLists.txt: LibSidServo library is now in external repository --- CMakeLists.txt | 19 +- LibSidServo/CMakeLists.txt | 87 -- LibSidServo/PID.c | 221 ----- LibSidServo/PID.h | 40 - LibSidServo/PID_test.deprecated/Dramp.c | 141 --- LibSidServo/PID_test.deprecated/Dramp.h | 23 - LibSidServo/PID_test.deprecated/Sramp.c | 27 - LibSidServo/PID_test.deprecated/Sramp.h | 23 - LibSidServo/PID_test.deprecated/Tramp.c | 223 ----- LibSidServo/PID_test.deprecated/Tramp.h | 23 - LibSidServo/PID_test.deprecated/main.c | 243 ----- LibSidServo/PID_test.deprecated/moving.c | 105 --- LibSidServo/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 - .../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 - LibSidServo/PID_test.deprecated/plot | 4 - LibSidServo/PID_test.deprecated/plot.cont | 8 - LibSidServo/PID_test.deprecated/plot_jpg | 6 - LibSidServo/PID_test.deprecated/plot_pdf | 5 - LibSidServo/PID_test.deprecated/plotacc | 4 - LibSidServo/PID_test.deprecated/ploterr | 4 - LibSidServo/PID_test.deprecated/ploterr.cont | 6 - LibSidServo/PID_test.deprecated/ploterr_jpg | 5 - LibSidServo/PID_test/PID.c | 64 -- LibSidServo/PID_test/PID.h | 39 - LibSidServo/PID_test/Tramp.c | 223 ----- LibSidServo/PID_test/Tramp.h | 23 - LibSidServo/PID_test/main.c | 222 ----- LibSidServo/PID_test/moving.c | 89 -- LibSidServo/PID_test/moving.h | 62 -- LibSidServo/PID_test/moving_model.cflags | 1 - LibSidServo/PID_test/moving_model.config | 4 - LibSidServo/PID_test/moving_model.creator | 1 - .../PID_test/moving_model.creator.user | 221 ----- .../moving_model.creator.user.7bd84e3 | 184 ---- LibSidServo/PID_test/moving_model.cxxflags | 1 - LibSidServo/PID_test/moving_model.files | 12 - LibSidServo/PID_test/moving_model.includes | 0 LibSidServo/PID_test/moving_private.h | 26 - LibSidServo/PID_test/plot | 4 - LibSidServo/PID_test/plot.cont | 8 - LibSidServo/PID_test/plot_jpg | 6 - LibSidServo/PID_test/plot_pdf | 5 - LibSidServo/PID_test/plotacc | 4 - LibSidServo/PID_test/ploterr | 4 - LibSidServo/PID_test/ploterr.cont | 6 - LibSidServo/PID_test/ploterr_jpg | 5 - LibSidServo/TODO | 3 - LibSidServo/examples/CMakeLists.txt | 14 - LibSidServo/examples/Readme.md | 28 - LibSidServo/examples/SSIIconf.c | 163 ---- LibSidServo/examples/conf.c | 128 --- LibSidServo/examples/conf.h | 28 - LibSidServo/examples/dump.c | 197 ---- LibSidServo/examples/dump.h | 30 - LibSidServo/examples/dumpmoving.c | 105 --- LibSidServo/examples/dumpmoving_dragNtrack.c | 228 ----- LibSidServo/examples/dumpmoving_scmd.c | 177 ---- LibSidServo/examples/dumpswing.c | 190 ---- LibSidServo/examples/goto.c | 145 --- LibSidServo/examples/scmd_traectory.c | 188 ---- LibSidServo/examples/servo.conf | 25 - LibSidServo/examples/simpleconv.h | 29 - LibSidServo/examples/traectories.c | 136 --- LibSidServo/examples/traectories.h | 32 - LibSidServo/libsidservo.cflags | 1 - LibSidServo/libsidservo.config | 7 - LibSidServo/libsidservo.creator | 1 - LibSidServo/libsidservo.creator.user | 220 ----- LibSidServo/libsidservo.creator.user.7bd84e3 | 165 ---- LibSidServo/libsidservo.creator.user.cf63021 | 184 ---- LibSidServo/libsidservo.cxxflags | 1 - LibSidServo/libsidservo.files | 30 - LibSidServo/libsidservo.includes | 2 - LibSidServo/main.c | 621 ------------ LibSidServo/main.h | 81 -- LibSidServo/movingfilter.c- | 46 - LibSidServo/movingmodel.c | 73 -- LibSidServo/movingmodel.h | 76 -- LibSidServo/polltest/main.c | 197 ---- LibSidServo/ramp.c | 259 ----- LibSidServo/ramp.h | 23 - LibSidServo/serial.c | 884 ------------------ LibSidServo/serial.h | 44 - LibSidServo/sidservo.h | 261 ------ LibSidServo/sidservo.pc.in | 10 - LibSidServo/ssii.c | 229 ----- LibSidServo/ssii.h | 350 ------- asibfm700_servocontroller.h | 4 +- 96 files changed, 19 insertions(+), 8545 deletions(-) delete mode 100644 LibSidServo/CMakeLists.txt delete mode 100644 LibSidServo/PID.c delete mode 100644 LibSidServo/PID.h delete mode 100644 LibSidServo/PID_test.deprecated/Dramp.c delete mode 100644 LibSidServo/PID_test.deprecated/Dramp.h delete mode 100644 LibSidServo/PID_test.deprecated/Sramp.c delete mode 100644 LibSidServo/PID_test.deprecated/Sramp.h delete mode 100644 LibSidServo/PID_test.deprecated/Tramp.c delete mode 100644 LibSidServo/PID_test.deprecated/Tramp.h delete mode 100644 LibSidServo/PID_test.deprecated/main.c delete mode 100644 LibSidServo/PID_test.deprecated/moving.c delete mode 100644 LibSidServo/PID_test.deprecated/moving.h delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.cflags delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.config delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.cxxflags delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.files delete mode 100644 LibSidServo/PID_test.deprecated/moving_model.includes delete mode 100644 LibSidServo/PID_test.deprecated/moving_private.h delete mode 100755 LibSidServo/PID_test.deprecated/plot delete mode 100755 LibSidServo/PID_test.deprecated/plot.cont delete mode 100755 LibSidServo/PID_test.deprecated/plot_jpg delete mode 100755 LibSidServo/PID_test.deprecated/plot_pdf delete mode 100755 LibSidServo/PID_test.deprecated/plotacc delete mode 100755 LibSidServo/PID_test.deprecated/ploterr delete mode 100755 LibSidServo/PID_test.deprecated/ploterr.cont delete mode 100755 LibSidServo/PID_test.deprecated/ploterr_jpg delete mode 100644 LibSidServo/PID_test/PID.c delete mode 100644 LibSidServo/PID_test/PID.h delete mode 100644 LibSidServo/PID_test/Tramp.c delete mode 100644 LibSidServo/PID_test/Tramp.h delete mode 100644 LibSidServo/PID_test/main.c delete mode 100644 LibSidServo/PID_test/moving.c delete mode 100644 LibSidServo/PID_test/moving.h delete mode 100644 LibSidServo/PID_test/moving_model.cflags delete mode 100644 LibSidServo/PID_test/moving_model.config delete mode 100644 LibSidServo/PID_test/moving_model.creator delete mode 100644 LibSidServo/PID_test/moving_model.creator.user delete mode 100644 LibSidServo/PID_test/moving_model.creator.user.7bd84e3 delete mode 100644 LibSidServo/PID_test/moving_model.cxxflags delete mode 100644 LibSidServo/PID_test/moving_model.files delete mode 100644 LibSidServo/PID_test/moving_model.includes delete mode 100644 LibSidServo/PID_test/moving_private.h delete mode 100755 LibSidServo/PID_test/plot delete mode 100755 LibSidServo/PID_test/plot.cont delete mode 100755 LibSidServo/PID_test/plot_jpg delete mode 100755 LibSidServo/PID_test/plot_pdf delete mode 100755 LibSidServo/PID_test/plotacc delete mode 100755 LibSidServo/PID_test/ploterr delete mode 100755 LibSidServo/PID_test/ploterr.cont delete mode 100755 LibSidServo/PID_test/ploterr_jpg delete mode 100644 LibSidServo/TODO delete mode 100644 LibSidServo/examples/CMakeLists.txt delete mode 100644 LibSidServo/examples/Readme.md delete mode 100644 LibSidServo/examples/SSIIconf.c delete mode 100644 LibSidServo/examples/conf.c delete mode 100644 LibSidServo/examples/conf.h delete mode 100644 LibSidServo/examples/dump.c delete mode 100644 LibSidServo/examples/dump.h delete mode 100644 LibSidServo/examples/dumpmoving.c delete mode 100644 LibSidServo/examples/dumpmoving_dragNtrack.c delete mode 100644 LibSidServo/examples/dumpmoving_scmd.c delete mode 100644 LibSidServo/examples/dumpswing.c delete mode 100644 LibSidServo/examples/goto.c delete mode 100644 LibSidServo/examples/scmd_traectory.c delete mode 100644 LibSidServo/examples/servo.conf delete mode 100644 LibSidServo/examples/simpleconv.h delete mode 100644 LibSidServo/examples/traectories.c delete mode 100644 LibSidServo/examples/traectories.h delete mode 100644 LibSidServo/libsidservo.cflags delete mode 100644 LibSidServo/libsidservo.config delete mode 100644 LibSidServo/libsidservo.creator delete mode 100644 LibSidServo/libsidservo.creator.user delete mode 100644 LibSidServo/libsidservo.creator.user.7bd84e3 delete mode 100644 LibSidServo/libsidservo.creator.user.cf63021 delete mode 100644 LibSidServo/libsidservo.cxxflags delete mode 100644 LibSidServo/libsidservo.files delete mode 100644 LibSidServo/libsidservo.includes delete mode 100644 LibSidServo/main.c delete mode 100644 LibSidServo/main.h delete mode 100644 LibSidServo/movingfilter.c- delete mode 100644 LibSidServo/movingmodel.c delete mode 100644 LibSidServo/movingmodel.h delete mode 100644 LibSidServo/polltest/main.c delete mode 100644 LibSidServo/ramp.c delete mode 100644 LibSidServo/ramp.h delete mode 100644 LibSidServo/serial.c delete mode 100644 LibSidServo/serial.h delete mode 100644 LibSidServo/sidservo.h delete mode 100644 LibSidServo/sidservo.pc.in delete mode 100644 LibSidServo/ssii.c delete mode 100644 LibSidServo/ssii.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0fb10bd..e43e461 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,10 +13,10 @@ option(USE_BSPLINE_PCM "Use of FITPACK bivariate splines for PCM" ON) find_package(cxxopts REQUIRED) -set(EXAMPLES OFF CACHE BOOL "" FORCE) +#set(EXAMPLES OFF CACHE BOOL "" FORCE) # set(CMAKE_BUILD_TYPE "Release") set(CMAKE_BUILD_TYPE "Debug") -add_subdirectory(LibSidServo) +#add_subdirectory(LibSidServo) # include(FetchContent) @@ -82,6 +82,19 @@ add_subdirectory(LibSidServo) include(FetchContent) +set(EXAMPLES OFF CACHE BOOL "" FORCE) +FetchContent_Declare( + servo_lib + GIT_REPOSITORY https://timur@git.sao.ru/timur/LibSidServo.git + GIT_SHALLOW 1 + GIT_PROGRESS 1 + SOURCE_DIR + ${CMAKE_BINARY_DIR}/LibSidServo +) + +FetchContent_MakeAvailable(servo_lib) +# FetchContent_GetProperties(servo_lib SOURCE_DIR servo_lib_inc) + FetchContent_Declare( mcclib GIT_REPOSITORY https://timur@git.sao.ru/timur/mcc.git @@ -124,7 +137,7 @@ add_library( # add_dependencies(${ASIBFM700_LIB} mcc) -# target_include_directories(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_INCLUDE_DIR}) +target_include_directories(${ASIBFM700_LIB} PUBLIC ${CMAKE_BINARY_DIR}) # LibSidServo headers # target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc spdlog ${ERFA_LIBFILE}) # target_link_libraries(${ASIBFM700_LIB} PUBLIC mcclib sidservo) target_link_libraries(${ASIBFM700_LIB} PUBLIC mcc sidservo atomic) diff --git a/LibSidServo/CMakeLists.txt b/LibSidServo/CMakeLists.txt deleted file mode 100644 index bdf2c99..0000000 --- a/LibSidServo/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -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/LibSidServo/PID.c b/LibSidServo/PID.c deleted file mode 100644 index 3f642cd..0000000 --- a/LibSidServo/PID.c +++ /dev/null @@ -1,221 +0,0 @@ -/* - * 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/LibSidServo/PID.h b/LibSidServo/PID.h deleted file mode 100644 index 5ecfad0..0000000 --- a/LibSidServo/PID.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Dramp.c b/LibSidServo/PID_test.deprecated/Dramp.c deleted file mode 100644 index 09a4f0e..0000000 --- a/LibSidServo/PID_test.deprecated/Dramp.c +++ /dev/null @@ -1,141 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Dramp.h b/LibSidServo/PID_test.deprecated/Dramp.h deleted file mode 100644 index e765c77..0000000 --- a/LibSidServo/PID_test.deprecated/Dramp.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Sramp.c b/LibSidServo/PID_test.deprecated/Sramp.c deleted file mode 100644 index 47f03ed..0000000 --- a/LibSidServo/PID_test.deprecated/Sramp.c +++ /dev/null @@ -1,27 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Sramp.h b/LibSidServo/PID_test.deprecated/Sramp.h deleted file mode 100644 index 3decd8b..0000000 --- a/LibSidServo/PID_test.deprecated/Sramp.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Tramp.c b/LibSidServo/PID_test.deprecated/Tramp.c deleted file mode 100644 index f164463..0000000 --- a/LibSidServo/PID_test.deprecated/Tramp.c +++ /dev/null @@ -1,223 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/Tramp.h b/LibSidServo/PID_test.deprecated/Tramp.h deleted file mode 100644 index ea4a257..0000000 --- a/LibSidServo/PID_test.deprecated/Tramp.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/main.c b/LibSidServo/PID_test.deprecated/main.c deleted file mode 100644 index 5a67b97..0000000 --- a/LibSidServo/PID_test.deprecated/main.c +++ /dev/null @@ -1,243 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/moving.c b/LibSidServo/PID_test.deprecated/moving.c deleted file mode 100644 index d7264c0..0000000 --- a/LibSidServo/PID_test.deprecated/moving.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/moving.h b/LibSidServo/PID_test.deprecated/moving.h deleted file mode 100644 index 6854308..0000000 --- a/LibSidServo/PID_test.deprecated/moving.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/moving_model.cflags b/LibSidServo/PID_test.deprecated/moving_model.cflags deleted file mode 100644 index 68d5165..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.cflags +++ /dev/null @@ -1 +0,0 @@ --std=c17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.config b/LibSidServo/PID_test.deprecated/moving_model.config deleted file mode 100644 index cadc51b..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.config +++ /dev/null @@ -1,4 +0,0 @@ -// Add predefined macros for your project here. For example: -// #define THE_ANSWER 42 -#define _XOPEN_SOURCE 666 -#define EBUG diff --git a/LibSidServo/PID_test.deprecated/moving_model.creator b/LibSidServo/PID_test.deprecated/moving_model.creator deleted file mode 100644 index e94cbbd..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.creator +++ /dev/null @@ -1 +0,0 @@ -[General] diff --git a/LibSidServo/PID_test.deprecated/moving_model.creator.user b/LibSidServo/PID_test.deprecated/moving_model.creator.user deleted file mode 100644 index eaf30ec..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.creator.user +++ /dev/null @@ -1,221 +0,0 @@ - - - - - - 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/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 b/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 deleted file mode 100644 index 1311ab3..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 +++ /dev/null @@ -1,184 +0,0 @@ - - - - - - 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/LibSidServo/PID_test.deprecated/moving_model.cxxflags b/LibSidServo/PID_test.deprecated/moving_model.cxxflags deleted file mode 100644 index 6435dfc..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.cxxflags +++ /dev/null @@ -1 +0,0 @@ --std=c++17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.files b/LibSidServo/PID_test.deprecated/moving_model.files deleted file mode 100644 index e522e0e..0000000 --- a/LibSidServo/PID_test.deprecated/moving_model.files +++ /dev/null @@ -1,10 +0,0 @@ -Dramp.c -Dramp.h -Sramp.c -Sramp.h -Tramp.c -Tramp.h -main.c -moving.c -moving.h -moving_private.h diff --git a/LibSidServo/PID_test.deprecated/moving_model.includes b/LibSidServo/PID_test.deprecated/moving_model.includes deleted file mode 100644 index e69de29..0000000 diff --git a/LibSidServo/PID_test.deprecated/moving_private.h b/LibSidServo/PID_test.deprecated/moving_private.h deleted file mode 100644 index e62a8dd..0000000 --- a/LibSidServo/PID_test.deprecated/moving_private.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * 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/LibSidServo/PID_test.deprecated/plot b/LibSidServo/PID_test.deprecated/plot deleted file mode 100755 index 476e1f3..0000000 --- a/LibSidServo/PID_test.deprecated/plot +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test.deprecated/plot.cont b/LibSidServo/PID_test.deprecated/plot.cont deleted file mode 100755 index 16dc0f5..0000000 --- a/LibSidServo/PID_test.deprecated/plot.cont +++ /dev/null @@ -1,8 +0,0 @@ -#!/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/LibSidServo/PID_test.deprecated/plot_jpg b/LibSidServo/PID_test.deprecated/plot_jpg deleted file mode 100755 index f926de6..0000000 --- a/LibSidServo/PID_test.deprecated/plot_jpg +++ /dev/null @@ -1,6 +0,0 @@ -#!/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/LibSidServo/PID_test.deprecated/plot_pdf b/LibSidServo/PID_test.deprecated/plot_pdf deleted file mode 100755 index 2001ca0..0000000 --- a/LibSidServo/PID_test.deprecated/plot_pdf +++ /dev/null @@ -1,5 +0,0 @@ -#!/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/LibSidServo/PID_test.deprecated/plotacc b/LibSidServo/PID_test.deprecated/plotacc deleted file mode 100755 index f15faaa..0000000 --- a/LibSidServo/PID_test.deprecated/plotacc +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot 'coords' using 1:5 with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr b/LibSidServo/PID_test.deprecated/ploterr deleted file mode 100755 index d2b990d..0000000 --- a/LibSidServo/PID_test.deprecated/ploterr +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot 'coords' using 1:6 with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr.cont b/LibSidServo/PID_test.deprecated/ploterr.cont deleted file mode 100755 index 95bcd4b..0000000 --- a/LibSidServo/PID_test.deprecated/ploterr.cont +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/gnuplot - -while(1){ - plot 'coords' using 1:6 with lines title columnheader - pause 1 -} diff --git a/LibSidServo/PID_test.deprecated/ploterr_jpg b/LibSidServo/PID_test.deprecated/ploterr_jpg deleted file mode 100755 index b1bd63b..0000000 --- a/LibSidServo/PID_test.deprecated/ploterr_jpg +++ /dev/null @@ -1,5 +0,0 @@ -#!/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/LibSidServo/PID_test/PID.c b/LibSidServo/PID_test/PID.c deleted file mode 100644 index 7774258..0000000 --- a/LibSidServo/PID_test/PID.c +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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/LibSidServo/PID_test/PID.h b/LibSidServo/PID_test/PID.h deleted file mode 100644 index e60ddd6..0000000 --- a/LibSidServo/PID_test/PID.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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/LibSidServo/PID_test/Tramp.c b/LibSidServo/PID_test/Tramp.c deleted file mode 100644 index f164463..0000000 --- a/LibSidServo/PID_test/Tramp.c +++ /dev/null @@ -1,223 +0,0 @@ -/* - * 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/LibSidServo/PID_test/Tramp.h b/LibSidServo/PID_test/Tramp.h deleted file mode 100644 index ea4a257..0000000 --- a/LibSidServo/PID_test/Tramp.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/LibSidServo/PID_test/main.c b/LibSidServo/PID_test/main.c deleted file mode 100644 index d1b3c0f..0000000 --- a/LibSidServo/PID_test/main.c +++ /dev/null @@ -1,222 +0,0 @@ -/* - * 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/LibSidServo/PID_test/moving.c b/LibSidServo/PID_test/moving.c deleted file mode 100644 index 4b77040..0000000 --- a/LibSidServo/PID_test/moving.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * 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/LibSidServo/PID_test/moving.h b/LibSidServo/PID_test/moving.h deleted file mode 100644 index eb77824..0000000 --- a/LibSidServo/PID_test/moving.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * 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/LibSidServo/PID_test/moving_model.cflags b/LibSidServo/PID_test/moving_model.cflags deleted file mode 100644 index 68d5165..0000000 --- a/LibSidServo/PID_test/moving_model.cflags +++ /dev/null @@ -1 +0,0 @@ --std=c17 \ No newline at end of file diff --git a/LibSidServo/PID_test/moving_model.config b/LibSidServo/PID_test/moving_model.config deleted file mode 100644 index cadc51b..0000000 --- a/LibSidServo/PID_test/moving_model.config +++ /dev/null @@ -1,4 +0,0 @@ -// Add predefined macros for your project here. For example: -// #define THE_ANSWER 42 -#define _XOPEN_SOURCE 666 -#define EBUG diff --git a/LibSidServo/PID_test/moving_model.creator b/LibSidServo/PID_test/moving_model.creator deleted file mode 100644 index e94cbbd..0000000 --- a/LibSidServo/PID_test/moving_model.creator +++ /dev/null @@ -1 +0,0 @@ -[General] diff --git a/LibSidServo/PID_test/moving_model.creator.user b/LibSidServo/PID_test/moving_model.creator.user deleted file mode 100644 index eaf30ec..0000000 --- a/LibSidServo/PID_test/moving_model.creator.user +++ /dev/null @@ -1,221 +0,0 @@ - - - - - - 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/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 b/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 deleted file mode 100644 index 1311ab3..0000000 --- a/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 +++ /dev/null @@ -1,184 +0,0 @@ - - - - - - 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/LibSidServo/PID_test/moving_model.cxxflags b/LibSidServo/PID_test/moving_model.cxxflags deleted file mode 100644 index 6435dfc..0000000 --- a/LibSidServo/PID_test/moving_model.cxxflags +++ /dev/null @@ -1 +0,0 @@ --std=c++17 \ No newline at end of file diff --git a/LibSidServo/PID_test/moving_model.files b/LibSidServo/PID_test/moving_model.files deleted file mode 100644 index 0086d29..0000000 --- a/LibSidServo/PID_test/moving_model.files +++ /dev/null @@ -1,12 +0,0 @@ -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/LibSidServo/PID_test/moving_model.includes b/LibSidServo/PID_test/moving_model.includes deleted file mode 100644 index e69de29..0000000 diff --git a/LibSidServo/PID_test/moving_private.h b/LibSidServo/PID_test/moving_private.h deleted file mode 100644 index e62a8dd..0000000 --- a/LibSidServo/PID_test/moving_private.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * 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/LibSidServo/PID_test/plot b/LibSidServo/PID_test/plot deleted file mode 100755 index 476e1f3..0000000 --- a/LibSidServo/PID_test/plot +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test/plot.cont b/LibSidServo/PID_test/plot.cont deleted file mode 100755 index 16dc0f5..0000000 --- a/LibSidServo/PID_test/plot.cont +++ /dev/null @@ -1,8 +0,0 @@ -#!/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/LibSidServo/PID_test/plot_jpg b/LibSidServo/PID_test/plot_jpg deleted file mode 100755 index f926de6..0000000 --- a/LibSidServo/PID_test/plot_jpg +++ /dev/null @@ -1,6 +0,0 @@ -#!/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/LibSidServo/PID_test/plot_pdf b/LibSidServo/PID_test/plot_pdf deleted file mode 100755 index 2001ca0..0000000 --- a/LibSidServo/PID_test/plot_pdf +++ /dev/null @@ -1,5 +0,0 @@ -#!/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/LibSidServo/PID_test/plotacc b/LibSidServo/PID_test/plotacc deleted file mode 100755 index f15faaa..0000000 --- a/LibSidServo/PID_test/plotacc +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot 'coords' using 1:5 with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test/ploterr b/LibSidServo/PID_test/ploterr deleted file mode 100755 index d2b990d..0000000 --- a/LibSidServo/PID_test/ploterr +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/gnuplot - -plot 'coords' using 1:6 with lines title columnheader -pause mouse diff --git a/LibSidServo/PID_test/ploterr.cont b/LibSidServo/PID_test/ploterr.cont deleted file mode 100755 index 95bcd4b..0000000 --- a/LibSidServo/PID_test/ploterr.cont +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/gnuplot - -while(1){ - plot 'coords' using 1:6 with lines title columnheader - pause 1 -} diff --git a/LibSidServo/PID_test/ploterr_jpg b/LibSidServo/PID_test/ploterr_jpg deleted file mode 100755 index b1bd63b..0000000 --- a/LibSidServo/PID_test/ploterr_jpg +++ /dev/null @@ -1,5 +0,0 @@ -#!/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/LibSidServo/TODO b/LibSidServo/TODO deleted file mode 100644 index 1d2dcb7..0000000 --- a/LibSidServo/TODO +++ /dev/null @@ -1,3 +0,0 @@ -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/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt deleted file mode 100644 index 5afbbd6..0000000 --- a/LibSidServo/examples/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -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/LibSidServo/examples/Readme.md b/LibSidServo/examples/Readme.md deleted file mode 100644 index 6a7e1dc..0000000 --- a/LibSidServo/examples/Readme.md +++ /dev/null @@ -1,28 +0,0 @@ -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/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c deleted file mode 100644 index 7980878..0000000 --- a/LibSidServo/examples/SSIIconf.c +++ /dev/null @@ -1,163 +0,0 @@ -/* - * 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/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c deleted file mode 100644 index 8a77322..0000000 --- a/LibSidServo/examples/conf.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * 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/LibSidServo/examples/conf.h b/LibSidServo/examples/conf.h deleted file mode 100644 index 062528e..0000000 --- a/LibSidServo/examples/conf.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - * 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/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c deleted file mode 100644 index de111ed..0000000 --- a/LibSidServo/examples/dump.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * 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/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h deleted file mode 100644 index 0731f2e..0000000 --- a/LibSidServo/examples/dump.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c deleted file mode 100644 index 9b0c111..0000000 --- a/LibSidServo/examples/dumpmoving.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * 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/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c deleted file mode 100644 index 07d5b71..0000000 --- a/LibSidServo/examples/dumpmoving_dragNtrack.c +++ /dev/null @@ -1,228 +0,0 @@ -/* - * 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/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c deleted file mode 100644 index f16ee4b..0000000 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ /dev/null @@ -1,177 +0,0 @@ -/* - * 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/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c deleted file mode 100644 index 4d4b51f..0000000 --- a/LibSidServo/examples/dumpswing.c +++ /dev/null @@ -1,190 +0,0 @@ -/* - * 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/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c deleted file mode 100644 index b17deab..0000000 --- a/LibSidServo/examples/goto.c +++ /dev/null @@ -1,145 +0,0 @@ -/* - * 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/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c deleted file mode 100644 index 67ad655..0000000 --- a/LibSidServo/examples/scmd_traectory.c +++ /dev/null @@ -1,188 +0,0 @@ -/* - * 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/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf deleted file mode 100644 index 9e7d3d4..0000000 --- a/LibSidServo/examples/servo.conf +++ /dev/null @@ -1,25 +0,0 @@ -# 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/LibSidServo/examples/simpleconv.h b/LibSidServo/examples/simpleconv.h deleted file mode 100644 index 099ff86..0000000 --- a/LibSidServo/examples/simpleconv.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * 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/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c deleted file mode 100644 index e51f15a..0000000 --- a/LibSidServo/examples/traectories.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * 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(10.)* 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/LibSidServo/examples/traectories.h b/LibSidServo/examples/traectories.h deleted file mode 100644 index 513eae6..0000000 --- a/LibSidServo/examples/traectories.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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/libsidservo.cflags b/LibSidServo/libsidservo.cflags deleted file mode 100644 index 85d51b3..0000000 --- a/LibSidServo/libsidservo.cflags +++ /dev/null @@ -1 +0,0 @@ --std=c17 diff --git a/LibSidServo/libsidservo.config b/LibSidServo/libsidservo.config deleted file mode 100644 index 2a32b68..0000000 --- a/LibSidServo/libsidservo.config +++ /dev/null @@ -1,7 +0,0 @@ -// 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/libsidservo.creator b/LibSidServo/libsidservo.creator deleted file mode 100644 index e94cbbd..0000000 --- a/LibSidServo/libsidservo.creator +++ /dev/null @@ -1 +0,0 @@ -[General] diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user deleted file mode 100644 index 58c6b84..0000000 --- a/LibSidServo/libsidservo.creator.user +++ /dev/null @@ -1,220 +0,0 @@ - - - - - - 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 - - 0 - - - - ProjectExplorer.Project.Target.0 - - Desktop - true - Desktop - Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} - 0 - 0 - 0 - - /tmp/robo5/mountcontrol.git/LibSidServo - - - - 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 - 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 - %{RunConfig:Executable:Path} - - 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 - %{RunConfig:Executable:Path} - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - Version - 22 - - diff --git a/LibSidServo/libsidservo.creator.user.7bd84e3 b/LibSidServo/libsidservo.creator.user.7bd84e3 deleted file mode 100644 index e594907..0000000 --- a/LibSidServo/libsidservo.creator.user.7bd84e3 +++ /dev/null @@ -1,165 +0,0 @@ - - - - - - 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/libsidservo.creator.user.cf63021 b/LibSidServo/libsidservo.creator.user.cf63021 deleted file mode 100644 index 002b8fd..0000000 --- a/LibSidServo/libsidservo.creator.user.cf63021 +++ /dev/null @@ -1,184 +0,0 @@ - - - - - - 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/libsidservo.cxxflags b/LibSidServo/libsidservo.cxxflags deleted file mode 100644 index 6435dfc..0000000 --- a/LibSidServo/libsidservo.cxxflags +++ /dev/null @@ -1 +0,0 @@ --std=c++17 \ No newline at end of file diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files deleted file mode 100644 index aa9288a..0000000 --- a/LibSidServo/libsidservo.files +++ /dev/null @@ -1,30 +0,0 @@ -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/libsidservo.includes b/LibSidServo/libsidservo.includes deleted file mode 100644 index d494155..0000000 --- a/LibSidServo/libsidservo.includes +++ /dev/null @@ -1,2 +0,0 @@ -. -.. diff --git a/LibSidServo/main.c b/LibSidServo/main.c deleted file mode 100644 index f2d731d..0000000 --- a/LibSidServo/main.c +++ /dev/null @@ -1,621 +0,0 @@ -/* - * 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/LibSidServo/main.h b/LibSidServo/main.h deleted file mode 100644 index 769d305..0000000 --- a/LibSidServo/main.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * 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/LibSidServo/movingfilter.c- b/LibSidServo/movingfilter.c- deleted file mode 100644 index 16001b2..0000000 --- a/LibSidServo/movingfilter.c- +++ /dev/null @@ -1,46 +0,0 @@ -#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/LibSidServo/movingmodel.c b/LibSidServo/movingmodel.c deleted file mode 100644 index 4326859..0000000 --- a/LibSidServo/movingmodel.c +++ /dev/null @@ -1,73 +0,0 @@ -/* - * 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/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h deleted file mode 100644 index dfd7ab9..0000000 --- a/LibSidServo/movingmodel.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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/LibSidServo/polltest/main.c b/LibSidServo/polltest/main.c deleted file mode 100644 index 6ac84db..0000000 --- a/LibSidServo/polltest/main.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * 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/LibSidServo/ramp.c b/LibSidServo/ramp.c deleted file mode 100644 index b9f4728..0000000 --- a/LibSidServo/ramp.c +++ /dev/null @@ -1,259 +0,0 @@ -/* - * 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/LibSidServo/ramp.h b/LibSidServo/ramp.h deleted file mode 100644 index 486a367..0000000 --- a/LibSidServo/ramp.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/LibSidServo/serial.c b/LibSidServo/serial.c deleted file mode 100644 index fc57cd6..0000000 --- a/LibSidServo/serial.c +++ /dev/null @@ -1,884 +0,0 @@ -/* - * 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/LibSidServo/serial.h b/LibSidServo/serial.h deleted file mode 100644 index 45694b1..0000000 --- a/LibSidServo/serial.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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/LibSidServo/sidservo.h b/LibSidServo/sidservo.h deleted file mode 100644 index ed4feab..0000000 --- a/LibSidServo/sidservo.h +++ /dev/null @@ -1,261 +0,0 @@ -/* - * 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/LibSidServo/sidservo.pc.in b/LibSidServo/sidservo.pc.in deleted file mode 100644 index 431e00d..0000000 --- a/LibSidServo/sidservo.pc.in +++ /dev/null @@ -1,10 +0,0 @@ -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/LibSidServo/ssii.c b/LibSidServo/ssii.c deleted file mode 100644 index 6e6f3f9..0000000 --- a/LibSidServo/ssii.c +++ /dev/null @@ -1,229 +0,0 @@ -/* - * 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/LibSidServo/ssii.h b/LibSidServo/ssii.h deleted file mode 100644 index 0748369..0000000 --- a/LibSidServo/ssii.h +++ /dev/null @@ -1,350 +0,0 @@ -/* - * 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(); diff --git a/asibfm700_servocontroller.h b/asibfm700_servocontroller.h index 6a07f7d..48668f3 100644 --- a/asibfm700_servocontroller.h +++ b/asibfm700_servocontroller.h @@ -6,7 +6,9 @@ #include #include -#include "LibSidServo/sidservo.h" +#include + +// #include "LibSidServo/sidservo.h" #include "asibfm700_common.h"