eddy/pusirobot
1
0
Fork 0
mirror of https://github.com/eddyem/pusirobot.git synced 2025-12-06 10:35:11 +03:00
Code Issues Packages Projects Releases Wiki Activity

Fix bug with undesirable pauses

Browse Source
This commit is contained in:
Edward Emelianov 2020-09-22 17:38:06 +03:00
parent 1e5a01f0b3
commit 43de7a0259
6 changed files with 51 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
commandline/canbus.c
View File

@ -66,8 +66,7 @@ static int read_ttyX(TTY_descr *d){
l = 0;
FD_ZERO(&rfds);
FD_SET(d->comfd, &rfds);
// wait for 10ms
tv.tv_sec = 0; tv.tv_usec = 50000;
tv.tv_sec = 0; tv.tv_usec = 500;
retval = select(d->comfd + 1, &rfds, NULL, NULL, &tv);
if (!retval) break;
if(FD_ISSET(d->comfd, &rfds)){
@ -91,16 +90,18 @@ static int ttyWR(const char *buff, int len){
#ifdef EBUG
int _U_ n = write(STDERR_FILENO, buff, len);
fprintf(stderr, "\n");
double t0 = dtime();
#endif
int w = write_tty(dev->comfd, buff, (size_t)len);
if(!w) w = write_tty(dev->comfd, "\n", 1);
char *s = read_string(); // clear echo
DBG("Written, dt=%g", dtime() - t0);
char *s = read_string(); // clear echo & check
if(!s || strcmp(s, buff) != 0){
WARNX("wrong answer! Got '%s' instead of '%s'", s, buff);
return 1;
}
pthread_mutex_unlock(&mutex);
DBG("Success");
DBG("Success, dt=%g", dtime() - t0);
return w;
}
@ -113,8 +114,7 @@ void setserialspeed(int speed){
}
static void clearRXbuf(){
double t0 = dtime();
while(read_string() && dtime() - t0 < T_POLLING_TMOUT){usleep(1000);}
while(read_ttyX(dev));
}
int canbus_open(const char *devname){
@ -194,7 +194,10 @@ static char *read_string(){
}
memcpy(ptr, dev->buf, dev->buflen);
r += l; LL -= l; ptr += l;
if(ptr[-1] == '\n') break;
if(ptr[-1] == '\n'){
//DBG("Newline detected");
break;
}
d0 = dtime();
}
}while(dtime() - d0 < WAIT_TMOUT && LL);
@ -210,7 +213,7 @@ static char *read_string(){
optr = NULL;
return NULL;
}
DBG("buf: %s", buf);
DBG("buf: %s, time: %g", buf, dtime() - d0);
return buf;
}
return NULL;
@ -225,7 +228,7 @@ CANmesg *parseCANmesg(const char *str){
return &m;
}
#if 0
#ifdef EBUG
void showM(CANmesg *m){
printf("TS=%d, ID=0x%X", m->timemark, m->ID);
int l = m->len;
@ -246,8 +249,10 @@ int canbus_read(CANmesg *mesg){
while(dtime() - t0 < T_POLLING_TMOUT){ // read answer
if((ans = read_string())){ // parse new data
if((m = parseCANmesg(ans))){
//DBG("Got canbus message:");
//showM(m);
DBG("Got canbus message (dT=%g):", dtime() - t0);
#ifdef EBUG
showM(m);
#endif
if(ID && m->ID == ID){
memcpy(mesg, m, sizeof(CANmesg));
DBG("All OK");

6
commandline/canopen.c
View File

@ -259,8 +259,8 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
return ans;
}
// write SDO data
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){
// write SDO data, return 0 if all OK
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
if(!e || !data || e->datasize < 1 || e->datasize > 4){
WARNX("SDO_write(): bad datalen");
return 1;
@ -300,7 +300,7 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){
return 0;
}
int SDO_write(const SDO_dic_entry *e, _U_ uint8_t NID, int64_t data){
int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data){
if(!e) return 1;
uint8_t arr[4] = {0};
uint32_t U;

2
commandline/canopen.h
View File

@ -83,7 +83,7 @@ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID);
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID);
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data);
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data);
int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data);
//int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);

1
commandline/cmdlnopts.c
View File

@ -76,6 +76,7 @@ static myoption cmdlnopts[] = {
{"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")},
{"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")},
{"readvals",NO_ARGS, NULL, 'R', arg_int, APTR(&G.showpars), _("read values of used parameters")},
{"enablesw",NO_ARGS, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches 1 and 2")},
end_option
};

