diff --git a/Auxiliary_utils/LibSidServo/PID.c b/Auxiliary_utils/LibSidServo/PID.c new file mode 100644 index 0000000..8afe031 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/PID.c @@ -0,0 +1,193 @@ +/* + * 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; + 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){ + if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){ + DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT); + return axis->speed.val; // data is too old or wrong + } + double error = tagpos->val - axis->position.val, fe = fabs(error); + PIDController_t *pid = NULL; + switch(axis->state){ + case AXIS_SLEWING: + if(fe < MCC_MAX_POINTING_ERR){ + axis->state = AXIS_POINTING; + DBG("--> Pointing"); + pid = pidpair->PIDC; + }else{ + DBG("Slewing..."); + return -1.; // max speed for given axis + } + break; + case AXIS_POINTING: + if(fe < MCC_MAX_GUIDING_ERR){ + axis->state = AXIS_GUIDING; + DBG("--> Guiding"); + pid = pidpair->PIDV; + }else if(fe > MCC_MAX_POINTING_ERR){ + DBG("--> Slewing"); + axis->state = AXIS_SLEWING; + return -1.; + } else pid = pidpair->PIDC; + break; + case AXIS_GUIDING: + pid = pidpair->PIDV; + if(fe > MCC_MAX_GUIDING_ERR){ + DBG("--> Pointing"); + axis->state = AXIS_POINTING; + pid = pidpair->PIDC; + }else if(fe < MCC_MAX_ATTARGET_ERR){ + 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!!!!"); + axis->state = AXIS_SLEWING; + return -1.; + case AXIS_ERROR: + DBG("Can't move from erroneous state"); + return 0.; + } + if(!pid){ + DBG("WTF? Where is a PID?"); + return axis->speed.val; + } + if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){ + DBG("time diff too big: clear PID"); + pid_clear(pid); + } + double dt = tagpos->t - pid->prevT; + if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME; + pid->prevT = tagpos->t; + //DBG("CALC PID (er=%g, dt=%g)", error, dt); + double tagspeed = pid_calculate(pid, error, dt); + if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // 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, const coordpair_t *endpoint){ + static PIDpair_t pidX = {0}, pidY = {0}; + if(!pidX.PIDC){ + pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + if(!pidX.PIDC) return MCC_E_FATAL; + pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + if(!pidX.PIDV) return MCC_E_FATAL; + } + if(!pidY.PIDC){ + pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + if(!pidY.PIDC) return MCC_E_FATAL; + pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + if(!pidY.PIDV) return MCC_E_FATAL; + } + mountdata_t m; + coordpair_t tagspeed; + 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(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_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(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED; + axis_status_t ystate = axis.state; + if(m.Xstate != xstate || m.Ystate != ystate){ + DBG("State changed"); + setStat(xstate, ystate); + } + DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y); + return Mount.moveWspeed(endpoint, &tagspeed); +} diff --git a/Auxiliary_utils/LibSidServo/PID.h b/Auxiliary_utils/LibSidServo/PID.h new file mode 100644 index 0000000..5eb8cb9 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/PID.h @@ -0,0 +1,40 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include "sidservo.h" + +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + double 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, const coordpair_t *endpoint); diff --git a/Auxiliary_utils/LibSidServo/PID_test/PID.c b/Auxiliary_utils/LibSidServo/PID_test/PID.c new file mode 100644 index 0000000..7774258 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/PID_test/PID.c @@ -0,0 +1,64 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include + +#include "PID.h" + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){ + if(!gain || Iarrsz < 3) return NULL; + PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); + pid->gain = *gain; + pid->pidIarrSize = Iarrsz; + pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); + return pid; +} + +void pid_clear(PIDController_t *pid){ + if(!pid) return; + DBG("CLEAR PID PARAMETERS"); + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +void pid_delete(PIDController_t **pid){ + if(!pid || !*pid) return; + if((*pid)->pidIarray) free((*pid)->pidIarray); + free(*pid); + *pid = NULL; +} + +double pid_calculate(PIDController_t *pid, double error, double dt){ + // calculate flowing integral + double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; + DBG("oldi/new: %g, %g", oldi, newi); + pid->pidIarray[pid->curIidx++] = newi; + if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0; + pid->integral += newi - oldi; + double derivative = (error - pid->prev_error) / dt; + pid->prev_error = error; + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; + DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); + return sum; +} diff --git a/Auxiliary_utils/LibSidServo/PID_test/PID.h b/Auxiliary_utils/LibSidServo/PID_test/PID.h new file mode 100644 index 0000000..e60ddd6 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/PID_test/PID.h @@ -0,0 +1,39 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +typedef struct{ + double P, I, D; +} PIDpar_t; + +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz); +void pid_clear(PIDController_t *pid); +void pid_delete(PIDController_t **pid); +double pid_calculate(PIDController_t *pid, double error, double dt); diff --git a/Auxiliary_utils/LibSidServo/PID_test/main.c b/Auxiliary_utils/LibSidServo/PID_test/main.c index 5a67b97..d1b3c0f 100644 --- a/Auxiliary_utils/LibSidServo/PID_test/main.c +++ b/Auxiliary_utils/LibSidServo/PID_test/main.c @@ -22,13 +22,18 @@ #include #include "moving.h" +#include "PID.h" // errors for states: slewing/pointing/guiding -#define MAX_POINTING_ERR (50.) -#define MAX_GUIDING_ERR (5.) +// 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; @@ -48,93 +53,64 @@ typedef struct{ double dTcorr; double Tend; double minerr; - double P, I, D; + double startcoord; + double error; + PIDpar_t gainC, gainV; } pars; static pars G = { - .ramptype = "t", .dTmon = 0.01, .dTcorr = 0.05, .Tend = 100., .minerr = 0.1, - .P = 0.8, + .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 = 1e6, .speed = 1e3, .accel = 500.}, - .jerk = 10. + .max = {.coord = 6648000, .speed = 36000., .accel = 36000.} }; -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"}, + {"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 target_coord(20.); - double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5); + 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; } -/* 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); + PIDController_t *pid = NULL; switch(state){ case Slewing: if(fe < MAX_POINTING_ERR){ - pid_clear(&pid); + pid_clear(pidC); state = Pointing; green("--> Pointing\n"); + pid = pidC; }else{ red("Slewing...\n"); return (error > 0.) ? limits.max.speed : -limits.max.speed; @@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ break; case Pointing: if(fe < MAX_GUIDING_ERR){ - pid_clear(&pid); + 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"); - //pid_clear(&pid); - //return p->speed; - } + }else printf("Current error: %g\n", fe); 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; + 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; } -// ./moving -l coords -P.5 -I.05 -D1.5 -// ./moving -l coords -P1.3 -D1.6 +// -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.;//, Tlast = 0.; + 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; @@ -215,6 +192,9 @@ static void start_model(double Tend){ 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){ @@ -228,16 +208,15 @@ int main(int argc, char **argv){ 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); + 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/Auxiliary_utils/LibSidServo/PID_test/moving.c b/Auxiliary_utils/LibSidServo/PID_test/moving.c index d7264c0..4b77040 100644 --- a/Auxiliary_utils/LibSidServo/PID_test/moving.c +++ b/Auxiliary_utils/LibSidServo/PID_test/moving.c @@ -24,12 +24,9 @@ #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; +static movemodel_t *model = &trapez; double coord_tolerance = COORD_TOLERANCE_DEFAULT; double time_tick = TIME_TICK_DEFAULT; @@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){ *max = t; } -movemodel_t *init_moving(ramptype_t type, limits_t *l){ +movemodel_t *init_moving(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; diff --git a/Auxiliary_utils/LibSidServo/PID_test/moving.h b/Auxiliary_utils/LibSidServo/PID_test/moving.h index 6854308..eb77824 100644 --- a/Auxiliary_utils/LibSidServo/PID_test/moving.h +++ b/Auxiliary_utils/LibSidServo/PID_test/moving.h @@ -26,13 +26,6 @@ #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 @@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative typedef struct{ moveparam_t min; moveparam_t max; - double jerk; } limits_t; typedef struct{ @@ -64,7 +56,7 @@ typedef struct{ extern double coord_tolerance; double nanot(); -movemodel_t *init_moving(ramptype_t type, limits_t *l); +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/Auxiliary_utils/LibSidServo/examples/SSIIconf.c b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c index e05b42e..d399116 100644 --- a/Auxiliary_utils/LibSidServo/examples/SSIIconf.c +++ b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c @@ -51,10 +51,10 @@ static sl_option_t confopts[] = { end_option }; -static void dumpaxe(char axe, axe_config_t *c){ +static void dumpaxis(char axis, axis_config_t *c){ #define STRUCTPAR(p) (c)->p -#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0) -#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0) +#define DUMP(par) do{printf("%c%s=%g\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); @@ -99,14 +99,15 @@ static void dumpHWconf(){ #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 axe configuration:\n"); - dumpaxe('X', &HW.Xconf); + green("X axis configuration:\n"); + dumpaxis('X', &HW.Xconf); green("X bits:\n"); dumpxbits(&HW.xbits); - green("Y axe configuration:\n"); - dumpaxe('Y', &HW.Yconf); + 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); @@ -142,10 +143,12 @@ int main(int argc, char** argv){ } 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){ diff --git a/Auxiliary_utils/LibSidServo/examples/conf.c b/Auxiliary_utils/LibSidServo/examples/conf.c index 1969a0f..1271a21 100644 --- a/Auxiliary_utils/LibSidServo/examples/conf.c +++ b/Auxiliary_utils/LibSidServo/examples/conf.c @@ -31,6 +31,18 @@ static conf_t Config = { .EncoderReqInterval = 0.05, .SepEncoder = 2, .EncoderSpeedInterval = 0.1, + .XPIDC.P = 0.8, + .XPIDC.I = 0.1, + .XPIDC.D = 0.3, + .XPIDV.P = 1., + .XPIDV.I = 0.01, + .XPIDV.D = 0.2, + .YPIDC.P = 0.8, + .YPIDC.I = 0.1, + .YPIDC.D = 0.3, + .YPIDV.P = 0.5, + .YPIDV.I = 0.2, + .YPIDV.D = 0.5, }; static sl_option_t opts[] = { @@ -44,6 +56,19 @@ static sl_option_t opts[] = { {"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)"}, {"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"}, + {"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)"}, end_option }; diff --git a/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c index 5b818f6..27fe123 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.c +++ b/Auxiliary_utils/LibSidServo/examples/dump.c @@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){ enct = tmsr; if(fcoords) logmnt(fcoords, &mdata); if(mdata.millis == mdmillis) continue; - //DBG("ctr=%d", ctr); + //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; @@ -123,10 +123,11 @@ void dumpmoving(FILE *fcoords, double t, int N){ ctr = 0; }else ++ctr; } + DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0); } /** - * @brief waitmoving - wait until moving by both axes stops at least for N cycles + * @brief waitmoving - wait until moving by both axiss stops at least for N cycles * @param N - amount of stopped cycles */ void waitmoving(int N){ diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c index 326ec5e..67dcf71 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -120,16 +120,13 @@ 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(coordpair_t *target, coordpair_t *speed){ +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); - target->X = DEG2RAD(target->X); - target->Y = DEG2RAD(target->Y); - speed->X = DEG2RAD(speed->X); - speed->Y = DEG2RAD(speed->Y); - cmd.Xmot = target->X; cmd.Ymot = target->Y; - cmd.Xspeed = speed->X; cmd.Yspeed = 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; @@ -142,7 +139,8 @@ static mcc_errcodes_t return2zero(){ short_command_t cmd = {0}; DBG("Try to move to zero"); cmd.Xmot = 0.; cmd.Ymot = 0.; - cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.); + cmd.Xspeed = MCC_MAX_X_SPEED; + cmd.Yspeed = MCC_MAX_Y_SPEED; /*cmd.xychange = 1; cmd.XBits = 100; cmd.YBits = 20;*/ @@ -151,11 +149,11 @@ static mcc_errcodes_t return2zero(){ static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){ long_command_t cmd = {0}; - cmd.Xspeed = lastSpeed.X; - cmd.Yspeed = lastSpeed.Y; - cmd.Xmot = lastTag.X; - cmd.Ymot = lastTag.Y; - cmd.Xadder = adder->X; cmd.Yadder = adder->Y; + 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); } @@ -218,7 +216,7 @@ int main(int argc, char **argv){ sleep(5); // return to zero and wait green("Return 2 zero and wait\n"); - return2zero(); + if(!return2zero()) ERRX("Can't return"); Wait(0., 0); Wait(0., 1); // wait moving ends diff --git a/Auxiliary_utils/LibSidServo/examples/dumpswing.c b/Auxiliary_utils/LibSidServo/examples/dumpswing.c index da65bef..2facc3d 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpswing.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpswing.c @@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = { {"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 reaced if period is too small) - not more than 45deg (default: 5)"}, + {"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 @@ -89,12 +89,12 @@ void waithalf(double t){ 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)); + //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)); + //DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val)); ++ctr; } } @@ -114,7 +114,8 @@ int main(int argc, char **argv){ ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; if(G.Ncycles < 7) ERRX("Ncycles should be >7"); - if(G.amplitude < 0.01 || G.amplitude > 45.) + double absamp = fabs(G.amplitude); + if(absamp < 0.01 || absamp > 45.) ERRX("Amplitude should be from 0.01 to 45 degrees"); if(G.period < 0.1 || G.period > 900.) ERRX("Period should be from 0.1 to 900s"); @@ -162,9 +163,13 @@ int main(int argc, char **argv){ DBG("Moved to -, t=%g", t-t0); DBG("CMD: %g", Mount.currentT()-t0); } - tag = (coordpair_t){.X = 0., .Y = 0.}; + green("Move to zero @ %g\n", Mount.currentT()); + tag = (coordpair_t){0}; // be sure to move @ 0,0 - Mount.moveTo(&tag); + if(MCC_E_OK != Mount.moveTo(&tag)){ + Mount.emergStop(); + Mount.moveTo(&tag); + } // wait moving ends pthread_join(dthr, NULL); #undef SCMD diff --git a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c index 621f6f5..5acf339 100644 --- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c +++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c @@ -30,6 +30,7 @@ typedef struct{ int help; + int dumpconf; int Ncycles; // n cycles to wait stop double reqint; // requests interval (seconds) double Xmax; // maximal X to stop @@ -38,11 +39,12 @@ typedef struct{ 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 FILE *fcoords = NULL; +static FILE *fcoords = NULL, *errlog = NULL; static pthread_t dthr; static parameters G = { .Ncycles = 40, @@ -61,12 +63,14 @@ static sl_option_t cmdlnopts[] = { {"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 X coordinate for traectory (default: 45 degrees)"}, - {"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X coordinate for traectory (default: 45 degrees)"}, + {"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 }; @@ -76,6 +80,8 @@ void signals(int sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } + Mount.stop(); + sleep(1); Mount.quit(); if(fcoords) fclose(fcoords); exit(sig); @@ -90,22 +96,39 @@ static void *dumping(void _U_ *u){ static void runtraectory(traectory_fn tfn){ if(!tfn) return; coordval_pair_t telXY; - coordpair_t traectXY; - double t0 = Mount.currentT(); - double tlast = 0.; + coordval_pair_t target; + coordpair_t traectXY, endpoint; + endpoint.X = G.Xmax, endpoint.Y = G.Ymax; + double t0 = Mount.currentT(), tlast = 0.; + double tlastX = 0., tlastY = 0.; while(1){ if(!telpos(&telXY)){ WARNX("No next telescope position"); return; } - if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind - tlast = (telXY.X.t + telXY.Y.t) / 2.; + if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind + DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t); + tlastX = telXY.X.t; tlastY = telXY.Y.t; double t = Mount.currentT(); - if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break; + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break; if(!traectory_point(&traectXY, t)) break; - DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + target.X.val = traectXY.X; target.Y.val = traectXY.Y; + target.X.t = target.Y.t = t; + // check whether we should change direction + if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax; + else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax; + if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax; + else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax; + DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); + DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, 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.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!"); + while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50); + tlast = t; } - WARNX("No next traectory point"); + WARNX("No next traectory point or emulation ends"); } int main(int argc, char **argv){ @@ -118,12 +141,18 @@ int main(int argc, char **argv){ 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; conf_t *Config = readServoConf(G.conffile); - if(!Config){ + if(!Config || G.dumpconf){ dumpConf(); return 1; } diff --git a/Auxiliary_utils/LibSidServo/examples/servo.conf b/Auxiliary_utils/LibSidServo/examples/servo.conf index dfe0a25..220926e 100644 --- a/Auxiliary_utils/LibSidServo/examples/servo.conf +++ b/Auxiliary_utils/LibSidServo/examples/servo.conf @@ -1,9 +1,25 @@ +Current configuration: MountDevPath=/dev/ttyUSB0 MountDevSpeed=19200 +EncoderDevPath=(null) +EncoderDevSpeed=1000000 +MountReqInterval=0.1 +EncoderReqInterval=0.001 +SepEncoder=2 EncoderXDevPath=/dev/encoder_X0 EncoderYDevPath=/dev/encoder_Y0 -MountReqInterval=0.05 -SepEncoder=2 -EncoderReqInterval=0.001 -EncoderDevSpeed=1000000 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/Auxiliary_utils/LibSidServo/examples/simpleconv.h b/Auxiliary_utils/LibSidServo/examples/simpleconv.h index 8200def..099ff86 100644 --- a/Auxiliary_utils/LibSidServo/examples/simpleconv.h +++ b/Auxiliary_utils/LibSidServo/examples/simpleconv.h @@ -20,10 +20,10 @@ #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.) +#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/Auxiliary_utils/LibSidServo/examples/traectories.c b/Auxiliary_utils/LibSidServo/examples/traectories.c index 1977880..ebc31cf 100644 --- a/Auxiliary_utils/LibSidServo/examples/traectories.c +++ b/Auxiliary_utils/LibSidServo/examples/traectories.c @@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL; // starting point of traectory static coordpair_t XYstart = {0}; static double tstart = 0.; -// convert Xe/Ye to approximate motor coordinates: -// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have -// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0 -static coordval_pair_t XYcor = {0}; /** * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders @@ -52,9 +48,6 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){ if(MCC_E_OK == Mount.getMountData(&mdata)) break; } if(ntries == 10) return FALSE; - XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val; - XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val; - DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val)); return TRUE; } @@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){ } if(ntries == 10) return FALSE; coordval_pair_t pt; - pt.X.val = XYcor.X.val + mdata.encXposition.val; - pt.Y.val = XYcor.Y.val + mdata.encYposition.val; + 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; @@ -94,7 +87,7 @@ int telpos(coordval_pair_t *curpos){ // X=X0+1'/s, Y=Y0+15''/s int Linear(coordpair_t *nextpt, double t){ coordpair_t pt; - pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart); + pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart); pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); if(nextpt) *nextpt = pt; return TRUE; @@ -103,7 +96,7 @@ int Linear(coordpair_t *nextpt, double t){ // 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 + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI); + 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; @@ -116,8 +109,8 @@ typedef struct{ } tr_names; static tr_names names[] = { - {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"}, - {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"}, + {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} }; diff --git a/Auxiliary_utils/LibSidServo/libsidservo.config b/Auxiliary_utils/LibSidServo/libsidservo.config index 128fef3..2a32b68 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.config +++ b/Auxiliary_utils/LibSidServo/libsidservo.config @@ -1,6 +1,7 @@ // Add predefined macros for your project here. For example: // #define THE_ANSWER 42 #define EBUG -#define _POSIX_C_SOURCE 111 +#define _POSIX_C_SOURCE 11111111 #define PACKAGE_VERSION "0.0.1" - +#define _XOPEN_SOURCE 666 +#define _DEFAULT_SOURCE diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index 7ee670a..73de00a 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,10 +1,10 @@ - + EnvironmentId - {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} ProjectExplorer.Project.ActiveTarget @@ -40,9 +40,9 @@ 1 0 false - true + false false - 0 + 1 true true 0 @@ -51,10 +51,10 @@ false 1 true - false + true true *.md, *.MD, Makefile - false + true true true @@ -75,13 +75,11 @@ 0 true - - true true true Builtin.DefaultTidyAndClazy - 8 + 4 true @@ -97,12 +95,12 @@ true Desktop Desktop - {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + {91347f2c-5221-46a7-80b1-0a054ca02f79} 0 0 0 - /Big/Data/00__Small_tel/C-sources/erfa + /home/eddy/Docs/SAO/10micron/C-sources/erfa_functions @@ -112,8 +110,8 @@ GenericProjectManager.GenericMakeStep 1 - Build - Build + Сборка + Сборка ProjectExplorer.BuildSteps.Build @@ -125,8 +123,8 @@ GenericProjectManager.GenericMakeStep 1 - Clean - Clean + Очистка + Очистка ProjectExplorer.BuildSteps.Clean 2 @@ -141,8 +139,8 @@ 0 - Deploy - Deploy + Развёртывание + Развёртывание ProjectExplorer.BuildSteps.Deploy 1 @@ -154,7 +152,9 @@ true true + 0 true + 2 @@ -173,8 +173,8 @@ 0 - Deploy - Deploy + Развёртывание + Развёртывание ProjectExplorer.BuildSteps.Deploy 1 @@ -186,7 +186,9 @@ true true + 0 true + 2 diff --git a/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files index 3d32e92..7777ac7 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.files +++ b/Auxiliary_utils/LibSidServo/libsidservo.files @@ -1,5 +1,6 @@ CMakeLists.txt -dbg.h +PID.c +PID.h examples/SSIIconf.c examples/conf.c examples/conf.h @@ -18,6 +19,11 @@ serial.c examples/CMakeLists.txt examples/traectories.c examples/traectories.h +main.h +movingmodel.c +movingmodel.h +ramp.c +ramp.h serial.h ssii.c ssii.h diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index 1a392fc..6927a4b 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -16,28 +16,133 @@ * along with this program. If not, see . */ +/* + * main functions to fill struct `mount_t` + */ + #include +#include +#include #include #include -#include "dbg.h" +#include "main.h" +#include "movingmodel.h" #include "serial.h" #include "ssii.h" +#include "PID.h" conf_t Conf = {0}; - +// parameters for model +static movemodel_t *Xmodel, *Ymodel; +// radians, rad/sec, rad/sec^2 +static limits_t + Xlimits = { + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}}, + Ylimits = { + .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, + .max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}} +; static mcc_errcodes_t shortcmd(short_command_t *cmd); +/** + * @brief nanotime - monotonic time from first run + * @return time in seconds + */ +double nanotime(){ + static struct timespec *start = NULL; + struct timespec now; + if(!start){ + start = malloc(sizeof(struct timespec)); + if(!start) return -1.; + if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.; + } + if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.; + 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; +} + /** * @brief quit - close all opened and return to default state */ static void quit(){ - DBG("Close serial devices"); + 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(mountdata_t *mountdata){ + if(!mountdata || !Xmodel || !Ymodel) return; + static double oldmt = -100.; // old `millis measurement` time + static uint32_t oldmillis = 0; + double tnow = nanotime(); + 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); + mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow; + mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord; + mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord; + getXspeed(); getYspeed(); + if(tnow - oldmt > Conf.MountReqInterval){ + oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3); + oldmt = tnow; + }else mountdata->millis = oldmillis; +} + +/** + * 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]; + 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; + //DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator); + if(fabs(denominator) < 1e-7) return 0.; + double numerator = n * l->xtsum - l->xsum * l->tsum; + // point: (sum_x - slope * sum_t) / n; + return (numerator / denominator); +} + + /** * @brief init - open serial devices and do other job * @param c - initial configuration @@ -48,6 +153,12 @@ static mcc_errcodes_t init(conf_t *c){ if(!c) return MCC_E_BADFORMAT; Conf = *c; mcc_errcodes_t ret = MCC_E_OK; + Xmodel = model_init(&Xlimits); + Ymodel = model_init(&Ylimits); + if(Conf.RunModel){ + if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; + return MCC_E_OK; + } if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; @@ -72,10 +183,9 @@ static mcc_errcodes_t init(conf_t *c){ DBG("Wrong speed interval"); ret = MCC_E_BADFORMAT; } - uint8_t buf[1024]; - data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; - // read input data as there may be some trash on start - if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED; + //uint8_t buf[1024]; + //data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; + if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; return updateMotorPos(); } @@ -99,16 +209,34 @@ static int chkYs(double s){ return TRUE; } +// set SLEWING state if axis was stopped later +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!"); +} + +/* static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ (void)target; (void)flags; + //if(Conf.RunModel) return ... ; if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; //... - setStat(MNT_SLEWING, MNT_SLEWING); + setStat(AXIS_SLEWING, AXIS_SLEWING); //... return MCC_E_FAILED; } - +*/ /** * @brief move2 - simple move to given point and stop @@ -128,7 +256,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){ cmd.Yspeed = MCC_MAX_Y_SPEED; mcc_errcodes_t r = shortcmd(&cmd); if(r != MCC_E_OK) return r; - setStat(MNT_SLEWING, MNT_SLEWING); + setslewingstate(); return MCC_E_OK; } @@ -140,6 +268,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){ */ 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); @@ -165,7 +294,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed cmd.Yspeed = speed->Y; mcc_errcodes_t r = shortcmd(&cmd); if(r != MCC_E_OK) return r; - setStat(MNT_SLEWING, MNT_SLEWING); + setslewingstate(); return MCC_E_OK; } @@ -174,11 +303,25 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed * @return errcode */ static mcc_errcodes_t emstop(){ + FNAME(); + if(Conf.RunModel){ + double curt = nanotime(); + 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 = nanotime(); + Xmodel->stop(Xmodel, curt); + Ymodel->stop(Ymodel,curt); + return MCC_E_OK; + } if(!SSstop(FALSE)) return MCC_E_FAILED; return MCC_E_OK; } @@ -190,6 +333,16 @@ static mcc_errcodes_t stop(){ */ static mcc_errcodes_t shortcmd(short_command_t *cmd){ if(!cmd) return MCC_E_BADFORMAT; + if(Conf.RunModel){ + double curt = nanotime(); + 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); @@ -201,16 +354,27 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){ 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 shortcmd - send and receive long binary command + * @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 = nanotime(); + 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); @@ -221,11 +385,13 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){ 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; if(!cmdC(&config, FALSE)) return MCC_E_FAILED; // Convert acceleration (ticks per loop^2 to rad/s^2) @@ -266,8 +432,8 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ // Copy ticks per revolution hwConfig->Xsetpr = __bswap_32(config.Xsetpr); hwConfig->Ysetpr = __bswap_32(config.Ysetpr); - hwConfig->Xmetpr = __bswap_32(config.Xmetpr); - hwConfig->Ymetpr = __bswap_32(config.Ymetpr); + hwConfig->Xmetpr = __bswap_32(config.Xmetpr) / 4; // as documentation said, real ticks are 4 times less + hwConfig->Ymetpr = __bswap_32(config.Ymetpr) / 4; // Convert slew rates (ticks per loop to rad/s) hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); @@ -290,6 +456,7 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ 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); @@ -349,7 +516,7 @@ mount_t Mount = { .init = init, .quit = quit, .getMountData = getMD, - .slewTo = slew2, +// .slewTo = slew2, .moveTo = move2, .moveWspeed = move2s, .setSpeed = setspeed, @@ -359,5 +526,7 @@ mount_t Mount = { .longCmd = longcmd, .getHWconfig = get_hwconf, .saveHWconfig = write_hwconf, - .currentT = dtime, + .currentT = nanotime, + .correctTo = correct2, }; + diff --git a/Auxiliary_utils/LibSidServo/main.h b/Auxiliary_utils/LibSidServo/main.h new file mode 100644 index 0000000..0eb9fe2 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/main.h @@ -0,0 +1,80 @@ +/* + * 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 "sidservo.h" + +extern conf_t Conf; +double nanotime(); +void getModData(mountdata_t *mountdata); +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__)) +// break absent in `case` +#define FALLTHRU __attribute__ ((fallthrough)) +// and synonym for FALLTHRU +#define NOBREAKHERE __attribute__ ((fallthrough)) +// 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/Auxiliary_utils/LibSidServo/movingmodel.c b/Auxiliary_utils/LibSidServo/movingmodel.c new file mode 100644 index 0000000..83936ac --- /dev/null +++ b/Auxiliary_utils/LibSidServo/movingmodel.c @@ -0,0 +1,68 @@ +/* + * 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 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(model, target, t); +} diff --git a/Auxiliary_utils/LibSidServo/movingmodel.h b/Auxiliary_utils/LibSidServo/movingmodel.h new file mode 100644 index 0000000..d7b75bb --- /dev/null +++ b/Auxiliary_utils/LibSidServo/movingmodel.h @@ -0,0 +1,76 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#include + +#include "sidservo.h" + +// tolerance, time ticks +#define COORD_TOLERANCE_DEFAULT (1e-8) +#define COORD_TOLERANCE_MIN (1e-12) +#define COORD_TOLERANCE_MAX (10.) +#define TIME_TICK_DEFAULT (0.0001) +#define TIME_TICK_MIN (1e-9) +#define TIME_TICK_MAX (10.) + +typedef enum{ + ST_STOP, // stopped + ST_MOVE, // moving + ST_AMOUNT +} movestate_t; + +typedef struct{ + double coord; + double speed; + double accel; +} moveparam_t; + +typedef struct{ + moveparam_t min; + moveparam_t max; + double acceleration; +} limits_t; + +typedef enum{ + STAGE_ACCEL, // start from last speed and accelerate/decelerate to target speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +typedef struct movemodel{ + moveparam_t Min; + moveparam_t Max; + movingstage_t movingstage; + movestate_t state; + double *Times; + moveparam_t *Params; + moveparam_t curparams; // init values of limits, jerk + int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state + void (*stop)(struct movemodel *m, double t); // stop by ramp + void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration + double (*stoppedtime)(struct movemodel *m); // time when moving will ends + pthread_mutex_t mutex; +} movemodel_t; + +movemodel_t *model_init(limits_t *l); +int model_move2(movemodel_t *model, moveparam_t *target, double t); diff --git a/Auxiliary_utils/LibSidServo/ramp.c b/Auxiliary_utils/LibSidServo/ramp.c new file mode 100644 index 0000000..6e88293 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/ramp.c @@ -0,0 +1,258 @@ +/* + * 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(...) +#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]); + } + fflush(stdout); + 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/Auxiliary_utils/LibSidServo/ramp.h b/Auxiliary_utils/LibSidServo/ramp.h new file mode 100644 index 0000000..486a367 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/ramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model project. + * Copyright 2025 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "movingmodel.h" + +extern movemodel_t trapez; diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index b6d717d..8e7f82b 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -21,24 +21,26 @@ #include #include #include +#include #include #include #include #include #include #include -#include #include -#include "dbg.h" +#include "main.h" +#include "movingmodel.h" #include "serial.h" +#include "ssii.h" // serial devices FD static int encfd[2] = {-1, -1}, mntfd = -1; // main mount data static mountdata_t mountdata = {0}; // last encoders time and last encoders data - for speed measurement -static coordval_t lastXenc = {0}, lastYenc = {0}; +//static coordval_t lastXenc = {0}, lastYenc = {0}; // mutexes for RW operations with mount device and data static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, @@ -46,7 +48,7 @@ static pthread_mutex_t mntmutex = 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 = {0}, mntRtmout = {0}; +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 50000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -55,57 +57,43 @@ typedef struct __attribute__((packed)){ uint8_t CRC[4]; } enc_t; -/** - * @brief dtime - monotonic time from first run - * @return - */ -double dtime(){ - struct timespec start_time = {0}, cur_time; - if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){ - clock_gettime(CLOCK_MONOTONIC, &start_time); - } - clock_gettime(CLOCK_MONOTONIC, &cur_time); - return ((double)(cur_time.tv_sec - start_time.tv_sec) + - (cur_time.tv_nsec - start_time.tv_nsec) * 1e-9); -} -#if 0 -double dtime(){ - double t; - struct timeval tv; - gettimeofday(&tv, NULL); - t = tv.tv_sec + ((double)tv.tv_usec)/1e6; - return t; -} -#endif -#if 0 -double tv2d(struct timeval *tv){ - if(!tv) return 0.; - double t = tv->tv_sec + ((double)tv->tv_usec) / 1e6; - return t; -} -#endif -#if 0 -// init start time -static void gttime(){ - struct timeval tv; - gettimeofday(&tv, NULL); - tv_sec_got = tv.tv_sec; - tv_usec_got = tv.tv_usec; -} -#endif - // calculate current X/Y speeds -static void getXspeed(double t){ +void getXspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t); + if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ + mountdata.encXspeed.val = speed; + mountdata.encXspeed.t = mountdata.encXposition.t; + } + //DBG("Xspeed=%g", mountdata.encXspeed.val); +#if 0 mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t); - mountdata.encXspeed.t = (lastXenc.t + t) / 2.; + mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.; lastXenc.val = mountdata.encXposition.val; lastXenc.t = t; +#endif } -static void getYspeed(double t){ +void getYspeed(){ + static less_square_t *ls = NULL; + if(!ls){ + ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); + if(!ls) return; + } + double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t); + if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ + mountdata.encYspeed.val = speed; + mountdata.encYspeed.t = mountdata.encYposition.t; + } +#if 0 mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t); - mountdata.encYspeed.t = (lastYenc.t + t) / 2.; + mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.; lastYenc.val = mountdata.encYposition.val; lastYenc.t = t; +#endif } /** @@ -153,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); mountdata.encXposition.t = t; mountdata.encYposition.t = t; - if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t); - if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t); + //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); + //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); + 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); } @@ -170,7 +159,7 @@ static int getencval(int fd, double *val, double *t){ if(fd < 0) return FALSE; char buf[128]; int got = 0, Lmax = 127; - double t0 = dtime(); + double t0 = nanotime(); do{ fd_set rfds; FD_ZERO(&rfds); @@ -189,7 +178,7 @@ static int getencval(int fd, double *val, double *t){ buf[got] = 0; } else continue; if(strchr(buf, '\n')) break; - }while(Lmax && dtime() - t0 < Conf.EncoderReqInterval); + }while(Lmax && nanotime() - t0 < Conf.EncoderReqInterval); if(got == 0) return 0; // WTF? char *estr = strrchr(buf, '\n'); if(!estr) return 0; @@ -254,12 +243,40 @@ static int getmntbyte(){ if(FD_ISSET(mntfd, &rfds)){ ssize_t l = read(mntfd, &byte, 1); //DBG("MNT read=%zd byte=0x%X", l, byte); - if(l != 1) return -2; // disconnected ?? + 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; + //double t0 = nanotime(); + //int n = 0; + 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; + //++n; + } else break; + }while(1); + //DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n); +} // main encoder thread (for separate encoder): read next data and make parsing static void *encoderthread1(void _U_ *u){ @@ -277,7 +294,7 @@ static void *encoderthread1(void _U_ *u){ if((uint8_t)b == ENC_MAGICK){ // DBG("Got magic -> start filling packet"); databuf[wridx++] = (uint8_t) b; - t = dtime(); + t = nanotime(); } continue; }else databuf[wridx++] = (uint8_t) b; @@ -298,25 +315,27 @@ static void *encoderthread2(void _U_ *u){ if(Conf.SepEncoder != 2) return NULL; DBG("Thread started"); int errctr = 0; - double t0 = dtime(); - const char *req = "next\n"; + double t0 = nanotime(); + const char *req = "\n"; int need2ask = 0; // need or not to ask encoder for new data while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ if(need2ask){ - if(5 != write(encfd[0], req, 5)) { ++errctr; continue; } - else if(5 != write(encfd[1], req, 5)) { ++errctr; continue; } + if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } + else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } } double v, t; if(getencval(encfd[0], &v, &t)){ mountdata.encXposition.val = X_ENC2RAD(v); //DBG("encX(%g) = %g", t, mountdata.encXposition.val); mountdata.encXposition.t = t; - if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t); + //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); + getXspeed(); if(getencval(encfd[1], &v, &t)){ mountdata.encYposition.val = Y_ENC2RAD(v); //DBG("encY(%g) = %g", t, mountdata.encYposition.val); mountdata.encYposition.t = t; - if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t); + //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); + getYspeed(); errctr = 0; need2ask = 0; } else { @@ -329,9 +348,9 @@ static void *encoderthread2(void _U_ *u){ else need2ask = 1; continue; } - while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); } - //DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval); - t0 = dtime(); + while(nanotime() - t0 < Conf.EncoderReqInterval){ usleep(50); } + //DBG("DT=%g (RI=%g)", nanotime()-t0, Conf.EncoderReqInterval); + t0 = nanotime(); } DBG("ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ @@ -364,20 +383,49 @@ static void *mountthread(void _U_ *u){ int errctr = 0; uint8_t buf[2*sizeof(SSstat)]; SSstat *status = (SSstat*) buf; + bzero(&mountdata, sizeof(mountdata)); + if(Conf.RunModel) while(1){ + pthread_mutex_lock(&datamutex); + // now change data + getModData(&mountdata); + pthread_mutex_unlock(&datamutex); + double t0 = nanotime(); + while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50); + t0 = nanotime(); + } // 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; - double t0 = dtime(); -/* -#ifdef EBUG - double t00 = t0; -#endif -*/ while(mntfd > -1 && errctr < MAX_ERR_CTR){ // read data to status - double tgot = dtime(); + double t0 = nanotime(); +#if 0 +// 127 milliseconds to get answer on X/Y commands!!! + int64_t ans; + int ctr = 0; + if(SSgetint(CMD_MOTX, &ans)){ + pthread_mutex_lock(&datamutex); + mountdata.motXposition.t = tgot; + mountdata.motXposition.val = X_MOT2RAD(ans); + pthread_mutex_unlock(&datamutex); + ++ctr; + } + tgot = nanotime(); + if(SSgetint(CMD_MOTY, &ans)){ + pthread_mutex_lock(&datamutex); + mountdata.motXposition.t = tgot; + mountdata.motXposition.val = X_MOT2RAD(ans); + pthread_mutex_unlock(&datamutex); + ++ctr; + } + if(ctr == 2){ + mountdata.millis = (uint32_t)(1e3 * tgot); + DBG("Got both coords; millis=%d", mountdata.millis); + } +#endif + // 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); @@ -393,13 +441,14 @@ static void *mountthread(void _U_ *u){ errctr = 0; pthread_mutex_lock(&datamutex); // now change data - SSconvstat(status, &mountdata, tgot); + SSconvstat(status, &mountdata, t0); pthread_mutex_unlock(&datamutex); + //DBG("GOT FULL stat by %g", nanotime() - t0); // allow writing & getters - //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00); - if(dtime() - t0 >= Conf.MountReqInterval) usleep(50); - while(dtime() - t0 < Conf.MountReqInterval); - t0 = dtime(); + do{ + usleep(500); + }while(nanotime() - t0 < Conf.MountReqInterval); + t0 = nanotime(); } data_free(&cmd_getstat); failed: @@ -435,6 +484,7 @@ static int ttyopen(const char *path, speed_t speed){ // 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"); @@ -442,7 +492,7 @@ int openEncoder(){ encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed); if(encfd[0] < 0) return FALSE; encRtmout.tv_sec = 0; - encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; // 10 bytes if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ close(encfd[0]); encfd[0] = -1; @@ -472,6 +522,7 @@ int openEncoder(){ // 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); @@ -487,11 +538,14 @@ int openMount(){ DBG("got %zd", l); }while(1);*/ mntRtmout.tv_sec = 0; - mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes + mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed +create_thread: if(pthread_create(&mntthread, NULL, mountthread, NULL)){ - DBG("Can't create thread"); - close(mntfd); - mntfd = -1; + DBG("Can't create mount thread"); + if(!Conf.RunModel){ + close(mntfd); + mntfd = -1; + } return FALSE; } DBG("Mount opened, thread started"); @@ -500,21 +554,22 @@ int openMount(){ // close all opened serial devices and quit threads void closeSerial(){ + if(Conf.RunModel) return; if(mntfd > -1){ - DBG("Kill mount thread"); + DBG("Cancel mount thread"); pthread_cancel(mntthread); DBG("join mount thread"); pthread_join(mntthread, NULL); - DBG("close fd"); + DBG("close mount fd"); close(mntfd); mntfd = -1; } if(encfd[0] > -1){ - DBG("Kill encoder thread"); + DBG("Cancel encoder thread"); pthread_cancel(encthread); DBG("join encoder thread"); pthread_join(encthread, NULL); - DBG("close fd"); + DBG("close encoder's fd"); close(encfd[0]); encfd[0] = -1; if(Conf.SepEncoder == 2){ @@ -533,16 +588,22 @@ mcc_errcodes_t getMD(mountdata_t *d){ return MCC_E_OK; } -void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){ +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.Xstatus = Xstatus; - mountdata.Ystatus = Ystatus; + 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) return FALSE; + if((!out && !in) || mntfd < 0){ + DBG("Wrong arguments or no mount fd"); + return FALSE; + } + clrmntbuf(); + //double t0 = nanotime(); if(out){ if(out->len != (size_t)write(mntfd, out->buf, out->len)){ DBG("written bytes not equal to need"); @@ -554,16 +615,18 @@ static int wr(const data_t *out, data_t *in, int needeol){ (void) g; } } - uint8_t buf[256]; - data_t dumb = {.buf = buf, .maxlen = 256}; - if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash + //DBG("sent by %g", nanotime() - t0); + //uint8_t buf[256]; + //data_t dumb = {.buf = buf, .maxlen = 256}; + if(!in) return TRUE; + //if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash 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; } - //DBG("Clear trashing input"); + //DBG("got %zd bytes by %g", in->len, nanotime() - t0); while(getmntbyte() > -1); return TRUE; } @@ -575,6 +638,7 @@ static int wr(const data_t *out, data_t *in, int needeol){ * @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); @@ -582,6 +646,7 @@ int MountWriteRead(const data_t *out, data_t *in){ } // 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); @@ -605,32 +670,32 @@ static void loglcmd(SSlcmd *c){ // 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}; + //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, &a, 1)) goto rtn; + 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, &a, 1)) goto rtn; + if(!wr(dlcmd, NULL, 1)) goto rtn; }else{ goto rtn; } - DBG("Write %d bytes and wait for ans", len); data_t d; d.buf = cmd; d.len = d.maxlen = len; @@ -641,6 +706,7 @@ rtn: return ret; } +// short, long and config text-binary commands // return TRUE if OK int cmdS(SSscmd *cmd){ return bincmd((uint8_t *)cmd, sizeof(SSscmd)); @@ -650,6 +716,7 @@ int cmdL(SSlcmd *cmd){ } // 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 diff --git a/Auxiliary_utils/LibSidServo/serial.h b/Auxiliary_utils/LibSidServo/serial.h index b488f72..45694b1 100644 --- a/Auxiliary_utils/LibSidServo/serial.h +++ b/Auxiliary_utils/LibSidServo/serial.h @@ -28,16 +28,17 @@ // max error counter (when read() returns -1) #define MAX_ERR_CTR (100) -double dtime(); 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(mnt_status_t Xstatus, mnt_status_t Ystatus); +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/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index 9efdab0..e43bacf 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -16,6 +16,11 @@ * along with this program. If not, see . */ +/* + * This file contains all need for external usage + */ + + #pragma once #ifdef __cplusplus @@ -27,14 +32,37 @@ extern "C" #include #include +// acceptable position error - 0.1'' +#define MCC_POSITION_ERROR (5e-7) +// acceptable disagreement between motor and axis encoders - 2'' +#define MCC_ENCODERS_ERROR (1e-7) + // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s #define MCC_MAX_X_SPEED (0.174533) #define MCC_MAX_Y_SPEED (0.139626) +// accelerations by both axis (for model); TODO: move speeds/accelerations into config? +// xa=12.6 deg/s^2, ya= 9.5 deg/s^2 +#define MCC_X_ACCELERATION (0.219911) +#define MCC_Y_ACCELERATION (0.165806) // max speed interval, seconds #define MCC_CONF_MAX_SPEEDINT (2.) // minimal speed interval in parts of EncoderReqInterval #define MCC_CONF_MIN_SPEEDC (3.) +// PID I cycle time (analog of "RC" for PID on opamps) +#define MCC_PID_CYCLE_TIME (5.) +// maximal PID refresh time interval (if larger all old data will be cleared) +#define MCC_PID_MAX_DT (1.) +// normal PID refresh interval +#define MCC_PID_REFRESH_DT (0.1) +// boundary conditions for axis state: "slewing/pointing/guiding" +// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees +//#define MCC_MAX_POINTING_ERR (0.20943951) +#define MCC_MAX_POINTING_ERR (0.08726646) +// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg +#define MCC_MAX_GUIDING_ERR (0.026179939) +// if error less than this value we suppose that target is captured and guiding is good: 0.1'' +#define MCC_MAX_ATTARGET_ERR (4.8481368e-7) // error codes typedef enum{ @@ -47,16 +75,25 @@ typedef enum{ } mcc_errcodes_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 + 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 MountReqInterval; // interval between subsequent mount requests (seconds) - double EncoderReqInterval; // interval between subsequent encoder requests (seconds) - double EncoderSpeedInterval;// interval between speed calculations + 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 + 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; } conf_t; // coordinates/speeds in degrees or d/s: X, Y @@ -113,16 +150,16 @@ typedef struct{ } extradata_t; typedef enum{ - MNT_STOPPED, - MNT_SLEWING, - MNT_POINTING, - MNT_GUIDING, - MNT_ERROR, -} mnt_status_t; + AXIS_STOPPED, + AXIS_SLEWING, + AXIS_POINTING, + AXIS_GUIDING, + AXIS_ERROR, +} axis_status_t; typedef struct{ - mnt_status_t Xstatus; - mnt_status_t Ystatus; + axis_status_t Xstate; + axis_status_t Ystate; coordval_t motXposition; coordval_t motYposition; coordval_t encXposition; @@ -157,7 +194,7 @@ typedef struct{ double Yatime; // 28 } long_command_t; // long command -// hardware axe configuration +// hardware axis configuration typedef struct{ double accel; // Default Acceleration, rad/s^2 double backlash; // Backlash (???) @@ -168,13 +205,13 @@ typedef struct{ double outplimit; // Output Limit, percent (0..100) double currlimit; // Current Limit (A) double intlimit; // Integral Limit (???) -} __attribute__((packed)) axe_config_t; +} __attribute__((packed)) axis_config_t; // hardware configuration typedef struct{ - axe_config_t Xconf; + axis_config_t Xconf; xbits_t xbits; - axe_config_t Yconf; + axis_config_t Yconf; ybits_t ybits; uint8_t address; double eqrate; // Equatorial Rate (???) @@ -197,19 +234,19 @@ typedef struct{ double backlspd; // Backlash speed (rad/s) } hardware_configuration_t; -// flags for slew function +/* 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)(coordval_pair_t *target); +// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); + mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint); 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 @@ -224,7 +261,6 @@ typedef struct{ extern mount_t Mount; - #ifdef __cplusplus } #endif diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index e695c8d..51ed8b1 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -19,8 +19,9 @@ #include #include #include +#include -#include "dbg.h" +#include "main.h" #include "serial.h" #include "ssii.h" @@ -35,16 +36,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ return checksum; } - -static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){ +// 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 = MNT_STOPPED; - }else if(*stat != MNT_STOPPED){ - if(*prev == cur){ - if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED; + *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){ - //*stat = MNT_SLEWING; + DBG("AXIS moving"); *nstopped = 0; } *prev = cur; @@ -53,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta 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 - axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus); - axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus); + chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); + chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate); } /** @@ -65,13 +68,6 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){ */ void SSconvstat(const SSstat *s, mountdata_t *m, double t){ if(!s || !m) return; -/* -#ifdef EBUG - static double t0 = -1.; - if(t0 < 0.) t0 = dtime(); -#endif - DBG("Convert, t=%g", dtime()-t0); -*/ m->motXposition.val = X_MOT2RAD(s->Xmot); m->motYposition.val = Y_MOT2RAD(s->Ymot); ChkStopped(s, m); @@ -81,10 +77,8 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){ m->encXposition.val = X_ENC2RAD(s->Xenc); m->encYposition.val = Y_ENC2RAD(s->Yenc); m->encXposition.t = m->encYposition.t = t; + getXspeed(); getYspeed(); } - //m->lastmotposition.X = X_MOT2RAD(s->XLast); - //m->lastmotposition.Y = Y_MOT2RAD(s->YLast); - //m->lastmotposition.msrtime = *tdat; m->keypad = s->keypad; m->extradata.ExtraBits = s->ExtraBits; m->extradata.ain0 = s->ain0; @@ -110,7 +104,7 @@ int SStextcmd(const char *cmd, data_t *answer){ data_t d; d.buf = (uint8_t*) cmd; d.len = d.maxlen = strlen(cmd); - DBG("send %zd bytes: %s", d.len, d.buf); + //DBG("send %zd bytes: %s", d.len, d.buf); return MountWriteRead(&d, answer); } @@ -123,7 +117,7 @@ int SSrawcmd(const char *cmd, data_t *answer){ data_t d; d.buf = (uint8_t*) cmd; d.len = d.maxlen = strlen(cmd); - DBG("send %zd bytes: %s", d.len, d.buf); + //DBG("send %zd bytes: %s", d.len, d.buf); return MountWriteReadRaw(&d, answer); } @@ -180,22 +174,38 @@ int SSstop(int emerg){ // update motors' positions due to encoders' mcc_errcodes_t updateMotorPos(){ mountdata_t md = {0}; - double t0 = dtime(), t = 0.; + if(Conf.RunModel) return MCC_E_OK; + double t0 = nanotime(), t = 0.; DBG("start @ %g", t0); do{ - t = dtime(); + t = nanotime(); if(MCC_E_OK == getMD(&md)){ - DBG("got"); - if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){ - DBG("FIX motors position to encoders"); - int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val); - if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){ - DBG("All OK"); - return MCC_E_OK; - } - }else{ - DBG("on position"); - return MCC_E_OK; + if(md.encXposition.t == 0 || md.encYposition.t == 0){ + DBG("Just started, t-t0 = %g!", t - t0); + sleep(1); + DBG("t-t0 = %g", nanotime() - t0); + //usleep(10000); + continue; + } + DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t); + mcc_errcodes_t OK = MCC_E_OK; + if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ + DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, 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", nanotime() - t0); + } + if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ + DBG("NEED to sync Y: motors=%g, axiss=%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", nanotime() - t0); + } + if(MCC_E_OK == OK){ + DBG("Encoders synced"); + return OK; } } DBG("NO DATA; dt = %g", t - t0); diff --git a/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index 0db9251..504bb1b 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -16,6 +16,10 @@ * along with this program. If not, see . */ +/* + * This file contains stuff for sidereal-servo specific protocol + */ + #pragma once #include @@ -169,18 +173,32 @@ #define SITECH_LOOP_FREQUENCY (1953.) // amount of consequent same coordinates to detect stop -#define MOTOR_STOPPED_CNT (20) +#define MOTOR_STOPPED_CNT (4) +// TODO: take it from settings? // steps per revolution (SSI - x4 - for SSI) -// 13312000 / 4 = 3328000 #define X_MOT_STEPSPERREV_SSI (13312000.) -//#define X_MOT_STEPSPERREV (3325952.) +// 13312000 / 4 = 3328000 #define X_MOT_STEPSPERREV (3328000.) -// 17578668 / 4 = 4394667 #define Y_MOT_STEPSPERREV_SSI (17578668.) -//#define Y_MOT_STEPSPERREV (4394960.) +// 17578668 / 4 = 4394667 #define Y_MOT_STEPSPERREV (4394667.) +// encoder per revolution +#define X_ENC_STEPSPERREV (67108864.) +#define Y_ENC_STEPSPERREV (67108864.) +// encoder zero position +#define X_ENC_ZERO (61245239) +#define Y_ENC_ZERO (36999830) +// encoder reversed (no: +1) +#define X_ENC_SIGN (-1.) +#define Y_ENC_SIGN (-1.) +// encoder position to radians and back +#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) +#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) +#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) + // convert angle in radians to +-pi static inline double ang2half(double ang){ if(ang < -M_PI) ang += 2.*M_PI; @@ -214,21 +232,6 @@ static inline double ang2full(double ang){ #define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY) #define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) -// encoder per revolution -#define X_ENC_STEPSPERREV (67108864.) -#define Y_ENC_STEPSPERREV (67108864.) -// encoder zero position -#define X_ENC_ZERO (61245239) -#define Y_ENC_ZERO (36999830) -// encoder reversed (no: +1) -#define X_ENC_SIGN (-1.) -#define Y_ENC_SIGN (-1.) -// encoder position to radians and back -#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV) -#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) -#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) -#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) - // encoder's tolerance (ticks) #define YencTOL (25.) #define XencTOL (25.) @@ -334,5 +337,4 @@ 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); -int SSshortCmd(SSscmd *cmd); mcc_errcodes_t updateMotorPos(); diff --git a/Daemons/10micron_stellarium/main.c b/Daemons/10micron_stellarium/main.c index d5287cf..9b7a19e 100644 --- a/Daemons/10micron_stellarium/main.c +++ b/Daemons/10micron_stellarium/main.c @@ -226,6 +226,7 @@ int proc_data(uint8_t *data, ssize_t len){ // convert RA/DEC to hours/degrees double tagRA = RA2HRS(ra), tagDec = DEC2DEG(dec); DBG("RA: %u (%g), DEC: %d (%g)", ra, tagRA, dec, tagDec); + putlog("RA: %u (%g degr), DEC: %d (%g degr)", ra, tagRA, dec, tagDec); // check RA/DEC horizCrds hnow; // without refraction polarCrds p2000, pnow; @@ -236,7 +237,7 @@ int proc_data(uint8_t *data, ssize_t len){ WARNX("Can't convert coordinates to Jnow"); return 0; } -#ifdef EBUG +/* int i[4], j[4]; char pm, pm1; eraA2af(2, hnow.az, &pm, i); eraA2af(2, hnow.zd, &pm1, j); @@ -245,10 +246,10 @@ int proc_data(uint8_t *data, ssize_t len){ pm1,j[0],j[1],j[2],j[3]); eraA2af(2, M_PI_2 - hnow.zd, &pm, i); DBG("h: %c%02d %02d %02d.%2.d", pm, i[0],i[1],i[2],i[3]); -#endif +*/ if(hnow.zd > 80.*ERFA_DD2R){ - WARNX("Z > 80degr, stop telescope"); - putlog("Z>80 - stop!"); + WARNX("Z > 80degr (%g), stop telescope", hnow.zd * ERFA_DR2D); + putlog("Z=%.1f > 80 - stop!", hnow.zd * ERFA_DR2D); stop_telescope(); return 0; }