eddy/mmpp
1
0
Fork 0
mirror of https://github.com/eddyem/mmpp.git synced 2025-12-07 11:05:17 +03:00
Code Issues Packages Projects Releases Wiki Activity
mmpp/MMPP_control/new/tty_procs.c

531 lines
14 KiB
C
Raw Blame History

/*
* geany_encoding=koi8-r
* tty_procs.c
*
* Copyright 2018 Edward V. Emelianov <eddy@sao.ru, 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 2 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, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
* MA 02110-1301, USA.
*
*/
#include "cmdlnopts.h" // for `verbose`
#include "tty_procs.h"
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <usefull_macros.h>
// tty Rx static buffer
#define TBUFLEN (1024)
// read timeout (in seconds)
#define TTYTIMEOUT (0.05)
static TTY_descr *dev = NULL;
// two buffers for data
static char buf[TBUFLEN+1];
static char bufo[TBUFLEN+1];
static int motpos[3][2]; // motor's positions, 1 - number of mcu, 2 - number of motor
static int reset[3]; // reset occured
static int endswitches[3][2]; // ESW of motors: 0 - zero esw, 1 - limit esw, -1 - clear
static int alive[3] = {0,0,0}; // ==1 if controller answers, 0 if no
/**
* @brief tty_tryopen - try to open serial device
* @param devnm - path to device
* @param spd - speed (number)
* @return 0 if all OK
*/
int tty_tryopen(char *devnm, int spd){
dev = new_tty(devnm, spd, 256);
if(!tty_open(dev, TRUE)) return 1;
return 0;
}
void tty_close(){
close_tty(&dev);
}
/**
* read data from TTY
* WARNING! Not thread-safe!!!
* @return static buffer with data read or NULL
*/
static char *tty_get(){
char *ptr = buf;
size_t L = 0, l = TBUFLEN;
double t0 = dtime();
*ptr = 0;
while(dtime() - t0 < TTYTIMEOUT && l){
size_t r = read_tty(dev);
if(!r) continue;
t0 = dtime();
if(r > l) r = l;
DBG("got %zd bytes: %s", r, dev->buf);
strncpy(ptr, dev->buf, r);
L += r; l -= r; ptr += r;
}
buf[L] = 0;
if(L){
return buf;
}
DBG("no answer");
return NULL;
}
/**
* copy given string to `buf` & add '\n' if need
* @return 0 if failed
*/
static size_t cpy2buf(char *string){
size_t l = strlen(string);
if(l > TBUFLEN-1){
WARNX(_("String too long! Nothing would be send."));
return 0;
}
strcpy(bufo, string);
if(bufo[l-1] != '\n'){
bufo[l++] = '\n';
bufo[l] = 0;
}
return l;
}
/**
* Send given string command to port
* @return 0 if failed
*/
static int ttysend(char *cmd){
size_t l = cpy2buf(cmd);
if(!l) return 0;
if(write_tty(dev->comfd, bufo, l)) return 0;
return 1;
}
/**
* send RAW string to port device
* @param string - string to send
* @return string received or NULL in case of error
*/
char* tty_sendraw(char *string){
DBG("sendraw %s", string);
if(!ttysend(string)) return NULL;
return tty_get();
}
/**
* Send given string command to port with answer analysis
* @return status
*/
ttysend_status tty_sendcmd(char *cmd){
DBG("SEND: %s", cmd);
if(!ttysend(cmd)) return SEND_ERR;
char *got = tty_get();
if(!got) return SEND_ERR;
DBG("GOT: %s", got);
if(strcmp(got, "ALLOK\n") == 0) return SEND_ALLOK;
if(strcmp(got, "OnEndSwitch\n") == 0) return SEND_ESWITCH;
return SEND_OTHER;
}
/**
* return static buffer - value of `key`
* NOT THREAD SAFE!
*/
static char *keyval(char *key){
//DBG("search %s in\n%s", key, buf);
static char buff[32];
char *got = strstr(buf, key);
if(!got) return NULL;
got = strchr(got, '=');
if(!got) return NULL;
++got;
char *el = strchr(got, '\n');
if(!el) return NULL;
size_t L = (size_t)(el - got);
if(L > 31 || L == 0 || !*got) return NULL;
strncpy(buff, got, L);
buff[L] = 0;
return buff;
}
/**
* parse status with given command `cmd`
* @return:
* 1 if one of motors still moving
* 0 if both are stopped
* -1 if failed
*/
static int parsestatus(char *cmd){
if(!cmd) return 0;
if(!tty_sendraw(cmd)) return -1;
char buff[32], stat[2][5], left[2][7], pos[2][7];
int mvng = 0;
for(int i = 0; i < 2; ++i){
sprintf(buff, "POS%d", i);
char *val = keyval(buff);
if(!val) return -1;
strncpy(pos[i], val, 7);
sprintf(buff, "MOTOR%d", i);
val = keyval(buff);
if(!val) return -1;
if(strcmp(val, "SLEEP") == 0){
strcpy(stat[i], "STOP");
strcpy(left[i], "0");
continue;
}else if(strcmp(val, "UNKNOWN")){
strcpy(stat[i], "MOVE");
mvng = 1;
sprintf(buff, "STEPSLEFT%d", i);
val = keyval(buff);
if(!val) return -1;
strncpy(left[i], val, 7);
}else return -1;
}
snprintf(bufo, TBUFLEN, "%4s %6s %6s - %4s %6s %6s",
stat[0], left[0], pos[0], stat[1], left[1], pos[1]);
if(mvng){
return 1;
} else return 0;
}
/**
* Wait for all motors stop with current data indication
* @return 0 if all OK
*/
int tty_wait(){
FNAME();
int failcount = 0, chk1 = 1, chk2 = 1;
if(!quiet)
green("Pol: M0ST M0LEFT M0POS - M1ST M1LEFT M1POS || L/4: M0ST M0LEFT M0POS - M1ST M1LEFT M1POS \n");
while(failcount < 5 && (chk1 || chk2)){ // 5 tries
if(alive[1]){
chk1 = parsestatus("1GS");
if(!quiet){
printf("Pol: ");
if(chk1 == -1){
chk1 = 1;
++failcount;
printf("%39s", "failed");
}else printf("%s", bufo);
}
}else if(!quiet) printf("%39s", "failed");
if(alive[2]){
chk2 = parsestatus("2GS");
if(!quiet){
printf(" || L/4: ");
if(chk2 == -1){
chk2 = 1;
++failcount;
printf("%39s", "failed");
}else printf("%s", bufo);
printf(" \r");
}
}else if(!quiet) printf("%39s", "failed");
}
if(!quiet) printf("\n\n");
if(failcount > 4){
WARNX(_("Can't get status answer!"));
return 1;
}
return 0;
}
#if 0
MOTOR0=SLEEP
POS0=43
ESW00=RLSD
ESW01=HALL
MOTOR1=MOVE
STEPSLEFT1=6921
POS1=3238
ESW10=HALL
ESW11=RLSD
#endif
// tty_getstatus when `quiet==1`
static void ttystatq(){
char st[4] = "xGS";
char *nm[3] = {NULL, "POL", "L4"};
for(int Nmcu = 1; Nmcu < 3; ++Nmcu){
if(!alive[Nmcu]) continue;
st[0] = '0' + Nmcu;
if(!tty_sendraw(st)) continue;
char *str = strtok(buf, "\n");
while(str){
printf("%s%s\n", nm[Nmcu], str);
str = strtok(NULL, "\n");
}
}
}
// tty_getstatus when `quiet==0`
static void ttystat(){
int chk1 = -1, chk2 = -1;
char buff[TBUFLEN+1];
if(!quiet) green("Pol: M0ST M0LEFT M0POS - M1ST M1LEFT M1POS || L/4: M0ST M0LEFT M0POS - M1ST M1LEFT M1POS \n");
if(alive[1]) chk1 = parsestatus("1GS");
MSG(NULL, "Pol: ");
if(chk1 == -1){
if(!quiet) printf("%39s", "failed");
}else printf("%s", bufo);
char *val = keyval("ESW00");
if(!val) val = " ";
sprintf(buff, "%5s ", val);
val = keyval("ESW01");
if(!val) val = " ";
sprintf(&buff[6], "%5s ", val);
val = keyval("ESW10");
if(!val) val = " ";
sprintf(&buff[12], "%5s ", val);
val = keyval("ESW11");
if(!val) val = " ";
sprintf(&buff[18], "%5s ", val);
if(alive[2]) chk2 = parsestatus("2GS");
printf(" || L/4: ");
if(chk2 == -1){
printf("%39s", "failed");
}else printf("%s", bufo);
printf("\n");
sprintf(&buff[24], "|| ");
val = keyval("ESW00");
DBG("ESW00=%s", val);
if(!val) val = " ";
sprintf(&buff[27], "%5s ", val);
val = keyval("ESW01");
if(!val) val = " ";
sprintf(&buff[33], "%5s ", val);
val = keyval("ESW10");
if(!val) val = " ";
sprintf(&buff[39], "%5s ", val);
val = keyval("ESW11");
if(!val) val = " ";
sprintf(&buff[45], "%5s ", val);
// end-switches
green("ESW00 ESW01 ESW10 ESW11 || ESW00 ESW01 ESW10 ESW11\n");
printf("%s\n", buff);
for(int i = 1; i < 3; ++i){
if(reset[i]) printf("RESET%d=1\n", i);
}
}
void tty_getstatus(){
FNAME();
if(quiet) ttystatq();
else ttystat();
}
/**
* Show temperature of both MCU
* @return amount of successful calls
*/
int tty_showtemp(){
char *val, buff[] = "xGT\n";
const char *nm[3] = {NULL, "POL", "L4"};
int ret = 0;
for(int i = 1; i < 3; ++i){
if(!alive[i]){
DBG("MCU %d didn't respond!", i);
continue;
}
buff[0] = '0' + (char)i;
if(tty_sendraw(buff)){
val = keyval("TEMP");
if(val){
++ret;
double t;
if(str2double(&t, val)){
if(quiet){
printf("%sTEMP=%g\n", nm[i], t/10.);
}else{
green("MCU%d temperature:", i);
printf(" %g degC\n", t/10.);
}
}
}
}
}
return ret;
}
/**
* get current motor position (if move or error return INT_MIN)
*
*/
int mot_getpos(int mcu, int motor){
if(mcu < 1 || mcu > 2){
WARNX(_("Bad MCU number"));
return INT_MIN;
}
if(motor < 0 || motor > 1){
WARNX(_("Bad motor number"));
return INT_MIN;
}
return motpos[mcu][motor];
}
/**
* get motor endswitches status
* @return -1 if error or intermediate position, 0 - on zero's esw, 1 - on end's esw
*/
int mot_getesw(int mcu, int motor){
if(mcu < 1 || mcu > 2){
WARNX(_("Bad MCU number"));
return -1;
}
if(motor < 0 || motor > 1){
WARNX(_("Bad motor number"));
return -1;
}
return endswitches[mcu][motor];
}
/**
* test connection (1,2 -> ALIVE)
* and get positions
* @return 1 if none of MCU found, 0 if at least 1 found
*/
int handshake(){
char buff[32], *val;
int mcu, motor;
FNAME();
for(mcu = 1; mcu < 3; ++ mcu){
// check if MCU alive
sprintf(buff, "%d", mcu);
int notresp = 1;
// make HANDSHAKE_TRIES tries
for(int tr = 0; tr < HANDSHAKE_TRIES; ++tr){
if(tty_sendraw(buff) && 0 == strcmp(buf, "ALIVE\n")){
notresp = 0;
break;
}
}
if(notresp){
WARNX(_("MCU %d not response!"), mcu);
continue;
}
alive[mcu] = 1;
sprintf(buff, "%dGS", mcu);
if(tty_sendraw(buff)){
// check reboot states
if(strstr(buf, "RESET")){
reset[mcu] = 1;
WARNX(_("MCU %d had reset state!"), mcu);
}else
reset[mcu] = 0;
for(motor = 0; motor < 2; ++motor){
/*sprintf(buff, "MOTOR%d", motor);
val = keyval(buff);
if(!val || strcmp(val, "SLEEP")){
motpos[mcu][motor] = INT_MIN;
continue;
}*/
sprintf(buff, "POS%d", motor);
val = keyval(buff);
DBG("----%s=%s", buff, val);
motpos[mcu][motor] = val ? atoi(val) : INT_MIN;
DBG("MOTPOS[%d][%d] = %d", mcu, motor, motpos[mcu][motor]);
// end-switches
sprintf(buff, "ESW%d0", motor);
val = keyval(buff);
if(!val || strcmp(val, "HALL")){
sprintf(buff, "ESW%d1", motor);
val = keyval(buff);
if(!val || strcmp(val, "HALL")){
endswitches[mcu][motor] = -1;
}else
endswitches[mcu][motor] = 1;
}else endswitches[mcu][motor] = 0;
DBG("ENDSWITCHES[%d][%d] = %d", mcu, motor, endswitches[mcu][motor]);
}
}else{
motpos[mcu][0] = INT_MIN;
motpos[mcu][1] = INT_MIN;
}
}
if(alive[1] == 0 && alive[2] == 0) return 1;
return 0;
}
/**
* @brief tty_stopall - send commands to stop all motors
* @return 0 if all OK, else return amount of motors failed to stop
*/
int tty_stopall(){
int ret = 4;
if(alive[1]){
if(SEND_ALLOK == tty_sendcmd("1M0S")) --ret;
if(SEND_ALLOK == tty_sendcmd("1M1S")) --ret;
}
if(alive[2]){
if(SEND_ALLOK == tty_sendcmd("2M0S")) --ret;
if(SEND_ALLOK == tty_sendcmd("2M1S")) --ret;
}
return ret;
}
/**
* @brief init_motors - init all motors simultaneously (if they need to)
* @return 0 if all OK, or Nmcu*10+motnum for problem motor
*/
int init_motors(){
#define RETVAL() (Nmcu*10+motnum)
int Nmcu, motnum, needinit = 0;
for(Nmcu = 1; Nmcu < 3; ++Nmcu){
for(motnum = 0; motnum < 2; ++motnum){
// check position
int pos = motpos[Nmcu][motnum];
if(pos == INT_MIN) continue; // communication error on handshake
if(pos < 0) needinit = 1;
}}
if(!needinit) return 0;
DBG("Need to init, start!");
for(Nmcu = 1; Nmcu < 3; ++Nmcu){
for(motnum = 0; motnum < 2; ++motnum){
int pos = motpos[Nmcu][motnum];
if(pos >= 0) continue;
// check if we are on zero endswitch
int esw = mot_getesw(Nmcu, motnum);
if(esw == 0){ // move a little from zero esw
sprintf(buf, "%dM%dM100", Nmcu, motnum);
if(SEND_ERR == tty_sendcmd(buf)){
WARNX(_("Can't move from endswitch"));
return RETVAL();
}
tty_wait();
}
sprintf(buf, "%dM%dM-40000", Nmcu, motnum);
if(SEND_ALLOK != tty_sendcmd(buf)){
WARNX(_("Can't move to endwsitch 0"));
return RETVAL();
}
}}
tty_wait();
handshake();
for(Nmcu = 1; Nmcu < 3; ++Nmcu){
for(motnum = 0; motnum < 2; ++motnum){
int curpos = mot_getpos(Nmcu, motnum);
if(curpos){
WARNX(_("Can't return to zero"));
return RETVAL();
}
}}
return 0;
#undef RETVAL
}
Reference in New Issue View Git Blame Copy Permalink