1
commandline/cmdlnopts.h
View File

@ -49,6 +49,7 @@ typedef struct{
int zeropos; // set position to zero
int disable; // disable motor
int showpars; // show values of some parameters
int enableESW; // send signal to enable end-switches
} glob_pars;

33
commandline/main.c
View File

@ -37,7 +37,6 @@ static uint16_t microstepping = 0;
void signals(int sig){
putlog("Exit with status %d", sig);
DBG("Exit with status %d", sig);
restore_console();
if(GP->pidfile) // remove unnesessary PID file
unlink(GP->pidfile);
canbus_close();
@ -183,46 +182,71 @@ int main(int argc, char *argv[]){
ID = GP->NodeID;
#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
#define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else WARNX(e);}while(0)
getSDOe(ERRSTATE, chkerr, "Can't get error status");
getSDOe(DEVSTATUS, chkstat, "Can't get device status");
#define Mesg(...)
//#define Mesg(...) green(__VA_ARGS__)
//double d0 = dtime();
getSDOe(ERRSTATE, chkerr, "Can't get error status");
Mesg("ERRSTATE: %g\n", dtime() - d0);
getSDOe(DEVSTATUS, chkstat, "Can't get device status");
Mesg("DEVSTATUS: %g\n", dtime() - d0);
if(GP->parsefile){
green("Try to parse %s and send SDOs to device\n", GP->parsefile);
parse_data_file(GP->parsefile, GP->NodeID);
Mesg("parse_data_file: %g\n", dtime() - d0);
}
if(GP->showpars){
showAllPars();
Mesg("showAllPars: %g\n", dtime() - d0);
}
if(GP->enableESW){
if(SDO_write(&EXTENABLE, ID, 3)){
WARNX("Error when trying to enable limit switches");
if(GP->absmove || GP->relmove) signals(-1);
}
Mesg("EXTENABLE: %g\n", dtime() - d0);
}
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
Mesg("MICROSTEPS: %g\n", dtime() - d0);
if(GP->zeropos){
if(SDO_write(&POSITION, ID, 0))
ERRX("Can't clear position counter");
Mesg("POSITION: %g\n", dtime() - d0);
}
if(INT64_MIN != (i64 = SDO_read(&POSITION, ID)))
green("CURPOS=%d\n", (int)i64/microstepping);
else ERRX("Can't read current position");
else WARNX("Can't read current position");
Mesg("CURPOS: %g\n", dtime() - d0);
getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
Mesg("MAXSPEED: %g\n", dtime() - d0);
getSDOw(GPIOVAL, gpioval, "Can't read GPIO values");
Mesg("GPIOVAL: %g\n", dtime() - d0);
if(GP->disable){
if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor");
Mesg("DISABLE: %g\n", dtime() - d0);
}
if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
if(i64) green("ENABLE=1\n");
else red("ENABLE=0\n");
Mesg("Status: %g\n", dtime() - d0);
}
if(GP->stop){
if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
Mesg("STOP: %g\n", dtime() - d0);
}
if(GP->absmove != INT_MIN){
SDO_write(&ENABLE, ID, 1);
if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping))
ERRX("Can't move to absolute position %d", GP->absmove);
Mesg("AbsMove: %g\n", dtime() - d0);
}
if(GP->relmove != INT_MIN && GP->relmove){
uint8_t dir = 1;
@ -236,6 +260,7 @@ int main(int argc, char *argv[]){
DBG("i64=%ld, dir=%d", i64, dir);
if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping))
ERRX("Can't move to relative position %d", GP->relmove);
Mesg("RelMove: %g\n", dtime() - d0);
}
#undef getSDOe