diff --git a/commandline/Start_settings.cfg b/commandline/Start_settings.cfg new file mode 100644 index 0000000..0a623c2 --- /dev/null +++ b/commandline/Start_settings.cfg @@ -0,0 +1,24 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set Node ID to 1 +0x2002, 0, 1 +# Set microstepping to 32 +0x600A, 0, 32 +# Set max phase current to 600mA +0x600B, 0, 600 +# Set acceleration (0 - no, 1 - highest, 8 - lowest) +0x6008, 0, 4 +# Set deceleration +0x6009, 0, 5 + +# EXT1 emergency stop +# Rising edge trigger +0x600F, 2, 1 +# Turn on pullup +0x600F, 3, 1 +# Enable EXT1 +0x600F, 1, 1 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/canbus.c b/commandline/canbus.c index fe3f5df..6924125 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -54,7 +54,7 @@ static char *read_string(); * @return amount of bytes read */ static int read_ttyX(TTY_descr *d){ - if(d->comfd < 0) return 0; + if(!d || d->comfd < 0) return -1; size_t L = 0; ssize_t l; size_t length = d->bufsz; diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index 32c0179..e033b0f 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -64,7 +64,7 @@ static myoption cmdlnopts[] = { {"serialspd",NEED_ARG, NULL, 't', arg_int, APTR(&G.serialspeed),_("serial (tty) device speed (default: " STR(DEFAULT_SPEED) ")")}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs")}, {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, - {"nodeid", NEED_ARG, NULL, 'i', arg_int, APTR(&G.NodeID), _("node ID (1..127)")}, + {"nodeid", NEED_ARG, NULL, 'I', arg_int, APTR(&G.NodeID), _("node ID (1..127)")}, {"microsteps", NEED_ARG,NULL, 'u', arg_int, APTR(&G.microsteps),_("microstepping (0..256)")}, {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (steps)")}, {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (steps)")}, @@ -72,6 +72,8 @@ static myoption cmdlnopts[] = { {"stop", NO_ARGS, NULL, 'S', arg_int, APTR(&G.stop), _("stop motor")}, {"clearerr",NO_ARGS, NULL, 'c', arg_int, APTR(&G.clearerr), _("clear errors")}, {"zeropos", NO_ARGS, NULL, '0', arg_int, APTR(&G.zeropos), _("set current position to zero")}, + {"parse", NEED_ARG, NULL, 'p', arg_string, APTR(&G.parsefile), _("file with SDO data to send to device")}, + {"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index 80d4d86..2ac586d 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -35,6 +35,7 @@ typedef struct{ char *device; // serial device name char *pidfile; // name of PID file char *logfile; // logging to this file + char *parsefile; // file to parse int canspeed; // CAN bus speed int serialspeed; // serial device speed (CAN-bus to USB) int NodeID; // node ID to work with @@ -45,6 +46,7 @@ typedef struct{ int stop; // stop motor int clearerr; // try to clear errors int zeropos; // set position to zero + char *checkfile; // SDO data filename to check } glob_pars; diff --git a/commandline/dataparser.c b/commandline/dataparser.c new file mode 100644 index 0000000..941aaa2 --- /dev/null +++ b/commandline/dataparser.c @@ -0,0 +1,138 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "canopen.h" +#include "dataparser.h" +#include "pusirobot.h" + +#include // basename +#include // fopen +#include // strchr +#include + +#define BUFSZ 256 + +char *getl(char *str, long long *lp){ + long long l; + char *eptr; + int base = 0; + if(str[0] == '0' && (str[1] == 'b' || str[1] == 'B')){ // binary number format + base = 2; + str += 2; + } + l = strtoll(str, &eptr, base); + if(str == eptr) return NULL; + if(lp) *lp = l; + return eptr; +} + +// get next value - after ',' and spaces +char *getnxt(char *str){ + char *comma = strchr(str, ','); + if(!comma || !*comma) return NULL; + for(; str < comma; ++str) if(*str > ' ') return NULL; + ++comma; + while(*comma && *comma <= ' ') ++comma; + return comma; +} + +/** + * @brief parse_data_file - read SDO & data from file and send them to node `nid` + * @param fname - file name + * @param nid - node ID or 0 to check file + * @return 0 if no errors found + */ +int parse_data_file(const char *fname, uint8_t nid){ + int ret = 0; + if(!fname){ + WARNX("No filename for parsing"); + return 1; + } + FILE *f = fopen(fname, "r"); + if(!f){ + WARN("Can't open %s for parsing", basename((char*)fname)); + return 2; + } + char str[BUFSZ]; + int lineno = 0; + while(fgets(str, BUFSZ, f)){ + char *num = strchr(str, '#'); + if(num) *num = 0; + long long l; + uint16_t idx; + uint8_t sidx; + int64_t data; + int isgood = 1; + do{ + char *ptr; + if(!(ptr = getl(str, &l))) break; + idx = (uint16_t) l; + if(!(ptr = getnxt(ptr))){ + isgood = 0; + break; + } + if(!(ptr = getl(ptr, &l))){ + isgood = 0; + break; + } + sidx = (uint8_t) l; + if(!(ptr = getnxt(ptr))){ + isgood = 0; + break; + } + if(!(ptr = getl(ptr, &l))){ + isgood = 0; + break; + } + data = (int64_t) l; + DBG("Got: idx=0x%04X, subidx=0x%02X, data=0x%lX", idx, sidx, data); + if(nid == 0) printf("line #%d: read SDO with index=0x%04X, subindex=0x%02X, data=0x%lX (dec: %ld)\n", lineno, idx, sidx, data, data); + SDO_dic_entry *entry = dictentry_search(idx, sidx); + if(!entry){ + WARNX("SDO 0x%04X/0x%02X isn't in dictionary", idx, sidx); + continue; + } + if(data<0 && !entry->issigned){ + WARNX("SDO 0x%04X/0x%02X is only positive", idx, sidx); + continue; + } + uint64_t u64 = (uint64_t) (l > 0) ? l : -l; + if(u64 > UINT32_MAX){ + WARNX("The data size of SDO 0x%04X/0x%02X out of possible range for uint32_t", idx, sidx); + continue; + } + uint32_t u = (uint32_t) (l > 0) ? l : -l; + if(entry->datasize != 4){ + uint32_t mask = 0xff; + if(entry->datasize == 2) mask = 0xffff; + if((u & mask) != u){ + WARNX("The data size of SDO 0x%04X/0x%02X is larger than possible (%d byte[s])", idx, sidx, entry->datasize); + continue; + } + } + if(nid) SDO_write(entry, nid, data); + }while(0); + if(!isgood){ + WARNX("Bad syntax in line #%d: %s\nFormat: index, subindex, data (all may be hex, dec, oct or bin)", lineno, str); + } + ++lineno; + } + fclose(f); + return ret; +} + diff --git a/commandline/dataparser.h b/commandline/dataparser.h new file mode 100644 index 0000000..7b07547 --- /dev/null +++ b/commandline/dataparser.h @@ -0,0 +1,27 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef DATAPARSER_H__ +#define DATAPARSER_H__ + +#include + +int parse_data_file(const char *fname, uint8_t nid); + +#endif // DATAPARSER_H__ diff --git a/commandline/dicentries.h b/commandline/dicentries.h new file mode 100644 index 0000000..45d569e --- /dev/null +++ b/commandline/dicentries.h @@ -0,0 +1,67 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +// this file can be included more than once! + +// heartbeat time +DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0) +// node ID +DICENTRY(NODEID, 0x2002, 0, 1, 0) +// baudrate +DICENTRY(BAUDRATE, 0x2003, 0, 1, 0) +// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings +DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0) +// error state +DICENTRY(ERRSTATE, 0x6000, 0, 1, 0) +// controller status +DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0) +// rotation direction +DICENTRY(ROTDIR, 0x6002, 0, 1, 0) +// maximal speed +DICENTRY(MAXSPEED, 0x6003, 0, 4, 1) +// relative displacement +DICENTRY(RELSTEPS, 0x6004, 0, 4, 0) +// operation mode +DICENTRY(OPMODE, 0x6005, 0, 1, 0) +// start speed +DICENTRY(STARTSPEED, 0x6006, 0, 2, 0) +// stop speed +DICENTRY(STOPSPEED, 0x6007, 0, 2, 0) +// acceleration coefficient +DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0) +// deceleration coefficient +DICENTRY(DECELCOEF, 0x6009, 0, 1, 0) +// microstepping +DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0) +// max current +DICENTRY(MAXCURNT, 0x600B, 0, 2, 0) +// current position +DICENTRY(POSITION, 0x600C, 0, 4, 0) +// motor enable +DICENTRY(ENABLE, 0x600E, 0, 1, 0) +// EXT emergency stop enable +DICENTRY(EXTENABLE, 0x600F, 1, 1, 0) +// EXT emergency stop trigger mode +DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0) +// EXT emergency sensor type +DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0) +// absolute displacement +DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1) +// stop motor +DICENTRY(STOP, 0x6020, 0, 1, 0) + diff --git a/commandline/main.c b/commandline/main.c index d8e054b..85bd484 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include "dataparser.h" #include #include #include @@ -48,6 +49,9 @@ int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); GP = parse_args(argc, argv); + if(GP->checkfile){ // just check and exit + return parse_data_file(GP->checkfile, 0); + } check4running(self, GP->pidfile); free(self); signal(SIGTERM, signals); // kill (-15) - quit @@ -80,6 +84,11 @@ int main(int argc, char *argv[]){ signals(2); } + if(GP->parsefile){ + green("Try to parse %s and send SDOs to device\n", GP->parsefile); + parse_data_file(GP->parsefile, GP->NodeID); + } + //setup_con(); // print current position and state int64_t i64; diff --git a/commandline/pusirobot.c b/commandline/pusirobot.c index 4910a93..b1e7e51 100644 --- a/commandline/pusirobot.c +++ b/commandline/pusirobot.c @@ -18,10 +18,21 @@ #include // for NULL -// we should init constants here! -#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s}; #include "pusirobot.h" +// we should init constants here! +#undef DICENTRY +#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s}; +#include "dicentries.h" + +// now init array with all dictionary +#undef DICENTRY +#define DICENTRY(name, idx, sidx, sz, s) {idx, sidx, sz, s}, +static const SDO_dic_entry allrecords[] = { +#include "dicentries.h" +}; +const int DEsz = sizeof(allrecords) / sizeof(SDO_dic_entry); + // controller status for bits static const char *DevStatus[] = { "External stop 1", @@ -58,12 +69,12 @@ const char *errname(uint8_t error, uint8_t bit){ return NULL; } -/* -// get current position for node ID `NID`, @return INT_MIN if error -int get_current_position(uint8_t NID){ - int64_t val = SDO_read(&POSITION, NID); - if(val == INT64_MIN) return INT_MIN; - return (int) val; +// search if the object exists in dictionary - for config file parser +SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex){ + // the search is linear as dictionary can be unsorted!!! + for(int i = 0; i < DEsz; ++i){ + const SDO_dic_entry *entry = &allrecords[i]; + if(entry->index == index && entry->subindex == subindex) return (SDO_dic_entry*)entry; + } + return NULL; } -*/ - diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h index f16c38f..70e93ed 100644 --- a/commandline/pusirobot.h +++ b/commandline/pusirobot.h @@ -33,46 +33,7 @@ typedef struct{ #define DICENTRY(name, idx, sidx, sz, s) extern const SDO_dic_entry name; #endif -// heartbeat time -DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0) -// node ID -DICENTRY(NODEID, 0x2002, 0, 1, 0) -// baudrate -DICENTRY(BAUDRATE, 0x2003, 0, 1, 0) -// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings -DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0) -// error state -DICENTRY(ERRSTATE, 0x6000, 0, 1, 0) -// controller status -DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0) -// rotation direction -DICENTRY(ROTDIR, 0x6002, 0, 1, 0) -// maximal speed -DICENTRY(MAXSPEED, 0x6003, 0, 4, 1) -// relative displacement -DICENTRY(RELSTEPS, 0x6004, 0, 4, 0) -// operation mode -DICENTRY(OPMODE, 0x6005, 0, 1, 0) -// start speed -DICENTRY(STARTSPEED, 0x6006, 0, 2, 0) -// stop speed -DICENTRY(STOPSPEED, 0x6007, 0, 2, 0) -// acceleration coefficient -DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0) -// deceleration coefficient -DICENTRY(DECELCOEF, 0x6009, 0, 1, 0) -// microstepping -DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0) -// max current -DICENTRY(MAXCURNT, 0x600B, 0, 2, 0) -// current position -DICENTRY(POSITION, 0x600C, 0, 4, 0) -// motor enable -DICENTRY(ENABLE, 0x600E, 0, 1, 0) -// absolute displacement -DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1) -// stop motor -DICENTRY(STOP, 0x6020, 0, 1, 0) +#include "dicentries.h" #define MAX_SPEED_MIN -200000 #define MAX_SPEED_MAX 200000 @@ -81,6 +42,7 @@ DICENTRY(STOP, 0x6020, 0, 1, 0) #define BUSY_STATE (1<<3) const char *devstatus(uint8_t status, uint8_t bit); const char *errname(uint8_t error, uint8_t bit); +SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex); //int get_current_position(uint8_t NID); #endif // PUSIROBOT_H__