mirror of
https://github.com/eddyem/mmpp.git
synced 2025-12-06 02:25:15 +03:00
265 lines
8.8 KiB
C
265 lines
8.8 KiB
C
/*
|
|
* This file is part of the libmmpp project.
|
|
* Copyright 2019 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/>.
|
|
*/
|
|
|
|
/******************************************************************************
|
|
* Motors testing tool *
|
|
******************************************************************************/
|
|
|
|
#include "tmcmdlnopts.h"
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
#include <libmmpp.h>
|
|
#include <usefull_macros.h>
|
|
|
|
// All return states of main():
|
|
enum{
|
|
RET_ALLOK = 0,
|
|
RET_NOTFOUND, // none of MCU found or didn't found seeking MCU
|
|
RET_ONLYONE, // only one MCU found
|
|
RET_COMMERR, // communication error
|
|
RET_CANTINIT, // can't init motors: error during initiation or some of motors are moving
|
|
RET_WAITERR, // error occured during waiting procedure
|
|
RET_ERROR = 9, // uncoverable error - from libsnippets
|
|
RET_HELPCALL = 255 // user call help (or give wrong parameter[s]) - from libsnippets
|
|
};
|
|
static glob_pars *G;
|
|
|
|
/**
|
|
* We REDEFINE the default WEAK function of signal processing
|
|
*/
|
|
void __attribute__((noreturn)) signals(int sig){
|
|
if(sig){
|
|
signal(sig, SIG_IGN);
|
|
DBG("Get signal %d, quit.\n", sig);
|
|
}
|
|
if(G->pidfile) // remove unnesessary PID file
|
|
unlink(G->pidfile);
|
|
restore_console();
|
|
mmpp_close();
|
|
exit(sig);
|
|
}
|
|
|
|
static double convangle(double val){
|
|
int X = (int)(val / 360.);
|
|
val -= 360. * (double)X;
|
|
return val;
|
|
}
|
|
|
|
static void parsestatus(ttysend_status st){
|
|
if(quiet) return;
|
|
switch(st){
|
|
case SEND_ERR:
|
|
red("communication error");
|
|
break;
|
|
case SEND_ALLOK:
|
|
green("all OK");
|
|
break;
|
|
case SEND_ACTIVE:
|
|
red("motor is still moving");
|
|
break;
|
|
case SEND_TOOBIG:
|
|
red("the steps amount is too large");
|
|
break;
|
|
case SEND_ZEROMOVE:
|
|
green("already at position");
|
|
break;
|
|
case SEND_ESWITCH:
|
|
red("on end-switch and can't move further");
|
|
break;
|
|
case SEND_NEEDINIT:
|
|
red("motors aren't initialised");
|
|
break;
|
|
case SEND_NEGATMOVE:
|
|
red("try to move into negative position");
|
|
break;
|
|
case SEND_OTHER:
|
|
default:
|
|
red("other error (wrong motor/MCU number?)");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
static void pollstatus(){
|
|
motor_state S;
|
|
bool mvng[3] = {false, true, true}, starting = true;
|
|
int curpos[4];
|
|
double adc[6];
|
|
while(mvng[1] || mvng[2]){
|
|
for(int Nmcu = 1; Nmcu < 3; ++Nmcu){
|
|
if(!mot_getstatus(Nmcu, &S)){
|
|
mvng[Nmcu] = false;
|
|
continue;
|
|
}
|
|
if(S.state[0] == STP_SLEEP && S.state[1] == STP_SLEEP)
|
|
mvng[Nmcu] = false;
|
|
curpos[2*(Nmcu-1)+0] = S.curpos[0];
|
|
curpos[2*(Nmcu-1)+1] = S.curpos[1];
|
|
if(G->getADC){
|
|
ADC_state s;
|
|
for(int Nmcu = 1; Nmcu < 3; ++Nmcu){
|
|
if(get_ADC(Nmcu, &s)){
|
|
adc[3*(Nmcu-1)+0] = s.Vdd;
|
|
adc[3*(Nmcu-1)+1] = s.Imot;
|
|
adc[3*(Nmcu-1)+2] = s.Vmot;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if(!mvng[1] && !mvng[2]){
|
|
if(starting){
|
|
starting = false;
|
|
continue;
|
|
}
|
|
}else starting = false;
|
|
if(!quiet){
|
|
printf("1: %6d, %6d", curpos[0],curpos[1]);
|
|
if(G->getADC){
|
|
printf(", VDD=%-5.1f Imot=%-5.1f Vmot=%-5.1f", adc[0], adc[1], adc[2]);
|
|
}
|
|
printf(" 2: %6d, %6d", curpos[2],curpos[3]);
|
|
if(G->getADC){
|
|
printf(", VDD=%-5.1f Imot=%-5.1f Vmot=%-5.1f", adc[3], adc[4], adc[5]);
|
|
}
|
|
printf(" \r");
|
|
}
|
|
}
|
|
printf("\n\n");
|
|
}
|
|
|
|
int main(int argc, char **argv){
|
|
initial_setup();
|
|
signal(SIGTERM, signals); // kill (-15)
|
|
signal(SIGINT, signals); // ctrl+C
|
|
signal(SIGQUIT, SIG_IGN); // ctrl+\ .
|
|
signal(SIGTSTP, SIG_IGN); // ctrl+Z
|
|
setbuf(stdout, NULL);
|
|
G = parse_args(argc, argv);
|
|
check4running(NULL, G->pidfile);
|
|
DBG("Try to open serial %s", G->comdev);
|
|
if(mmpp_tryopen(G->comdev, G->speed)){
|
|
ERRX(_("Can't open %s with speed %d. Exit."), G->comdev, G->speed);
|
|
}
|
|
if(mot_handshake()){
|
|
WARNX(_("Controllers not found"));
|
|
signals(RET_NOTFOUND);
|
|
}
|
|
for(int i = 1; i < 3; ++i){
|
|
if(get_alive(i)){
|
|
if(!quiet) green("MCU #%d found\n", i);
|
|
}else WARNX(_("MCU #%d not found"), i);
|
|
}
|
|
if(G->stopall){
|
|
int r = stop_all();
|
|
if(r) WARNX(_("Error for %d motors of 4"), r);
|
|
else MSG("Successfully send command to stop all", NULL);
|
|
}
|
|
if(G->gettemp){
|
|
double t1, t2;
|
|
if(get_temp(&t1, &t2)){
|
|
MSG("Got MCU temp:", NULL);
|
|
if(t1 > -300.) printf("\tTMCU1=%gdegrC\n", t1);
|
|
if(t2 > -300.) printf("\tTMCU2=%gdegrC\n", t2);
|
|
}else WARNX(_("Can't get MCU temp"));
|
|
}
|
|
if(G->getstatus){
|
|
for(int N = 1; N < 3; ++N){
|
|
motor_state s;
|
|
if(mot_getstatus(N, &s)){
|
|
if(get_rst(N, true))
|
|
WARNX(_("Controller #%d was in reset state"), N);
|
|
if(!quiet) green("MCU#%d state:\n", N);
|
|
for(int i = 0; i < 2; ++i){
|
|
printf("\tSTATE[%d]=%d\n",i, s.state[i]);
|
|
printf("\tSTEPSLEFT[%d]=%d\n", i, s.stepsleft[i]);
|
|
printf("\tCURPOS[%d]=%d\n", i, s.curpos[i]);
|
|
for(int j = 0; j < 2; ++j)
|
|
printf("\tESW_STATE[%d][%d]=%d\n", i, j, s.ESW_status[i][j]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if(G->sendraw){
|
|
char **raw = G->sendraw;
|
|
do{
|
|
MSG(_("Send raw string"), *raw);
|
|
char *got = tty_sendraw(*raw);
|
|
if(got){
|
|
MSG(_("Receive"), got);
|
|
if(quiet) printf("%s", got);
|
|
}else WARNX(_("Nothing received"));
|
|
}while(*(++raw));
|
|
}
|
|
if(G->getADC){
|
|
MSG(_("Get ADC values"), NULL);
|
|
ADC_state s;
|
|
for(int i = 1; i < 3; ++i){
|
|
if(get_ADC(i, &s)){
|
|
printf("VDD%d=%g\n", i, s.Vdd);
|
|
printf("IMOT%d=%g\n", i, s.Imot);
|
|
printf("VMOT%d=%g\n", i, s.Vmot);
|
|
}else WARNX(_("Can't get ADC values for MCU#%d"), i);
|
|
}
|
|
}
|
|
if(G->rot1angle > -999. || G->rot2angle > -999. || G->l1steps != INT_MAX || G->l2steps != INT_MAX){
|
|
// all other commands are tied with moving, so check if motors are inited
|
|
MSG("Init motors", NULL);
|
|
// move all uninitialized motors to their zero position
|
|
int ini = init_motors(); // BLOCKING call!!!
|
|
// you can use non-blocking initialisation by proper rewriting of `init_motors`
|
|
if(ini){
|
|
WARNX(_("Can't init motors: %d\n"), ini);
|
|
signals(RET_CANTINIT);
|
|
}else green("Motors are ready!\n");
|
|
ttysend_status st;
|
|
if(G->rot1angle > -999.){
|
|
double angle = convangle(G->rot1angle);
|
|
int steps = (int)((STEPSREV1/360.) * angle);
|
|
MSG("Try to rotate polaroid ...", NULL);
|
|
st = movemotor(1, 1, steps, G->absmove);
|
|
parsestatus(st);
|
|
}
|
|
if(G->rot2angle > -999.){
|
|
double angle = convangle(G->rot2angle);
|
|
int steps = (int)((STEPSREV2/360.) * angle);
|
|
MSG("Try to rotate waveplate ...", NULL);
|
|
st = movemotor(2, 1, steps, G->absmove);
|
|
parsestatus(st);
|
|
}
|
|
if(G->l1steps != INT_MAX){
|
|
MSG("Try to move polaroid stage ...", NULL);
|
|
st = movemotor(1, 0, G->l1steps, G->absmove);
|
|
parsestatus(st);
|
|
}
|
|
if(G->l2steps != INT_MAX){
|
|
MSG("Try to move waveplate stage ...", NULL);
|
|
st = movemotor(2, 0, G->l2steps, G->absmove);
|
|
parsestatus(st);
|
|
}
|
|
}
|
|
pollstatus();
|
|
if(G->reset){
|
|
int **N = G->reset;
|
|
while(*N){
|
|
if(!quiet) green("Reset controller #%d\n", **N);
|
|
reset_MCU(**N);
|
|
++N;
|
|
}
|
|
}
|
|
signals(0);
|
|
}
|