some fixes
This commit is contained in:
parent
9066b3f091
commit
6dde28e8d9
@ -14,8 +14,8 @@ set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
|
||||
|
||||
set(EXAMPLES OFF CACHE BOOL "" FORCE)
|
||||
# set(CMAKE_BUILD_TYPE "Release")
|
||||
add_subdirectory(LibSidServo)
|
||||
set(CMAKE_BUILD_TYPE "Debug")
|
||||
add_subdirectory(LibSidServo)
|
||||
|
||||
# add_subdirectory(cxx)
|
||||
add_subdirectory(mcc)
|
||||
|
||||
@ -84,41 +84,42 @@ typedef struct{
|
||||
* @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);
|
||||
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > Conf.PIDMaxDt){
|
||||
DBG("target time: %g, axis time: %g - too big! (tag-ax=%g)", tagpos->t, axis->position.t, tagpos->t - axis->position.t);
|
||||
return axis->speed.val; // data is too old or wrong
|
||||
}
|
||||
double error = tagpos->val - axis->position.val, fe = fabs(error);
|
||||
DBG("error: %g", error);
|
||||
PIDController_t *pid = NULL;
|
||||
switch(axis->state){
|
||||
case AXIS_SLEWING:
|
||||
if(fe < MCC_MAX_POINTING_ERR){
|
||||
if(fe < Conf.MaxPointingErr){
|
||||
axis->state = AXIS_POINTING;
|
||||
DBG("--> Pointing");
|
||||
pid = pidpair->PIDC;
|
||||
}else{
|
||||
DBG("Slewing...");
|
||||
return -1.; // max speed for given axis
|
||||
return NAN; // max speed for given axis
|
||||
}
|
||||
break;
|
||||
case AXIS_POINTING:
|
||||
if(fe < MCC_MAX_GUIDING_ERR){
|
||||
if(fe < Conf.MaxFinePoingintErr){
|
||||
axis->state = AXIS_GUIDING;
|
||||
DBG("--> Guiding");
|
||||
pid = pidpair->PIDV;
|
||||
}else if(fe > MCC_MAX_POINTING_ERR){
|
||||
}else if(fe > Conf.MaxPointingErr){
|
||||
DBG("--> Slewing");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return -1.;
|
||||
return NAN;
|
||||
} else pid = pidpair->PIDC;
|
||||
break;
|
||||
case AXIS_GUIDING:
|
||||
pid = pidpair->PIDV;
|
||||
if(fe > MCC_MAX_GUIDING_ERR){
|
||||
if(fe > Conf.MaxFinePoingintErr){
|
||||
DBG("--> Pointing");
|
||||
axis->state = AXIS_POINTING;
|
||||
pid = pidpair->PIDC;
|
||||
}else if(fe < MCC_MAX_ATTARGET_ERR){
|
||||
}else if(fe < Conf.MaxGuidingErr){
|
||||
DBG("At target");
|
||||
// TODO: we can point somehow that we are at target or introduce new axis state
|
||||
}else DBG("Current error: %g", fe);
|
||||
@ -126,7 +127,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
|
||||
case AXIS_STOPPED: // start pointing to target; will change speed next time
|
||||
DBG("AXIS STOPPED!!!!");
|
||||
axis->state = AXIS_SLEWING;
|
||||
return -1.;
|
||||
return NAN;
|
||||
case AXIS_ERROR:
|
||||
DBG("Can't move from erroneous state");
|
||||
return 0.;
|
||||
@ -135,12 +136,12 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
|
||||
DBG("WTF? Where is a PID?");
|
||||
return axis->speed.val;
|
||||
}
|
||||
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
|
||||
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > Conf.PIDMaxDt){
|
||||
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;
|
||||
if(dt > Conf.PIDMaxDt) dt = Conf.PIDCycleDt;
|
||||
pid->prevT = tagpos->t;
|
||||
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state);
|
||||
double tagspeed = pid_calculate(pid, error, dt);
|
||||
@ -154,22 +155,23 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
|
||||
* @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){
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target){
|
||||
static PIDpair_t pidX = {0}, pidY = {0};
|
||||
if(!pidX.PIDC){
|
||||
pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDC) return MCC_E_FATAL;
|
||||
pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidX.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
if(!pidY.PIDC){
|
||||
pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDC) return MCC_E_FATAL;
|
||||
pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT);
|
||||
pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt);
|
||||
if(!pidY.PIDV) return MCC_E_FATAL;
|
||||
}
|
||||
mountdata_t m;
|
||||
coordpair_t tagspeed;
|
||||
coordpair_t tagspeed; // absolute value of speed
|
||||
double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation)
|
||||
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
|
||||
axisdata_t axis;
|
||||
DBG("state: %d/%d", m.Xstate, m.Ystate);
|
||||
@ -177,20 +179,42 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
|
||||
axis.position = m.encXposition;
|
||||
axis.speed = m.encXspeed;
|
||||
tagspeed.X = getspeed(&target->X, &pidX, &axis);
|
||||
if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X;
|
||||
if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
|
||||
if(isnan(tagspeed.X)){ // max speed
|
||||
if(target->X.val < axis.position.val) Xsign = -1.;
|
||||
tagspeed.X = Xlimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; }
|
||||
if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed;
|
||||
}
|
||||
axis_status_t xstate = axis.state;
|
||||
axis.state = m.Ystate;
|
||||
axis.position = m.encYposition;
|
||||
axis.speed = m.encYspeed;
|
||||
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
|
||||
if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y;
|
||||
if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
|
||||
if(isnan(tagspeed.Y)){ // max speed
|
||||
if(target->Y.val < axis.position.val) Ysign = -1.;
|
||||
tagspeed.Y = Ylimits.max.speed;
|
||||
}else{
|
||||
if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; }
|
||||
if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed;
|
||||
}
|
||||
axis_status_t ystate = axis.state;
|
||||
if(m.Xstate != xstate || m.Ystate != ystate){
|
||||
DBG("State changed");
|
||||
setStat(xstate, ystate);
|
||||
}
|
||||
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
|
||||
return Mount.moveWspeed(endpoint, &tagspeed);
|
||||
coordpair_t endpoint;
|
||||
// allow at least PIDMaxDt moving with target speed
|
||||
double dv = fabs(tagspeed.X - m.encXspeed.val);
|
||||
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
|
||||
+ Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving
|
||||
+ tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping
|
||||
endpoint.X = m.encXposition.val + Xsign * adder;
|
||||
dv = fabs(tagspeed.Y - m.encYspeed.val);
|
||||
adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.)
|
||||
+ Conf.PIDMaxDt * tagspeed.Y
|
||||
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
|
||||
endpoint.Y = m.encYposition.val + Ysign * tagspeed.Y / Ylimits.max.accel;
|
||||
DBG("TAG speeds: %g/%g; TAG pos: %g/%g", tagspeed.X, tagspeed.Y, endpoint.X, endpoint.Y);
|
||||
return Mount.moveWspeed(&endpoint, &tagspeed);
|
||||
}
|
||||
|
||||
@ -37,4 +37,4 @@ 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);
|
||||
mcc_errcodes_t correct2(const coordval_pair_t *target);
|
||||
|
||||
@ -1,64 +0,0 @@
|
||||
/*
|
||||
* This file is part of the libsidservo project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
extern conf_t Conf;
|
||||
|
||||
// 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 <stdio.h>
|
||||
#define COLOR_RED "\033[1;31;40m"
|
||||
#define COLOR_GREEN "\033[1;32;40m"
|
||||
#define COLOR_OLD "\033[0;0;0m"
|
||||
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
|
||||
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
|
||||
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%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
|
||||
@ -24,25 +24,32 @@
|
||||
static conf_t Config = {
|
||||
.MountDevPath = "/dev/ttyUSB0",
|
||||
.MountDevSpeed = 19200,
|
||||
.EncoderXDevPath = "/dev/encoderX0",
|
||||
.EncoderYDevPath = "/dev/encoderY0",
|
||||
.EncoderXDevPath = "/dev/encoder_X0",
|
||||
.EncoderYDevPath = "/dev/encoder_Y0",
|
||||
.EncoderDevSpeed = 153000,
|
||||
.MountReqInterval = 0.1,
|
||||
.EncoderReqInterval = 0.05,
|
||||
.EncoderReqInterval = 0.001,
|
||||
.SepEncoder = 2,
|
||||
.EncoderSpeedInterval = 0.1,
|
||||
.XPIDC.P = 0.8,
|
||||
.EncoderSpeedInterval = 0.05,
|
||||
.EncodersDisagreement = 1e-5, // 2''
|
||||
.PIDMaxDt = 1.,
|
||||
.PIDRefreshDt = 0.1,
|
||||
.PIDCycleDt = 5.,
|
||||
.XPIDC.P = 0.5,
|
||||
.XPIDC.I = 0.1,
|
||||
.XPIDC.D = 0.3,
|
||||
.XPIDV.P = 1.,
|
||||
.XPIDV.I = 0.01,
|
||||
.XPIDV.D = 0.2,
|
||||
.YPIDC.P = 0.8,
|
||||
.XPIDC.D = 0.2,
|
||||
.XPIDV.P = 0.09,
|
||||
.XPIDV.I = 0.0,
|
||||
.XPIDV.D = 0.05,
|
||||
.YPIDC.P = 0.5,
|
||||
.YPIDC.I = 0.1,
|
||||
.YPIDC.D = 0.3,
|
||||
.YPIDV.P = 0.5,
|
||||
.YPIDV.I = 0.2,
|
||||
.YPIDV.D = 0.5,
|
||||
.YPIDC.D = 0.2,
|
||||
.YPIDV.P = 0.09,
|
||||
.YPIDV.I = 0.0,
|
||||
.YPIDV.D = 0.05,
|
||||
.MaxPointingErr = 0.13962634,
|
||||
.MaxFinePoingintErr = 0.026179939,
|
||||
.MaxGuidingErr = 4.8481368e-7,
|
||||
};
|
||||
|
||||
static sl_option_t opts[] = {
|
||||
@ -50,13 +57,17 @@ static sl_option_t opts[] = {
|
||||
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
|
||||
{"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"},
|
||||
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
|
||||
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
|
||||
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
|
||||
{"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
|
||||
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
|
||||
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
|
||||
{"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
|
||||
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
|
||||
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
|
||||
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
|
||||
{"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
|
||||
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
|
||||
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
|
||||
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
|
||||
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
|
||||
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
|
||||
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
|
||||
@ -69,6 +80,10 @@ static sl_option_t opts[] = {
|
||||
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
|
||||
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
|
||||
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
|
||||
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
|
||||
{"MaxFinePoingintErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePoingintErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
|
||||
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
|
||||
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
|
||||
end_option
|
||||
};
|
||||
|
||||
@ -93,5 +108,5 @@ void dumpConf(){
|
||||
}
|
||||
|
||||
void confHelp(){
|
||||
sl_showhelp(-1, opts);
|
||||
sl_conf_showhelp(-1, opts);
|
||||
}
|
||||
|
||||
@ -75,7 +75,7 @@ void logmnt(FILE *fcoords, mountdata_t *m){
|
||||
if(!fcoords) return;
|
||||
//DBG("LOG %s", m ? "data" : "header");
|
||||
if(!m){ // write header
|
||||
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
|
||||
fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
|
||||
return;
|
||||
}else if(dumpT0 < 0.) dumpT0 = m->encXposition.t;
|
||||
// write data
|
||||
|
||||
@ -73,6 +73,7 @@ int main(int argc, char **argv){
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
if(!Config){
|
||||
dumpConf();
|
||||
confHelp();
|
||||
return 1;
|
||||
}
|
||||
if(G.coordsoutput){
|
||||
|
||||
@ -139,8 +139,10 @@ static mcc_errcodes_t return2zero(){
|
||||
short_command_t cmd = {0};
|
||||
DBG("Try to move to zero");
|
||||
cmd.Xmot = 0.; cmd.Ymot = 0.;
|
||||
cmd.Xspeed = MCC_MAX_X_SPEED;
|
||||
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
||||
coordpair_t maxspd;
|
||||
if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
|
||||
cmd.Xspeed = maxspd.X;
|
||||
cmd.Yspeed = maxspd.Y;
|
||||
/*cmd.xychange = 1;
|
||||
cmd.XBits = 100;
|
||||
cmd.YBits = 20;*/
|
||||
|
||||
@ -44,6 +44,7 @@ typedef struct{
|
||||
char *conffile;
|
||||
} parameters;
|
||||
|
||||
static conf_t *Config = NULL;
|
||||
static FILE *fcoords = NULL, *errlog = NULL;
|
||||
static pthread_t dthr;
|
||||
static parameters G = {
|
||||
@ -96,8 +97,7 @@ static void runtraectory(traectory_fn tfn){
|
||||
if(!tfn) return;
|
||||
coordval_pair_t telXY;
|
||||
coordval_pair_t target;
|
||||
coordpair_t traectXY, endpoint;
|
||||
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
|
||||
coordpair_t traectXY;
|
||||
double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT();
|
||||
double tlastX = 0., tlastY = 0.;
|
||||
while(1){
|
||||
@ -113,11 +113,6 @@ static void runtraectory(traectory_fn tfn){
|
||||
if(!traectory_point(&traectXY, t)) break;
|
||||
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;
|
||||
if(t0 < 0.) t0 = dumpt0();
|
||||
else{
|
||||
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
|
||||
@ -126,8 +121,8 @@ static void runtraectory(traectory_fn tfn){
|
||||
if(errlog)
|
||||
fprintf(errlog, "%10.4f %10.4f %10.4f\n", telXY.X.t-t0, 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);
|
||||
if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
|
||||
while((t = Mount.currentT()) - tlast < Config->PIDRefreshDt) usleep(500);
|
||||
tlast = t;
|
||||
}
|
||||
WARNX("No next traectory point or emulation ends");
|
||||
@ -153,7 +148,7 @@ int main(int argc, char **argv){
|
||||
if(!(fcoords = fopen(G.coordsoutput, "w")))
|
||||
ERRX("Can't open %s", G.coordsoutput);
|
||||
}else fcoords = stdout;
|
||||
conf_t *Config = readServoConf(G.conffile);
|
||||
Config = readServoConf(G.conffile);
|
||||
if(!Config || G.dumpconf){
|
||||
dumpConf();
|
||||
return 1;
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 17.0.2, 2025-11-01T14:58:43. -->
|
||||
<!-- Written by QtCreator 18.0.0, 2025-11-27T17:22:09. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
@ -86,6 +86,7 @@
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
<value type="int" key="RcSync">0</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
@ -164,6 +165,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@ -198,6 +200,7 @@
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.UniqueId"></value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
@ -208,10 +211,6 @@
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
|
||||
@ -35,14 +35,17 @@
|
||||
conf_t Conf = {0};
|
||||
// parameters for model
|
||||
static movemodel_t *Xmodel, *Ymodel;
|
||||
// limits for model and/or real mount (in latter case data should be read from mount on init)
|
||||
// radians, rad/sec, rad/sec^2
|
||||
static limits_t
|
||||
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
|
||||
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
|
||||
limits_t
|
||||
Xlimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
|
||||
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
|
||||
Ylimits = {
|
||||
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
|
||||
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
|
||||
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
|
||||
;
|
||||
static mcc_errcodes_t shortcmd(short_command_t *cmd);
|
||||
|
||||
@ -58,7 +61,7 @@ double nanotime(){
|
||||
struct timespec start;
|
||||
if(clock_gettime(CLOCK_REALTIME, &start)) return -1.;
|
||||
t0 = (double)start.tv_sec + (double)start.tv_nsec * 1e-9
|
||||
- (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
|
||||
- (double)now.tv_sec - (double)now.tv_nsec * 1e-9;
|
||||
}
|
||||
return (double)now.tv_sec + (double)now.tv_nsec * 1e-9 + t0;
|
||||
}
|
||||
@ -136,7 +139,6 @@ double LS_calc_slope(less_square_t *l, double x, double t){
|
||||
return (numerator / denominator);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief init - open serial devices and do other job
|
||||
* @param c - initial configuration
|
||||
@ -149,11 +151,15 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
mcc_errcodes_t ret = MCC_E_OK;
|
||||
Xmodel = model_init(&Xlimits);
|
||||
Ymodel = model_init(&Ylimits);
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
if(Conf.RunModel){
|
||||
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
|
||||
if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){
|
||||
DBG("Define mount device path and speed");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}else if(!openMount()){
|
||||
@ -169,16 +175,11 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
ret = MCC_E_ENCODERDEV;
|
||||
}
|
||||
}
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
// TODO: read hardware configuration on init
|
||||
if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){
|
||||
DBG("Wrong speed interval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
//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();
|
||||
@ -187,19 +188,19 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
|
||||
// TODO fix to real limits!!!
|
||||
static int chkX(double X){
|
||||
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
|
||||
if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkY(double Y){
|
||||
if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
|
||||
if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkXs(double s){
|
||||
if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE;
|
||||
if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkYs(double s){
|
||||
if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE;
|
||||
if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
@ -246,8 +247,8 @@ static mcc_errcodes_t move2(const coordpair_t *target){
|
||||
DBG("x,y: %g, %g", target->X, target->Y);
|
||||
cmd.Xmot = target->X;
|
||||
cmd.Ymot = target->Y;
|
||||
cmd.Xspeed = MCC_MAX_X_SPEED;
|
||||
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
||||
cmd.Xspeed = Xlimits.max.speed;
|
||||
cmd.Yspeed = Ylimits.max.speed;
|
||||
mcc_errcodes_t r = shortcmd(&cmd);
|
||||
if(r != MCC_E_OK) return r;
|
||||
setslewingstate();
|
||||
@ -280,6 +281,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
||||
if(!target || !speed) return MCC_E_BADFORMAT;
|
||||
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
||||
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
|
||||
// updateMotorPos() here can make a problem; TODO: remove?
|
||||
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||
short_command_t cmd = {0};
|
||||
cmd.Xmot = target->X;
|
||||
@ -498,13 +500,33 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
||||
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
|
||||
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
|
||||
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
|
||||
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
|
||||
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
|
||||
config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4);
|
||||
config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4);
|
||||
// TODO - next
|
||||
(void) config;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// getters of max/min speed and acceleration
|
||||
mcc_errcodes_t maxspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.max.speed;
|
||||
v->Y = Ylimits.max.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t minspeed(coordpair_t *v){
|
||||
if(!v) return MCC_E_BADFORMAT;
|
||||
v->X = Xlimits.min.speed;
|
||||
v->Y = Ylimits.min.speed;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
mcc_errcodes_t acceleration(coordpair_t *a){
|
||||
if(!a) return MCC_E_BADFORMAT;
|
||||
a->X = Xlimits.max.accel;
|
||||
a->Y = Ylimits.max.accel;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// init mount class
|
||||
mount_t Mount = {
|
||||
.init = init,
|
||||
@ -522,5 +544,8 @@ mount_t Mount = {
|
||||
.saveHWconfig = write_hwconf,
|
||||
.currentT = nanotime,
|
||||
.correctTo = correct2,
|
||||
.getMaxSpeed = maxspeed,
|
||||
.getMinSpeed = minspeed,
|
||||
.getAcceleration = acceleration,
|
||||
};
|
||||
|
||||
|
||||
@ -28,6 +28,7 @@
|
||||
#include "sidservo.h"
|
||||
|
||||
extern conf_t Conf;
|
||||
extern limits_t Xlimits, Ylimits;
|
||||
double nanotime();
|
||||
void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst);
|
||||
typedef struct{
|
||||
@ -43,10 +44,6 @@ 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))
|
||||
|
||||
|
||||
@ -44,7 +44,7 @@ typedef struct{
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
double acceleration;
|
||||
//double acceleration;
|
||||
} limits_t;
|
||||
|
||||
typedef enum{
|
||||
|
||||
@ -68,7 +68,7 @@ void getXspeed(){
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
|
||||
if(fabs(speed) < 1.5 * Xlimits.max.speed){
|
||||
mountdata.encXspeed.val = speed;
|
||||
mountdata.encXspeed.t = mountdata.encXposition.t;
|
||||
}
|
||||
@ -91,7 +91,7 @@ void getYspeed(){
|
||||
}
|
||||
pthread_mutex_lock(&datamutex);
|
||||
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0);
|
||||
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
|
||||
if(fabs(speed) < 1.5 * Ylimits.max.speed){
|
||||
mountdata.encYspeed.val = speed;
|
||||
mountdata.encYspeed.t = mountdata.encYposition.t;
|
||||
}
|
||||
@ -149,10 +149,10 @@ 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;
|
||||
pthread_mutex_unlock(&datamutex);
|
||||
//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);
|
||||
}
|
||||
|
||||
@ -790,13 +790,14 @@ int cmdC(SSconfig *conf, int rw){
|
||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
||||
if(d.len != d.maxlen) return FALSE;
|
||||
if(d.len != d.maxlen){ ret = FALSE; goto rtn; }
|
||||
// simplest checksum
|
||||
uint16_t sum = 0;
|
||||
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
||||
if(sum != conf->checksum){
|
||||
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
||||
return FALSE;
|
||||
ret = FALSE;
|
||||
goto rtn;
|
||||
}
|
||||
}
|
||||
rtn:
|
||||
|
||||
@ -32,38 +32,13 @@ extern "C"
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
// 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)
|
||||
|
||||
// minimal serial speed of mount device
|
||||
#define MOUNT_BAUDRATE_MIN (1200)
|
||||
// max speed interval, seconds
|
||||
#define MCC_CONF_MAX_SPEEDINT (2.)
|
||||
// minimal speed interval in parts of EncoderReqInterval
|
||||
#define MCC_CONF_MIN_SPEEDC (3.)
|
||||
// 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": 8 degrees
|
||||
//#define MCC_MAX_POINTING_ERR (0.20943951)
|
||||
//#define MCC_MAX_POINTING_ERR (0.08726646)
|
||||
#define MCC_MAX_POINTING_ERR (0.13962634)
|
||||
// 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{
|
||||
@ -87,14 +62,21 @@ typedef struct{
|
||||
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
|
||||
char* EncoderXDevPath; // paths to new controller devices
|
||||
char* EncoderYDevPath;
|
||||
double EncodersDisagreement; // acceptable disagreement between motor and axis encoders
|
||||
double MountReqInterval; // interval between subsequent mount requests (seconds)
|
||||
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
|
||||
double EncoderSpeedInterval; // interval between speed calculations
|
||||
int RunModel; // == 1 if you want to use model instead of real mount
|
||||
double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared)
|
||||
double PIDRefreshDt; // normal PID refresh interval
|
||||
double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps)
|
||||
PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
|
||||
PIDpar_t XPIDV;
|
||||
PIDpar_t YPIDC;
|
||||
PIDpar_t YPIDV;
|
||||
double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees
|
||||
double MaxFinePoingintErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg
|
||||
double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''
|
||||
} conf_t;
|
||||
|
||||
// coordinates/speeds in degrees or d/s: X, Y
|
||||
@ -247,7 +229,7 @@ typedef struct{
|
||||
void (*quit)(); // deinit
|
||||
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
||||
// mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
|
||||
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
|
||||
mcc_errcodes_t (*correctTo)(const coordval_pair_t *target);
|
||||
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
|
||||
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
|
||||
mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
|
||||
@ -258,6 +240,9 @@ typedef struct{
|
||||
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
||||
mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration
|
||||
double (*currentT)(); // current time
|
||||
mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis
|
||||
mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//-
|
||||
mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration
|
||||
} mount_t;
|
||||
|
||||
extern mount_t Mount;
|
||||
|
||||
@ -183,21 +183,21 @@ mcc_errcodes_t updateMotorPos(){
|
||||
if(MCC_E_OK == getMD(&md)){
|
||||
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;
|
||||
}
|
||||
if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK;
|
||||
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){
|
||||
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && 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){
|
||||
if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == 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!");
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user