diff --git a/canserver/socket.c b/canserver/socket.c
index 9973b63..252b8b6 100644
--- a/canserver/socket.c
+++ b/canserver/socket.c
@@ -53,12 +53,12 @@ message ServerMessages = {0};
*/
static size_t send_data(int sock, const char *textbuf){
ssize_t Len = strlen(textbuf);
- if(Len != write(sock, textbuf, Len)){
+ if(Len != send(sock, textbuf, Len, MSG_NOSIGNAL)){
WARN("write()");
LOGERR("send_data(): write() failed");
return 0;
}else LOGDBG("send_data(): sent '%s'", textbuf);
- if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1);
+ if(textbuf[Len-1] != '\n') Len += send(sock, "\n", 1, MSG_NOSIGNAL);
return (size_t)Len;
}
diff --git a/commandline/CMakeLists.txt b/commandline/CMakeLists.txt
index c356a80..5cd0e2d 100644
--- a/commandline/CMakeLists.txt
+++ b/commandline/CMakeLists.txt
@@ -20,7 +20,7 @@ set(CMAKE_COLOR_MAKEFILE ON)
# here is one of two variants: all .c in directory or .c files in list
aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES)
-# cmake -DDEBUG=1 -> debugging
+# cmake -DEBUG=1 -> debugging
if(DEFINED EBUG)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Wall -Werror -W")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -Wall -Werror -W")
diff --git a/commandline/OfflinePrgmng.cfg b/commandline/OfflinePrgmng.cfg
new file mode 100644
index 0000000..1e20ae2
--- /dev/null
+++ b/commandline/OfflinePrgmng.cfg
@@ -0,0 +1,6 @@
+# Turn off offline programming
+0x6018, 1, 0
+0x6018, 2, 0
+# Save parameters
+0x2007, 0, 2
+
diff --git a/commandline/Readme.md b/commandline/Readme.md
index c34cabf..42b0a5e 100644
--- a/commandline/Readme.md
+++ b/commandline/Readme.md
@@ -8,29 +8,33 @@ Usage: steppermove [args]
Where args are:
- -0, --zeropos set current position to zero
- -D, --disable disable motor
- -E, --enablesw enable end-switches 1 and 2
- -I, --nodeid=arg node ID (1..127)
- -P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid)
- -R, --readvals read values of used parameters
- -S, --stop stop motor
- -a, --abs=arg move to absolute position (steps)
- -c, --clearerr clear errors
- -d, --device=arg serial device name (default: /dev/ttyUSB0)
- -h, --help show this help
- -k, --check=arg check SDO data file
- -l, --logfile=arg file to save logs
- -m, --maxspd=arg maximal motor speed (steps per second)
- -p, --parse=arg file with SDO data to send to device
- -q, --quick directly send command without getting status
- -r, --rel=arg move to relative position (steps)
- -s, --canspd=arg CAN bus speed
- -t, --serialspd=arg serial (tty) device speed (default: 115200)
- -u, --microsteps=arg microstepping (0..256)
- -w, --wait wait while motor is busy
+ -0, --zeropos set current position to zero
+ -A, --disablesw disable end-switches
+ -D, --disable disable motor
+ -E, --enablesw=arg enable end-switches with given mask
+ -I, --nodeid=arg node ID (1..127)
+ -P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid)
+ -R, --readvals read values of used parameters
+ -S, --stop stop motor
+ -a, --abs=arg move to absolute position (in encoder ticks)
+ -c, --clearerr clear errors
+ -d, --device=arg serial device name (default: /dev/ttyUSB0)
+ -h, --help show this help
+ -k, --check check SDO data file
+ -l, --logfile=arg file to save logs
+ -m, --maxspd=arg maximal motor speed (enc ticks per second)
+ -p, --parse file[s] with SDO data to send to device
+ -q, --quick directly send command without getting status
+ -r, --rel=arg move to relative position (in encoder ticks)
+ -s, --canspd=arg CAN bus speed
+ -t, --serialspd=arg serial (tty) device speed (default: 115200)
+ -u, --microsteps=arg set microstepping (0..256)
+ -v, --verbose verbosity level for logging (each -v increases level)
+ -w, --wait wait while motor is busy
## Some usefull information
Factory settings of pusirobot drivers: 125kBaud, nodeID=5
+
+Speed 6553.6 == 1rev/s
diff --git a/commandline/canopen.c b/commandline/canopen.c
index 17800d9..18bd5da 100644
--- a/commandline/canopen.c
+++ b/commandline/canopen.c
@@ -144,7 +144,7 @@ SDO *parseSDO(CANmesg *mesg){
return &sdo;
}
-// send request to read SDO
+// send request to read SDO, return 0 if all OK
static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
SDO sdo;
sdo.NID = NID;
@@ -153,7 +153,10 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
sdo.index = idx;
sdo.subindex = subidx;
CANmesg *mesg = mkMesg(&sdo);
- return canbus_write(mesg);
+ int ans = 1; // error
+ for(int i = 0; i < NTRIES; ++i)
+ if(!(ans = canbus_write(mesg))) return 0;
+ return ans;
}
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){
@@ -277,7 +280,10 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
sdo.subindex = e->subindex;
CANmesg *mesgp = mkMesg(&sdo);
DBG("Canbus write..");
- if(canbus_write(mesgp)){
+ int ans = 1;
+ for(int i = 0; i < NTRIES; ++i)
+ if(!(ans = canbus_write(mesgp))) break;
+ if(ans){
WARNX("SDO_write(): Can't initiate download");
return 2;
}
diff --git a/commandline/canopen.h b/commandline/canopen.h
index 2c01c78..1e26a49 100644
--- a/commandline/canopen.h
+++ b/commandline/canopen.h
@@ -24,7 +24,9 @@
#include "pusirobot.h"
// timeout for answer from the SDO, seconds
-#define SDO_ANS_TIMEOUT (1.)
+#define SDO_ANS_TIMEOUT (2.5)
+// N tries to write CAN message
+#define NTRIES (15)
// COB-ID base:
#define NMT_COBID 0
diff --git a/commandline/cfg/Acceleration.cfg b/commandline/cfg/Acceleration.cfg
new file mode 100644
index 0000000..b266f1c
--- /dev/null
+++ b/commandline/cfg/Acceleration.cfg
@@ -0,0 +1,10 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set acceleration (0 - no, 1 - highest, 8 - lowest)
+0x6008, 0, 3
+# Set deceleration
+0x6009, 0, 3
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cfg/NoPDOmapping.cfg b/commandline/cfg/NoPDOmapping.cfg
new file mode 100644
index 0000000..04ef790
--- /dev/null
+++ b/commandline/cfg/NoPDOmapping.cfg
@@ -0,0 +1,2 @@
+# set number of mapped objects to 0
+0x1A00, 0, 0
diff --git a/commandline/cfg/SetCurnt0.6.cfg b/commandline/cfg/SetCurnt0.6.cfg
new file mode 100644
index 0000000..56f4b9b
--- /dev/null
+++ b/commandline/cfg/SetCurnt0.6.cfg
@@ -0,0 +1,8 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set max phase current to 600mA
+0x600B, 0, 600
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cfg/Start_settings.cfg b/commandline/cfg/Start_settings.cfg
index 8e9d77d..1b21d96 100644
--- a/commandline/cfg/Start_settings.cfg
+++ b/commandline/cfg/Start_settings.cfg
@@ -4,16 +4,23 @@
# Set heartbeat time to 0
0x1017, 0, 0
-# Set Node ID to 1
-0x2002, 0, 2
+# Turn off offline programming
+0x6018, 1, 0
+0x6018, 2, 0
+
+# Set Node ID to 10
+0x2002, 0, 10
# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000)
0x2003, 0, 5
+# Set max phase current to 1.5A
+0x600B, 0, 1500
+
# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
# Falling edge for trigger #1
-0x600F, 2, 0
+#0x600F, 2, 0
# Enable EXT1, EXT2
-0x600F, 1, 1
+#0x600F, 1, 1
# Save parameters
0x2007, 0, 2
diff --git a/commandline/cfg/Start_settingsEXT1.cfg b/commandline/cfg/Start_settingsEXT1.cfg
new file mode 100644
index 0000000..d2b6bba
--- /dev/null
+++ b/commandline/cfg/Start_settingsEXT1.cfg
@@ -0,0 +1,27 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 0
+0x1017, 0, 0
+
+# Set Node ID to 1
+0x2002, 0, 1
+# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000)
+0x2003, 0, 5
+# Set microstepping to 32
+0x600A, 0, 32
+# Set max phase current to 1.0A
+0x600B, 0, 1000
+# Set acceleration (0 - no, 1 - highest, 8 - lowest)
+0x6008, 0, 6
+# Set deceleration
+0x6009, 0, 7
+
+# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
+# Rising edge trigger for #2, falling for #1 (inverted!)
+0x600F, 2, 1
+# Enable EXT1
+0x600F, 1, 1
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c
index fde34e3..078bcd8 100644
--- a/commandline/cmdlnopts.c
+++ b/commandline/cmdlnopts.c
@@ -49,7 +49,7 @@ static glob_pars const Gdefault = {
.absmove = INT_MIN,
.relmove = INT_MIN,
.microsteps = -1,
- .maxspeed = INT_MIN
+ .maxspeed = INT_MIN,
};
/*
@@ -65,22 +65,22 @@ static myoption cmdlnopts[] = {
{"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)")},
- {"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 (full steps)")},
- {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (full steps)")},
- {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (full steps per second)")},
+ {"microsteps", NEED_ARG,NULL, 'u', arg_int, APTR(&G.microsteps),_("set microstepping (0..256)")},
+ {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (in encoder ticks)")},
+ {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (in encoder ticks)")},
+ {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (enc ticks per second)")},
{"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")},
+ {"parse", MULT_PAR, NULL, 'p', arg_string, APTR(&G.parsefile), _("file[s] with SDO data to send to device")},
+ {"check", MULT_PAR, 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",NEED_ARG, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches with given mask")},
{"disablesw",NO_ARGS, NULL, 'A', arg_int, APTR(&G.disableESW),_("disable end-switches")},
{"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), _("wait while motor is busy")},
{"quick", NO_ARGS, NULL, 'q', arg_int, APTR(&G.quick), _("directly send command without getting status")},
- {"fracsteps",NO_ARGS, NULL, 'F', arg_int, APTR(&G.fracsteps), _("move to steps fractions amount instead of full steps")},
+ {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verblevel), _("verbosity level for logging (each -v increases level)")},
end_option
};
diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h
index be51f27..5acab31 100644
--- a/commandline/cmdlnopts.h
+++ b/commandline/cmdlnopts.h
@@ -35,13 +35,14 @@ typedef struct{
char *device; // serial device name
char *pidfile; // name of PID file
char *logfile; // logging to this file
- char *parsefile; // file to parse
- char *checkfile; // SDO data filename to check
+ char **parsefile; // file[s] to parse
+ char **checkfile; // SDO data filename[s] to check
+ int verblevel; // verbose level for logfile
int canspeed; // CAN bus speed
int serialspeed; // serial device speed (CAN-bus to USB)
int NodeID; // node ID to work with
- int absmove; // absolute position to move
- int relmove; // relative position to move
+ int absmove; // absolute position to move (in encoder's ticks)
+ int relmove; // relative position to move (in encoder's ticks)
int microsteps; // set microstepping
int maxspeed; // max speed
int stop; // stop motor
@@ -53,7 +54,6 @@ typedef struct{
int disableESW; // --//-- disable
int wait; // wait while device is busy
int quick; // directly send command without getting status
- int fracsteps; // move in steps fractions instead of full steps
} glob_pars;
diff --git a/commandline/dataparser.c b/commandline/dataparser.c
index 9610771..b13358e 100644
--- a/commandline/dataparser.c
+++ b/commandline/dataparser.c
@@ -16,15 +16,16 @@
* along with this program. If not, see .
*/
-#include "canopen.h"
-#include "dataparser.h"
-#include "pusirobot.h"
-
#include // basename
#include // fopen
#include // strchr
#include
+#include "canopen.h"
+#include "dataparser.h"
+#include "pusirobot.h"
+#include "verblog.h"
+
#define BUFSZ 256
char *getl(char *str, long long *lp){
@@ -101,7 +102,7 @@ int parse_data_file(const char *fname, uint8_t nid){
}
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);
+ if(nid == 0) message(1, "line #%d: read SDO with index=0x%04X, subindex=0x%02X, data=0x%lX (dec: %ld)", 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);
@@ -127,7 +128,7 @@ int parse_data_file(const char *fname, uint8_t nid){
}
if(nid){
if(SDO_write(entry, nid, data)) WARNX("Can't write SDO idx=0x%04X", entry->index);
- else printf("Send to NID=%d SDO [0x%04X, 0x%02X] with data 0x%lX\n", nid, entry->index, entry->subindex, data);
+ else message(1, "Send to NID=%d SDO [0x%04X, 0x%02X] with data 0x%lX", nid, entry->index, entry->subindex, data);
}
}while(0);
if(!isgood){
diff --git a/commandline/dicentries.in b/commandline/dicentries.in
index 50f57e7..0e5fce1 100644
--- a/commandline/dicentries.in
+++ b/commandline/dicentries.in
@@ -98,7 +98,7 @@ DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping")
// max current
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current")
// current position
-DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position")
+DICENTRY(POSITION, 0x600C, 0, 4, 1, "current position")
// current reduction
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction")
// motor enable
diff --git a/commandline/main.c b/commandline/main.c
index c599bbe..24198eb 100644
--- a/commandline/main.c
+++ b/commandline/main.c
@@ -28,15 +28,15 @@
#include "cmdlnopts.h"
#include "dataparser.h"
#include "pusirobot.h"
+#include "verblog.h"
static glob_pars *GP = NULL; // for GP->pidfile need in `signals`
static uint8_t ID = 0;
-static uint16_t microstepping = 0;
static uint8_t devstat = 0; // device status after chkstat()
// default signal handler
void signals(int sig){
- putlog("Exit with status %d", sig);
+ LOGERR("Exit with status %d", sig);
DBG("Exit with status %d", sig);
if(GP->pidfile) // remove unnesessary PID file
unlink(GP->pidfile);
@@ -66,7 +66,7 @@ static inline void chkerr(int64_t es){
// device status check
static inline void chkstat(int64_t es){
if(es) red("DEVSTATUS=%d\n", (int)es);
- else green("DEVSTATUS=0\n");
+ else message(1, "DEVSTATUS=0");
devstat = (uint8_t)es;
if(devstat){
for(uint8_t i = 0; i < 8; ++i){
@@ -80,6 +80,7 @@ static inline void chkstat(int64_t es){
}
}
+/*
// setup microstepping
static inline void setusteps(int64_t es){
DBG("es=%zd", es);
@@ -94,7 +95,7 @@ static inline void setusteps(int64_t es){
}
microstepping = es > 0 ? (uint16_t) es : 1;
green("MICROSTEPPING=%u\n", microstepping);
-}
+}*/
// setup maximal speed
static inline void setmaxspd(int64_t es){
@@ -102,14 +103,14 @@ static inline void setmaxspd(int64_t es){
if(es == 0 && (GP->absmove != INT_MIN || GP->relmove != INT_MIN) && (GP->maxspeed == INT_MIN || GP->maxspeed == 0))
ERRX("Can't move when MAXSPEED==0");
if(GP->maxspeed != INT_MIN){
- GP->maxspeed *= microstepping;
- if(GP->maxspeed < MAX_SPEED_MIN || GP->maxspeed > MAX_SPEED_MAX)
- ERRX("MAXSPEED should be from %d to %d", MAX_SPEED_MIN/microstepping, MAX_SPEED_MAX/microstepping);
+ GP->maxspeed *= SPEED_MULTIPLIER;
+ if(GP->maxspeed < MAX_SPEED_MIN|| GP->maxspeed > MAX_SPEED_MAX)
+ ERRX("MAXSPEED should be from %d to %d", MAX_SPEED_MIN / SPEED_MULTIPLIER, MAX_SPEED_MAX / SPEED_MULTIPLIER);
DBG("Try to change max speed");
if(SDO_write(&MAXSPEED, ID, GP->maxspeed) || INT64_MIN == (es = SDO_read(&MAXSPEED, ID)))
ERRX("Can't change max speed");
}
- if(es) green("MAXSPEED=%d\n", (int)es/microstepping);
+ if(es) message(1, "MAXSPEED=%d", (int)es);
else red("MAXSPEED=0\n");
}
@@ -172,17 +173,17 @@ int main(int argc, char *argv[]){
initial_setup();
char *self = strdup(argv[0]);
GP = parse_args(argc, argv);
+ if(GP->verblevel) maxmesglevl(GP->verblevel);
if(GP->checkfile){ // just check and exit
- return parse_data_file(GP->checkfile, 0);
+ char **c = GP->checkfile;
+ int r = 0;
+ while(*c){
+ message(1, "\nCheck data file %s", *c);
+ r += parse_data_file(*c, 0);
+ ++c;
+ }
+ return r;
}
- check4running(self, GP->pidfile);
- free(self);
- signal(SIGTERM, signals); // kill (-15) - quit
- signal(SIGHUP, SIG_IGN); // hup - ignore
- signal(SIGINT, signals); // ctrl+C - quit
- signal(SIGQUIT, signals); // ctrl+\ - quit
- signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-
if(GP->NodeID != 1){
if(GP->NodeID < 1 || GP->NodeID > 127) ERRX("Node ID should be a number from 1 to 127");
}
@@ -195,45 +196,55 @@ int main(int argc, char *argv[]){
if(GP->maxspeed == 0)
ERRX("Set non-zero MAXSPEED");
}
+ if(GP->enableESW && GP->disableESW) ERRX("Enable & disable ESW can't meet together");
- if(GP->logfile) openlogfile(GP->logfile);
- putlog(("Start application..."));
- putlog("Try to open CAN bus device %s", GP->device);
+ check4running(self, GP->pidfile);
+ free(self);
+ signal(SIGTERM, signals); // kill (-15) - quit
+ signal(SIGHUP, SIG_IGN); // hup - ignore
+ signal(SIGINT, signals); // ctrl+C - quit
+ signal(SIGQUIT, signals); // ctrl+\ - quit
+ signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
+
+ if(GP->logfile){
+ sl_loglevel l = LOGLVL + GP->verblevel;
+ if(l > LOGLEVEL_ANY) l = LOGLEVEL_ANY;
+ OPENLOG(GP->logfile, l, 1);
+ LOGMSG(("Start application..."));
+ LOGMSG("Try to open CAN bus device %s", GP->device);
+ }
setserialspeed(GP->serialspeed);
if(canbus_open(GP->device)){
- putlog("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed);
- ERRX("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed);
+ LogAndErr("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed);
}
if(canbus_setspeed(GP->canspeed)){
- putlog("Can't set CAN speed %d. Exit.", GP->canspeed);
- ERRX("Can't set CAN speed %d. Exit.", GP->canspeed);
+ LogAndErr("Can't set CAN speed %d. Exit.", GP->canspeed);
}
// print current position and state
int64_t i64;
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)
+#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else LogAndWarn(e); }while(0)
+#define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else LogAndErr(e); }while(0)
#define Mesg(...)
//#define Mesg(...) green(__VA_ARGS__)
//double d0 = dtime();
// get mircostepping (need this to properly calculate current position and move
- if(GP->fracsteps) microstepping = 1; // Don't calculate microstepping in this case
- else{
+ /*if(!GP->fracsteps){
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
Mesg("MICROSTEPS: %g\n", dtime() - d0);
- }
+ }*/
if(!GP->quick){
// check error status
getSDOe(ERRSTATE, chkerr, "Can't get error status");
Mesg("ERRSTATE: %g\n", dtime() - d0);
// get current position
if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))){
- int enc = (int)i64;
- green("CURENCODER=%d\n", enc);
- green("CURSTEPS=%.2f\n", enc / ((double)microstepping));
+ //int enc = (int)i64;
+ message(1, "CURENCODER=%d", i64);
+ //message(1, "CURSTEPS=%.2f", i64 / ((double)microstepping));
}
else WARNX("Can't read current position");
Mesg("CURPOS: %g\n", dtime() - d0);
@@ -242,14 +253,20 @@ int main(int argc, char *argv[]){
Mesg("GPIOVAL: %g\n", dtime() - d0);
// get motor power status
if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
- if(i64) green("ENABLE=1\n");
- else red("ENABLE=0\n");
+ if(i64) message(1, "ENABLE=1");
+ else message(1, "ENABLE=0");
Mesg("Status: %g\n", dtime() - d0);
}
// get max speed
getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
Mesg("MAXSPEED: %g\n", dtime() - d0);
+ i64 = SDO_read(&MICROSTEPS, ID);
+ if(i64 == INT64_MIN) LogAndWarn("Can't get microstepping value");
+ else message(2, "MICROSTEPS=%u", i64);
}
+ i64 = SDO_read(&ENCRESOL, ID);
+ if(i64 == INT64_MIN) LogAndWarn("Can't get encoder resolution value");
+ else message(2, "ENCRESOL=%u", 1 << i64);
if(GP->absmove != INT_MIN || GP->relmove != INT_MIN || !GP->quick || GP->wait){
// check device status
getSDOe(DEVSTATUS, chkstat, "Can't get device status");
@@ -279,9 +296,13 @@ int main(int argc, char *argv[]){
}
// send values from external configuration file
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);
+ char **p = GP->parsefile;
+ while(*p){
+ message(1, "Try to parse %s and send SDOs to device", *p);
+ parse_data_file(*p, GP->NodeID);
+ Mesg("parse_data_file: %g\n", dtime() - d0);
+ ++p;
+ }
}
// enable limit switches
if(GP->enableESW){
@@ -300,12 +321,11 @@ int main(int argc, char *argv[]){
}
//int64_t es = SDO_read(&EXTENABLE, ID);
//green("LIMITSW=%lld\n", es);
- int multiplier = GP->fracsteps ? 1 : microstepping;
// move to absolute position
if(GP->absmove != INT_MIN){
if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state");
SDO_write(&ENABLE, ID, 1);
- if(SDO_write(&ABSSTEPS, ID, GP->absmove * multiplier))
+ if(SDO_write(&ABSSTEPS, ID, GP->absmove))
ERRX("Can't move to absolute position %d", GP->absmove);
}
if(GP->relmove != INT_MIN && GP->relmove){
@@ -318,8 +338,7 @@ int main(int argc, char *argv[]){
}
if(SDO_write(&ROTDIR, ID, dir) || INT64_MIN == (i64 = SDO_read(&ROTDIR, ID)))
ERRX("Can't change rotation direction");
- DBG("i64=%ld, dir=%d", i64, dir);
- if(SDO_write(&RELSTEPS, ID, GP->relmove * multiplier))
+ if(SDO_write(&RELSTEPS, ID, GP->relmove))
ERRX("Can't move to relative position %d", GP->relmove);
Mesg("RelMove: %g\n", dtime() - d0);
}
diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h
index af22e22..6ad6589 100644
--- a/commandline/pusirobot.h
+++ b/commandline/pusirobot.h
@@ -39,8 +39,11 @@ typedef struct{
extern const int DEsz;
extern const SDO_dic_entry* allrecords[];
+// speed limits
#define MAX_SPEED_MIN -200000
#define MAX_SPEED_MAX 200000
+// encoder ticks per rev * SPEED_MULTIPLIER = speed for 1 rev per second
+#define SPEED_MULTIPLIER (1.6)
// limit switches mask in GPIO status register (x=1,2,3)
#define EXTMASK(x) (1<<(6+x))
diff --git a/commandline/verblog.c b/commandline/verblog.c
new file mode 100644
index 0000000..b3de45c
--- /dev/null
+++ b/commandline/verblog.c
@@ -0,0 +1,38 @@
+/*
+ * This file is part of the stepper project.
+ * Copyright 2022 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 "verblog.h"
+#include
+#include
+#include
+
+// show messages only with level <= maxlevel
+static int maxlevel = MAXVERBLVL;
+
+void message(int level, const char *fmt, ...){
+ if(level > maxlevel) return;
+ va_list ar;
+ va_start(ar, fmt);
+ vprintf(fmt, ar);
+ va_end(ar);
+ printf("\n");
+}
+
+void maxmesglevl(int level){
+ maxlevel = level;
+}
diff --git a/commandline/verblog.h b/commandline/verblog.h
new file mode 100644
index 0000000..8f825bc
--- /dev/null
+++ b/commandline/verblog.h
@@ -0,0 +1,32 @@
+/*
+ * This file is part of the stepper project.
+ * Copyright 2022 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
+
+#ifdef EBUG
+#define MAXVERBLVL 1
+#define LOGLVL LOGLEVEL_DBG
+#else
+#define MAXVERBLVL 0
+#define LOGLVL LOGLEVEL_WARN
+#endif
+
+#define LogAndWarn(...) do{LOGWARN(__VA_ARGS__); WARNX(__VA_ARGS__);}while(0)
+#define LogAndErr(...) do{LOGERR(__VA_ARGS__); ERRX(__VA_ARGS__);}while(0)
+void message(int level, const char *fmt, ...);
+void maxmesglevl(int level);