mountcontrol/LibSidServo/examples/dumpmoving_scmd.c

/*
 * This file is part of the libsidservo project.
 * Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// dump telescope moving using short binary commands

#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>

#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"

typedef struct{
    int help;
    int Ncycles;
    char *coordsoutput;
} 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"},
    {"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"},
    end_option
};

void signals(int sig){
    if(sig){
        signal(sig, SIG_IGN);
        DBG("Get signal %d, quit.\n", sig);
    }
    Mount.quit();
    exit(sig);
}

static conf_t Config = {
    .MountDevPath = "/dev/ttyUSB0",
    .MountDevSpeed = 19200,
    //.EncoderDevPath = "/dev/ttyUSB1",
    //.EncoderDevSpeed = 153000,
    .MountReqInterval = 0.1,
    .SepEncoder = 0
};

// dump thread
static void *dumping(void _U_ *u){
    dumpmoving(fcoords, 90., G.Ncycles);
    return NULL;
}

// return TRUE if motor position is reached +- 0.1 degrees
#define X2count (DEG2RAD(0.1))
#define Y2count (DEG2RAD(0.1))
static int WaitX(double xtag){
    mountdata_t mdata;
    red("Wait for %g degrees\n", RAD2DEG(xtag));
    int errcnt = 0;
    double sign = 0.;
    uint32_t millis = 0;
    do{
        if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
        else{
            errcnt = 0;
            if(mdata.millis == millis) continue;
            millis = mdata.millis;
            if(sign == 0.) sign = (mdata.motposition.X > xtag) ? 1. : -1.;
            printf("X=%g deg, need %g deg; delta=%g arcmin\n", RAD2DEG(mdata.motposition.X),
                   RAD2DEG(xtag), RAD2DEG(sign*(mdata.motposition.X - xtag))*60.);
        }
    }while(sign*(mdata.motposition.X - xtag) > X2count && errcnt < 10);
    if(errcnt >= 10){
        WARNX("Too much errors");
        return FALSE;
    }
    green("X reached position %g degrees\n", RAD2DEG(xtag));
    return TRUE;
}
/*
static int Yreached(double ytag){
    mountdata_t mdata;
    if(MCC_E_OK != Mount.getMountData(&mdata)) return FALSE;
    if(fabs(mdata.motposition.Y - ytag) <= Y2count) return TRUE;
    return FALSE;
}
*/

// check current position and go to 0 if non-zero
static void chk0(){
    coords_t M;
    if(!getPos(&M, NULL)) signals(2);
    if(M.X || M.Y){
        WARNX("Mount position isn't @ zero; moving");
        Mount.moveTo(0., 0.);
        waitmoving(G.Ncycles);
        green("Now mount @ zero\n");
    }
}

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;
    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
    // move to X=30 degr with different speeds
    // start with 1degr/s, increase to 15, decrease to 1
    short_command_t cmd = {0};
    pthread_t dthr;
#define SCMD()   do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
    chk0();
    logmnt(fcoords, NULL);
    if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
    // goto 10 degr with 1deg/s
    cmd.Xmot = DEG2RAD(30.);
    cmd.Xspeed = DEG2RAD(1.);
    SCMD();
    if(!WaitX(DEG2RAD(10.))) signals(9);
    // goto 20 degr with 5deg/s
    cmd.Xmot = DEG2RAD(30.);
    cmd.Xspeed = DEG2RAD(2.);
    SCMD();
    if(!WaitX(DEG2RAD(20.))) signals(9);
    // goto 30 degr with 15deg/s
    cmd.Xmot = DEG2RAD(40.);
    cmd.Xspeed = DEG2RAD(5.);
    SCMD();
    if(!WaitX(DEG2RAD(30.))) signals(9);
    // goto 40 degr with 1deg/s
    cmd.Xmot = DEG2RAD(40.);
    cmd.Xspeed = DEG2RAD(1.);
    SCMD();
    if(!WaitX(DEG2RAD(40.))) signals(9);
    // and go back with 15deg/s
    cmd.Xmot = DEG2RAD(0.);
    cmd.Xspeed = DEG2RAD(15.);
    SCMD();
    if(!WaitX(DEG2RAD(0.))) signals(9);
    Mount.moveTo(0., 0.);
    waitmoving(10);
    // wait moving ends
    pthread_join(dthr, NULL);
#undef SCMD
    signals(0);
    return 0;
}