update pos, statuses
This commit is contained in:
parent
89b86f8b7f
commit
c862d160fe
@ -1,15 +1,3 @@
|
|||||||
1. WTF after closing?
|
1. PID: slew2
|
||||||
|
2. add model & config "model ON"
|
||||||
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 484): close fd
|
|
||||||
|
|
||||||
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 489): Kill encoder thread
|
|
||||||
|
|
||||||
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd
|
|
||||||
*** bit out of range 0 - FD_SETSIZE on fd_set ***: terminated
|
|
||||||
Aborted
|
|
||||||
|
|
||||||
|
|
||||||
2. Init: set "motors" positions due to "encoders" position
|
|
||||||
|
|
||||||
3. From time to time: repeat 2
|
|
||||||
|
|
||||||
|
|||||||
@ -59,6 +59,6 @@ extern conf_t Conf;
|
|||||||
fprintf(stderr, "\n");} while(0)
|
fprintf(stderr, "\n");} while(0)
|
||||||
|
|
||||||
#else // EBUG
|
#else // EBUG
|
||||||
#define FNAME() do{}while(0)
|
#define FNAME()
|
||||||
#define DBG(...) do{}while(0)
|
#define DBG(...)
|
||||||
#endif // EBUG
|
#endif // EBUG
|
||||||
|
|||||||
@ -16,6 +16,7 @@
|
|||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <usefull_macros.h>
|
#include <usefull_macros.h>
|
||||||
|
|
||||||
@ -50,6 +51,83 @@ static sl_option_t confopts[] = {
|
|||||||
end_option
|
end_option
|
||||||
};
|
};
|
||||||
|
|
||||||
|
static void dumpaxe(char axe, axe_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)
|
||||||
|
DUMPD(accel);
|
||||||
|
DUMPD(backlash);
|
||||||
|
DUMPD(errlimit);
|
||||||
|
DUMP(propgain);
|
||||||
|
DUMP(intgain);
|
||||||
|
DUMP(derivgain);
|
||||||
|
DUMP(outplimit);
|
||||||
|
DUMP(currlimit);
|
||||||
|
DUMP(intlimit);
|
||||||
|
#undef DUMP
|
||||||
|
#undef DUMPD
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dumpxbits(xbits_t *c){
|
||||||
|
#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
|
||||||
|
DUMPBIT(motrev);
|
||||||
|
DUMPBIT(motpolarity);
|
||||||
|
DUMPBIT(encrev);
|
||||||
|
DUMPBIT(dragtrack);
|
||||||
|
DUMPBIT(trackplat);
|
||||||
|
DUMPBIT(handpaden);
|
||||||
|
DUMPBIT(newpad);
|
||||||
|
DUMPBIT(guidemode);
|
||||||
|
#undef DUMPBIT
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dumpybits(ybits_t *c){
|
||||||
|
#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
|
||||||
|
DUMPBIT(motrev);
|
||||||
|
DUMPBIT(motpolarity);
|
||||||
|
DUMPBIT(encrev);
|
||||||
|
DUMPBIT(slewtrack);
|
||||||
|
DUMPBIT(digin_sens);
|
||||||
|
printf("Ydigin=%d\n", c->digin);
|
||||||
|
#undef DUMPBIT
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dumpHWconf(){
|
||||||
|
#undef STRUCTPAR
|
||||||
|
#define STRUCTPAR(p) (HW).p
|
||||||
|
#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
|
||||||
|
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
|
||||||
|
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
|
||||||
|
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
|
||||||
|
green("X axe configuration:\n");
|
||||||
|
dumpaxe('X', &HW.Xconf);
|
||||||
|
green("X bits:\n");
|
||||||
|
dumpxbits(&HW.xbits);
|
||||||
|
green("Y axe configuration:\n");
|
||||||
|
dumpaxe('Y', &HW.Yconf);
|
||||||
|
green("Y bits:\n");
|
||||||
|
dumpybits(&HW.ybits);
|
||||||
|
printf("address=%d\n", HW.address);
|
||||||
|
DUMP(eqrate);
|
||||||
|
DUMP(eqadj);
|
||||||
|
DUMP(trackgoal);
|
||||||
|
DUMPD(latitude);
|
||||||
|
DUMPU32(Xsetpr);
|
||||||
|
DUMPU32(Ysetpr);
|
||||||
|
DUMPU32(Xmetpr);
|
||||||
|
DUMPU32(Ymetpr);
|
||||||
|
DUMPD(Xslewrate);
|
||||||
|
DUMPD(Yslewrate);
|
||||||
|
DUMPD(Xpanrate);
|
||||||
|
DUMPD(Ypanrate);
|
||||||
|
DUMPD(Xguiderate);
|
||||||
|
DUMPD(Yguiderate);
|
||||||
|
DUMPU32(baudrate);
|
||||||
|
DUMPD(locsdeg);
|
||||||
|
DUMPD(locsspeed);
|
||||||
|
DUMPD(backlspd);
|
||||||
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
int main(int argc, char** argv){
|
||||||
sl_init();
|
sl_init();
|
||||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||||
@ -68,9 +146,11 @@ int main(int argc, char** argv){
|
|||||||
green("Got configuration:\n");
|
green("Got configuration:\n");
|
||||||
printf("%s\n", c);
|
printf("%s\n", c);
|
||||||
FREE(c);
|
FREE(c);
|
||||||
|
dumpHWconf();
|
||||||
|
/*
|
||||||
if(G.hwconffile && G.writeconf){
|
if(G.hwconffile && G.writeconf){
|
||||||
;
|
;
|
||||||
}
|
}*/
|
||||||
Mount.quit();
|
Mount.quit();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,6 +1,6 @@
|
|||||||
<?xml version="1.0" encoding="UTF-8"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<!DOCTYPE QtCreatorProject>
|
<!DOCTYPE QtCreatorProject>
|
||||||
<!-- Written by QtCreator 17.0.0, 2025-07-23T22:48:05. -->
|
<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
|
||||||
<qtcreator>
|
<qtcreator>
|
||||||
<data>
|
<data>
|
||||||
<variable>EnvironmentId</variable>
|
<variable>EnvironmentId</variable>
|
||||||
|
|||||||
@ -77,25 +77,7 @@ static mcc_errcodes_t init(conf_t *c){
|
|||||||
// read input data as there may be some trash on start
|
// read input data as there may be some trash on start
|
||||||
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
|
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
|
||||||
if(ret != MCC_E_OK) return ret;
|
if(ret != MCC_E_OK) return ret;
|
||||||
mountdata_t md = {0};
|
return updateMotorPos();
|
||||||
ret = MCC_E_FATAL;
|
|
||||||
double t0 = dtime(), t = 0.;
|
|
||||||
do{
|
|
||||||
t = dtime();
|
|
||||||
if(MCC_E_OK == getMD(&md)){
|
|
||||||
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");
|
|
||||||
ret = MCC_E_OK;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
DBG("NO DATA; dt = %g", t - t0);
|
|
||||||
}while(t - t0 < 2.);
|
|
||||||
return ret;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
|
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
|
||||||
@ -120,6 +102,10 @@ static int chkYs(double s){
|
|||||||
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
||||||
(void)target;
|
(void)target;
|
||||||
(void)flags;
|
(void)flags;
|
||||||
|
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||||
|
//...
|
||||||
|
setStat(MNT_SLEWING, MNT_SLEWING);
|
||||||
|
//...
|
||||||
return MCC_E_FAILED;
|
return MCC_E_FAILED;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -133,13 +119,17 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
|
|||||||
static mcc_errcodes_t move2(const coordpair_t *target){
|
static mcc_errcodes_t move2(const coordpair_t *target){
|
||||||
if(!target) return MCC_E_BADFORMAT;
|
if(!target) return MCC_E_BADFORMAT;
|
||||||
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
|
||||||
|
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||||
short_command_t cmd = {0};
|
short_command_t cmd = {0};
|
||||||
DBG("x,y: %g, %g", target->X, target->Y);
|
DBG("x,y: %g, %g", target->X, target->Y);
|
||||||
cmd.Xmot = target->X;
|
cmd.Xmot = target->X;
|
||||||
cmd.Ymot = target->Y;
|
cmd.Ymot = target->Y;
|
||||||
cmd.Xspeed = MCC_MAX_X_SPEED;
|
cmd.Xspeed = MCC_MAX_X_SPEED;
|
||||||
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
cmd.Yspeed = MCC_MAX_Y_SPEED;
|
||||||
return shortcmd(&cmd);
|
mcc_errcodes_t r = shortcmd(&cmd);
|
||||||
|
if(r != MCC_E_OK) return r;
|
||||||
|
setStat(MNT_SLEWING, MNT_SLEWING);
|
||||||
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -167,12 +157,16 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
|
|||||||
if(!target || !speed) return MCC_E_BADFORMAT;
|
if(!target || !speed) return MCC_E_BADFORMAT;
|
||||||
if(!chkX(target->X) || !chkY(target->Y)) 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;
|
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
|
||||||
|
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
|
||||||
short_command_t cmd = {0};
|
short_command_t cmd = {0};
|
||||||
cmd.Xmot = target->X;
|
cmd.Xmot = target->X;
|
||||||
cmd.Ymot = target->Y;
|
cmd.Ymot = target->Y;
|
||||||
cmd.Xspeed = speed->X;
|
cmd.Xspeed = speed->X;
|
||||||
cmd.Yspeed = speed->Y;
|
cmd.Yspeed = speed->Y;
|
||||||
return shortcmd(&cmd);
|
mcc_errcodes_t r = shortcmd(&cmd);
|
||||||
|
if(r != MCC_E_OK) return r;
|
||||||
|
setStat(MNT_SLEWING, MNT_SLEWING);
|
||||||
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -266,14 +260,14 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
hwConfig->address = config.address;
|
hwConfig->address = config.address;
|
||||||
|
|
||||||
// TODO: What to do with eqrate, eqadj and trackgoal?
|
// TODO: What to do with eqrate, eqadj and trackgoal?
|
||||||
|
config.latitude = __bswap_16(config.latitude);
|
||||||
// Convert latitude (degrees * 100 to radians)
|
// Convert latitude (degrees * 100 to radians)
|
||||||
hwConfig->latitude = (double)config.latitude / 100.0 * M_PI / 180.0;
|
hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
|
||||||
// Copy ticks per revolution
|
// Copy ticks per revolution
|
||||||
hwConfig->Xsetpr = config.Xsetpr;
|
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
|
||||||
hwConfig->Ysetpr = config.Ysetpr;
|
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
|
||||||
hwConfig->Xmetpr = config.Xmetpr;
|
hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
|
||||||
hwConfig->Ymetpr = config.Ymetpr;
|
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
|
||||||
// Convert slew rates (ticks per loop to rad/s)
|
// Convert slew rates (ticks per loop to rad/s)
|
||||||
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
|
||||||
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
|
||||||
@ -325,7 +319,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
config.xbits = hwConfig->xbits;
|
config.xbits = hwConfig->xbits;
|
||||||
config.ybits = hwConfig->ybits;
|
config.ybits = hwConfig->ybits;
|
||||||
// Convert latitude (radians to degrees * 100)
|
// Convert latitude (radians to degrees * 100)
|
||||||
config.latitude = (uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0);
|
config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
|
||||||
// Convert slew rates (rad/s to ticks per loop)
|
// Convert slew rates (rad/s to ticks per loop)
|
||||||
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
|
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
|
||||||
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
|
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
|
||||||
@ -341,6 +335,10 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
|
|||||||
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
|
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
|
||||||
// Convert backlash speed (rad/s to ticks per loop)
|
// Convert backlash speed (rad/s to ticks per loop)
|
||||||
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
|
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
|
||||||
|
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
|
||||||
|
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
|
||||||
|
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
|
||||||
|
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
|
||||||
// TODO - next
|
// TODO - next
|
||||||
(void) config;
|
(void) config;
|
||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
|
|||||||
@ -56,9 +56,19 @@ typedef struct __attribute__((packed)){
|
|||||||
} enc_t;
|
} enc_t;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief dtime - UNIX time with microsecond
|
* @brief dtime - monotonic time from first run
|
||||||
* @return value
|
* @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 dtime(){
|
||||||
double t;
|
double t;
|
||||||
struct timeval tv;
|
struct timeval tv;
|
||||||
@ -66,6 +76,7 @@ double dtime(){
|
|||||||
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
|
||||||
return t;
|
return t;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
#if 0
|
#if 0
|
||||||
double tv2d(struct timeval *tv){
|
double tv2d(struct timeval *tv){
|
||||||
if(!tv) return 0.;
|
if(!tv) return 0.;
|
||||||
@ -522,6 +533,13 @@ mcc_errcodes_t getMD(mountdata_t *d){
|
|||||||
return MCC_E_OK;
|
return MCC_E_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
|
||||||
|
pthread_mutex_lock(&datamutex);
|
||||||
|
mountdata.Xstatus = Xstatus;
|
||||||
|
mountdata.Ystatus = Ystatus;
|
||||||
|
pthread_mutex_unlock(&datamutex);
|
||||||
|
}
|
||||||
|
|
||||||
// write-read without locking mutex (to be used inside other functions)
|
// write-read without locking mutex (to be used inside other functions)
|
||||||
static int wr(const data_t *out, data_t *in, int needeol){
|
static int wr(const data_t *out, data_t *in, int needeol){
|
||||||
if((!out && !in) || mntfd < 0) return FALSE;
|
if((!out && !in) || mntfd < 0) return FALSE;
|
||||||
@ -598,12 +616,16 @@ static int bincmd(uint8_t *cmd, int len){
|
|||||||
if(len == sizeof(SSscmd)){
|
if(len == sizeof(SSscmd)){
|
||||||
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||||
DBG("Short command");
|
DBG("Short command");
|
||||||
|
#ifdef EBUG
|
||||||
logscmd((SSscmd*)cmd);
|
logscmd((SSscmd*)cmd);
|
||||||
|
#endif
|
||||||
if(!wr(dscmd, &a, 1)) goto rtn;
|
if(!wr(dscmd, &a, 1)) goto rtn;
|
||||||
}else if(len == sizeof(SSlcmd)){
|
}else if(len == sizeof(SSlcmd)){
|
||||||
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
|
||||||
DBG("Long command");
|
DBG("Long command");
|
||||||
|
#ifdef EBUG
|
||||||
loglcmd((SSlcmd*)cmd);
|
loglcmd((SSlcmd*)cmd);
|
||||||
|
#endif
|
||||||
if(!wr(dlcmd, &a, 1)) goto rtn;
|
if(!wr(dlcmd, &a, 1)) goto rtn;
|
||||||
}else{
|
}else{
|
||||||
goto rtn;
|
goto rtn;
|
||||||
|
|||||||
@ -35,6 +35,7 @@ int openEncoder();
|
|||||||
int openMount();
|
int openMount();
|
||||||
void closeSerial();
|
void closeSerial();
|
||||||
mcc_errcodes_t getMD(mountdata_t *d);
|
mcc_errcodes_t getMD(mountdata_t *d);
|
||||||
|
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
|
||||||
int MountWriteRead(const data_t *out, data_t *in);
|
int MountWriteRead(const data_t *out, data_t *in);
|
||||||
int MountWriteReadRaw(const data_t *out, data_t *in);
|
int MountWriteReadRaw(const data_t *out, data_t *in);
|
||||||
int cmdS(SSscmd *cmd);
|
int cmdS(SSscmd *cmd);
|
||||||
|
|||||||
@ -112,12 +112,21 @@ typedef struct{
|
|||||||
uint16_t ain1;
|
uint16_t ain1;
|
||||||
} extradata_t;
|
} extradata_t;
|
||||||
|
|
||||||
|
typedef enum{
|
||||||
|
MNT_STOPPED,
|
||||||
|
MNT_SLEWING,
|
||||||
|
MNT_POINTING,
|
||||||
|
MNT_GUIDING,
|
||||||
|
MNT_ERROR,
|
||||||
|
} mnt_status_t;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
|
mnt_status_t Xstatus;
|
||||||
|
mnt_status_t Ystatus;
|
||||||
coordval_t motXposition;
|
coordval_t motXposition;
|
||||||
coordval_t motYposition;
|
coordval_t motYposition;
|
||||||
coordval_t encXposition;
|
coordval_t encXposition;
|
||||||
coordval_t encYposition;
|
coordval_t encYposition;
|
||||||
// TODO: add speedX/Y
|
|
||||||
coordval_t encXspeed; // once per <config> s
|
coordval_t encXspeed; // once per <config> s
|
||||||
coordval_t encYspeed;
|
coordval_t encYspeed;
|
||||||
uint8_t keypad;
|
uint8_t keypad;
|
||||||
@ -127,10 +136,6 @@ typedef struct{
|
|||||||
double voltage;
|
double voltage;
|
||||||
} mountdata_t;
|
} mountdata_t;
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
;
|
|
||||||
} mountstat_t;
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
double Xmot; // 0 X motor position (rad)
|
double Xmot; // 0 X motor position (rad)
|
||||||
double Xspeed; // 4 X speed (rad/s)
|
double Xspeed; // 4 X speed (rad/s)
|
||||||
@ -194,7 +199,7 @@ typedef struct{
|
|||||||
|
|
||||||
// flags for slew function
|
// flags for slew function
|
||||||
typedef struct{
|
typedef struct{
|
||||||
uint32_t slewNguide : 1; // ==1 to gude after slewing
|
uint32_t slewNguide : 1; // ==1 to guide after slewing
|
||||||
} slewflags_t;
|
} slewflags_t;
|
||||||
|
|
||||||
// mount class
|
// mount class
|
||||||
@ -203,8 +208,6 @@ typedef struct{
|
|||||||
mcc_errcodes_t (*init)(conf_t *c); // init device
|
mcc_errcodes_t (*init)(conf_t *c); // init device
|
||||||
void (*quit)(); // deinit
|
void (*quit)(); // deinit
|
||||||
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
|
||||||
// TODO: change (or add flags) switching slew-and-stop and slew-and-track
|
|
||||||
// add mount state: stop/slew/guide
|
|
||||||
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
|
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
|
||||||
mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
|
mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
|
||||||
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
|
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
|
||||||
|
|||||||
@ -35,6 +35,28 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
|
|||||||
return checksum;
|
return checksum;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_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;
|
||||||
|
}
|
||||||
|
}else if(*prev != cur){
|
||||||
|
//*stat = MNT_SLEWING;
|
||||||
|
*nstopped = 0;
|
||||||
|
}
|
||||||
|
*prev = cur;
|
||||||
|
}
|
||||||
|
// check for stopped/pointing states
|
||||||
|
static void ChkStopped(const SSstat *s, mountdata_t *m){
|
||||||
|
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
|
||||||
|
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
|
||||||
|
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
|
||||||
|
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief SSconvstat - convert stat from SSII format to human
|
* @brief SSconvstat - convert stat from SSII format to human
|
||||||
* @param s (i) - just read data
|
* @param s (i) - just read data
|
||||||
@ -52,6 +74,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
|
|||||||
*/
|
*/
|
||||||
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
m->motXposition.val = X_MOT2RAD(s->Xmot);
|
||||||
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
m->motYposition.val = Y_MOT2RAD(s->Ymot);
|
||||||
|
ChkStopped(s, m);
|
||||||
m->motXposition.t = m->motYposition.t = t;
|
m->motXposition.t = m->motYposition.t = t;
|
||||||
// fill encoder data from here, as there's no separate enc thread
|
// fill encoder data from here, as there's no separate enc thread
|
||||||
if(!Conf.SepEncoder){
|
if(!Conf.SepEncoder){
|
||||||
@ -154,3 +177,28 @@ int SSstop(int emerg){
|
|||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// update motors' positions due to encoders'
|
||||||
|
mcc_errcodes_t updateMotorPos(){
|
||||||
|
mountdata_t md = {0};
|
||||||
|
double t0 = dtime(), t = 0.;
|
||||||
|
DBG("start @ %g", t0);
|
||||||
|
do{
|
||||||
|
t = dtime();
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
DBG("NO DATA; dt = %g", t - t0);
|
||||||
|
}while(t - t0 < 2.);
|
||||||
|
return MCC_E_FATAL;
|
||||||
|
}
|
||||||
|
|||||||
@ -168,6 +168,9 @@
|
|||||||
// Loop freq
|
// Loop freq
|
||||||
#define SITECH_LOOP_FREQUENCY (1953.)
|
#define SITECH_LOOP_FREQUENCY (1953.)
|
||||||
|
|
||||||
|
// amount of consequent same coordinates to detect stop
|
||||||
|
#define MOTOR_STOPPED_CNT (20)
|
||||||
|
|
||||||
// steps per revolution (SSI - x4 - for SSI)
|
// steps per revolution (SSI - x4 - for SSI)
|
||||||
// 13312000 / 4 = 3328000
|
// 13312000 / 4 = 3328000
|
||||||
#define X_MOT_STEPSPERREV_SSI (13312000.)
|
#define X_MOT_STEPSPERREV_SSI (13312000.)
|
||||||
@ -178,9 +181,22 @@
|
|||||||
//#define Y_MOT_STEPSPERREV (4394960.)
|
//#define Y_MOT_STEPSPERREV (4394960.)
|
||||||
#define Y_MOT_STEPSPERREV (4394667.)
|
#define Y_MOT_STEPSPERREV (4394667.)
|
||||||
|
|
||||||
|
// convert angle in radians to +-pi
|
||||||
|
static inline double ang2half(double ang){
|
||||||
|
if(ang < -M_PI) ang += 2.*M_PI;
|
||||||
|
else if(ang > M_PI) ang -= 2.*M_PI;
|
||||||
|
return ang;
|
||||||
|
}
|
||||||
|
// convert to only positive: 0..2pi
|
||||||
|
static inline double ang2full(double ang){
|
||||||
|
if(ang < 0.) ang += 2.*M_PI;
|
||||||
|
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
|
||||||
|
return ang;
|
||||||
|
}
|
||||||
|
|
||||||
// motor position to radians and back
|
// motor position to radians and back
|
||||||
#define X_MOT2RAD(n) (2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
|
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
|
||||||
#define Y_MOT2RAD(n) (2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
|
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
|
||||||
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
|
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV))
|
||||||
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
|
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
|
||||||
// motor speed in rad/s and back
|
// motor speed in rad/s and back
|
||||||
@ -202,15 +218,14 @@
|
|||||||
#define X_ENC_STEPSPERREV (67108864.)
|
#define X_ENC_STEPSPERREV (67108864.)
|
||||||
#define Y_ENC_STEPSPERREV (67108864.)
|
#define Y_ENC_STEPSPERREV (67108864.)
|
||||||
// encoder zero position
|
// encoder zero position
|
||||||
//#define X_ENC_ZERO (46033555)
|
#define X_ENC_ZERO (61245239)
|
||||||
#define X_ENC_ZERO (4603355)
|
#define Y_ENC_ZERO (36999830)
|
||||||
#define Y_ENC_ZERO (36674010)
|
|
||||||
// encoder reversed (no: +1)
|
// encoder reversed (no: +1)
|
||||||
#define X_ENC_SIGN (-1.)
|
#define X_ENC_SIGN (-1.)
|
||||||
#define Y_ENC_SIGN (-1.)
|
#define Y_ENC_SIGN (-1.)
|
||||||
// encoder position to radians and back
|
// encoder position to radians and back
|
||||||
#define X_ENC2RAD(n) (X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
|
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
|
||||||
#define Y_ENC2RAD(n) (Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_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 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))
|
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
|
||||||
|
|
||||||
@ -320,3 +335,4 @@ int SSgetint(const char *cmd, int64_t *ans);
|
|||||||
int SSsetterI(const char *cmd, int32_t ival);
|
int SSsetterI(const char *cmd, int32_t ival);
|
||||||
int SSstop(int emerg);
|
int SSstop(int emerg);
|
||||||
int SSshortCmd(SSscmd *cmd);
|
int SSshortCmd(SSscmd *cmd);
|
||||||
|
mcc_errcodes_t updateMotorPos();
|
||||||
|
|||||||
@ -38,12 +38,12 @@ static pars G = {
|
|||||||
};
|
};
|
||||||
|
|
||||||
static limits_t limits = {
|
static limits_t limits = {
|
||||||
.min = {.coord = -1e6, .speed = 0.1, .accel = 0.1},
|
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
|
||||||
.max = {.coord = 1e6, .speed = 1e3, .accel = 50.},
|
.max = {.coord = 1e6, .speed = 20., .accel = 9.53523},
|
||||||
.jerk = 10.
|
.jerk = 10.
|
||||||
};
|
};
|
||||||
|
|
||||||
static myoption opts[] = {
|
static sl_option_t opts[] = {
|
||||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
{"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"},
|
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
|
||||||
{"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"},
|
{"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"},
|
||||||
@ -84,9 +84,9 @@ static void monit(double tnext){
|
|||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
int main(int argc, char **argv){
|
||||||
initial_setup();
|
sl_init();
|
||||||
parseargs(&argc, &argv, opts);
|
sl_parseargs(&argc, &argv, opts);
|
||||||
if(G.help) showhelp(-1, opts);
|
if(G.help) sl_showhelp(-1, opts);
|
||||||
if(G.xlog){
|
if(G.xlog){
|
||||||
coordslog = fopen(G.xlog, "w");
|
coordslog = fopen(G.xlog, "w");
|
||||||
if(!coordslog) ERR("Can't open %s", G.xlog);
|
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||||
@ -101,8 +101,9 @@ int main(int argc, char **argv){
|
|||||||
model = init_moving(ramp, &limits);
|
model = init_moving(ramp, &limits);
|
||||||
if(!model) ERRX("Can't init moving model: check parameters");
|
if(!model) ERRX("Can't init moving model: check parameters");
|
||||||
Tstart = nanot();
|
Tstart = nanot();
|
||||||
moveparam_t target = {.speed = 10., .coord = 20.};
|
moveparam_t target = {.speed = 8.0, .coord = 90.};
|
||||||
if(move(&target)) monit(0.5);
|
if(move(&target)) monit(60.);
|
||||||
|
/*
|
||||||
for(int i = 0; i < 10; ++i){
|
for(int i = 0; i < 10; ++i){
|
||||||
target.coord = -target.coord;
|
target.coord = -target.coord;
|
||||||
if(move(&target)) monit(1.);
|
if(move(&target)) monit(1.);
|
||||||
@ -119,6 +120,7 @@ int main(int argc, char **argv){
|
|||||||
target.coord = 0.; target.speed = 20.;
|
target.coord = 0.; target.speed = 20.;
|
||||||
if(move(&target)) monit(1e6);
|
if(move(&target)) monit(1e6);
|
||||||
usleep(5000);
|
usleep(5000);
|
||||||
|
*/
|
||||||
fclose(coordslog);
|
fclose(coordslog);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user