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

fixed some little problems in commandline

Browse Source
This commit is contained in:
Edward Emelianov 2022-08-30 16:50:17 +03:00
parent 6aec61d4a4
commit 1453398f08
19 changed files with 258 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
canserver/socket.c
View File

@ -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;
}

2
commandline/CMakeLists.txt
View File

@ -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")

6
commandline/OfflinePrgmng.cfg Normal file
View File

@ -0,0 +1,6 @@
# Turn off offline programming
0x6018, 1, 0
0x6018, 2, 0
# Save parameters
0x2007, 0, 2

46
commandline/Readme.md
View File

@ -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

12
commandline/canopen.c
View File

@ -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;
}

4
commandline/canopen.h
View File

@ -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

10
commandline/cfg/Acceleration.cfg Normal file
View File

@ -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

2
commandline/cfg/NoPDOmapping.cfg Normal file
View File

@ -0,0 +1,2 @@
# set number of mapped objects to 0
0x1A00, 0, 0

8
commandline/cfg/SetCurnt0.6.cfg Normal file
View File

@ -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

15
commandline/cfg/Start_settings.cfg
View File

@ -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

27
commandline/cfg/Start_settingsEXT1.cfg Normal file
View File

@ -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

16
commandline/cmdlnopts.c
View File

@ -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
};

10
commandline/cmdlnopts.h
View File

@ -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;

13
commandline/dataparser.c
View File

@ -16,15 +16,16 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "canopen.h"
#include "dataparser.h"
#include "pusirobot.h"
#include <libgen.h> // basename
#include <stdio.h> // fopen
#include <string.h> // strchr
#include <usefull_macros.h>
#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){

2
commandline/dicentries.in
View File

@ -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

101
commandline/main.c
View File

@ -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);
}

3
commandline/pusirobot.h
View File

@ -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))

38
commandline/verblog.c Normal file
View File

@ -0,0 +1,38 @@
/*
* This file is part of the stepper project.
* Copyright 2022 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/>.
*/
#include "verblog.h"
#include <stdarg.h>
#include <stdio.h>
#include <usefull_macros.h>
// 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;
}

32
commandline/verblog.h Normal file
View File

@ -0,0 +1,32 @@
/*
* This file is part of the stepper project.
* Copyright 2022 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/>.
*/
#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);