diff --git a/commandline/Start_settings.cfg b/commandline/Start_settings.cfg
index 5b0c2cd..da0ee65 100644
--- a/commandline/Start_settings.cfg
+++ b/commandline/Start_settings.cfg
@@ -1,6 +1,9 @@
# 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)
diff --git a/commandline/canbus.c b/commandline/canbus.c
index 694ee58..35dbab1 100644
--- a/commandline/canbus.c
+++ b/commandline/canbus.c
@@ -54,6 +54,7 @@ static char *read_string();
* @return amount of bytes read
*/
static int read_ttyX(TTY_descr *d){
+ FNAME();
if(!d || d->comfd < 0) return -1;
size_t L = 0;
ssize_t l;
@@ -85,7 +86,10 @@ static int read_ttyX(TTY_descr *d){
// thread-safe writing, add trailing '\n'
static int ttyWR(const char *buff, int len){
+ FNAME();
pthread_mutex_lock(&mutex);
+ //canbus_clear();
+ read_string(); // clear RX buffer
DBG("Write 2tty %d bytes: ", len);
#ifdef EBUG
int _U_ n = write(STDERR_FILENO, buff, len);
@@ -95,10 +99,16 @@ static int ttyWR(const char *buff, int len){
int w = write_tty(dev->comfd, buff, (size_t)len);
if(!w) w = write_tty(dev->comfd, "\n", 1);
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;
+ int errctr = 0;
+ while(1){
+ char *s = read_string(); // clear echo & check
+ if(!s || strncmp(s, buff, strlen(buff)) != 0){
+ if(++errctr > 3){
+ WARNX("wrong answer! Got '%s' instead of '%s'", s, buff);
+ w = 1;
+ break;
+ }
+ }else break;
}
pthread_mutex_unlock(&mutex);
DBG("Success, dt=%g", dtime() - t0);
@@ -113,7 +123,7 @@ void setserialspeed(int speed){
serialspeed = speed;
}
-static void clearRXbuf(){
+void canbus_clear(){
while(read_ttyX(dev));
}
@@ -143,11 +153,13 @@ int canbus_setspeed(int speed){
}
int len = snprintf(buff, BUFLEN, "b %d", speed);
if(len < 1) return 2;
- clearRXbuf();
- return ttyWR(buff, len);
+ int r = ttyWR(buff, len);
+ read_string(); // clear RX buf ('Reinit CAN bus with speed XXXXkbps')
+ return r;
}
int canbus_write(CANmesg *mesg){
+ FNAME();
char buf[BUFLEN];
if(!mesg || mesg->len > 8) return 1;
int rem = BUFLEN, len = 0;
@@ -158,7 +170,7 @@ int canbus_write(CANmesg *mesg){
rem -= l; len += l;
if(rem < 0) return 2;
}
- clearRXbuf();
+ canbus_clear();
return ttyWR(buf, len);
}
@@ -167,6 +179,7 @@ int canbus_write(CANmesg *mesg){
* @return NULL if nothing was read or pointer to static buffer
*/
static char *read_string(){
+ FNAME();
static char buf[1024];
int LL = 1023, r = 0, l;
char *ptr = NULL;
diff --git a/commandline/canbus.h b/commandline/canbus.h
index f390cdc..f2c2bc8 100644
--- a/commandline/canbus.h
+++ b/commandline/canbus.h
@@ -38,6 +38,7 @@ int canbus_open(const char *devname);
int canbus_write(CANmesg *mesg);
int canbus_read(CANmesg *mesg);
int canbus_setspeed(int speed);
+void canbus_clear();
// auxiliary (not necessary) functions
void setserialspeed(int speed);
diff --git a/commandline/canopen.c b/commandline/canopen.c
index 52a14dc..17800d9 100644
--- a/commandline/canopen.c
+++ b/commandline/canopen.c
@@ -157,6 +157,7 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
}
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){
+ FNAME();
CANmesg mesg;
SDO *sdo = NULL;
double t0 = dtime();
@@ -184,8 +185,9 @@ static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){
* @return SDO received or NULL if error
*/
SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID){
+ FNAME();
if(ask2read(idx, subidx, NID)){
- WARNX("Can't initiate upload");
+ WARNX("readSDOvalue(): Can't initiate upload");
return NULL;
}
return getSDOans(idx, subidx, NID);
@@ -217,16 +219,16 @@ static inline int8_t mki8(uint8_t data[4]){
// read SDO value, if error - return INT64_MIN
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
+ FNAME();
SDO *sdo = readSDOvalue(e->index, e->subindex, NID);
if(!sdo){
- WARNX("SDO read error");
return INT64_MIN;
}
if(sdo->ccs == CCS_ABORT_TRANSFER){ // error
WARNX("Got error for SDO 0x%X", e->index);
uint32_t ac = mku32(sdo->data);
const char *etxt = abortcode_text(ac);
- if(etxt) red("Abort code 0x%X: ", ac, etxt);
+ if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt);
return INT64_MIN;
}
if(sdo->datalen != e->datasize){
@@ -261,6 +263,7 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
// write SDO data, return 0 if all OK
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
+ FNAME();
if(!e || !data || e->datasize < 1 || e->datasize > 4){
WARNX("SDO_write(): bad datalen");
return 1;
@@ -273,10 +276,12 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
sdo.index = e->index;
sdo.subindex = e->subindex;
CANmesg *mesgp = mkMesg(&sdo);
+ DBG("Canbus write..");
if(canbus_write(mesgp)){
WARNX("SDO_write(): Can't initiate download");
return 2;
}
+ DBG("get answer");
SDO *sdop = getSDOans(e->index, e->subindex, NID);
if(!sdop){
WARNX("SDO_write(): SDO read error");
diff --git a/commandline/cfg/Default_settings.cfg b/commandline/cfg/Default_settings.cfg
new file mode 100644
index 0000000..e690e76
--- /dev/null
+++ b/commandline/cfg/Default_settings.cfg
@@ -0,0 +1,13 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 1000
+0x1017, 0, 1000
+
+# Set Node ID to 5
+0x2002, 0, 5
+# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000)
+0x2003, 0, 4
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cfg/Start_settings.cfg b/commandline/cfg/Start_settings.cfg
new file mode 100644
index 0000000..8e9d77d
--- /dev/null
+++ b/commandline/cfg/Start_settings.cfg
@@ -0,0 +1,19 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 0
+0x1017, 0, 0
+
+# Set Node ID to 1
+0x2002, 0, 2
+# 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
+
+# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
+# Falling edge for trigger #1
+0x600F, 2, 0
+# Enable EXT1, EXT2
+0x600F, 1, 1
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cfg/Start_settings_onlySpeed.cfg b/commandline/cfg/Start_settings_onlySpeed.cfg
new file mode 100644
index 0000000..5fc5d47
--- /dev/null
+++ b/commandline/cfg/Start_settings_onlySpeed.cfg
@@ -0,0 +1,11 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# Set heartbeat time to 0
+0x1017, 0, 0
+
+# 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
+
+# Save parameters
+0x2007, 0, 2
diff --git a/commandline/cfg/endswitches.cfg b/commandline/cfg/endswitches.cfg
new file mode 100644
index 0000000..0d88c6b
--- /dev/null
+++ b/commandline/cfg/endswitches.cfg
@@ -0,0 +1,13 @@
+# Transmit SDO to driver
+# Format: index, subindex, data
+
+# EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
+# Falling edge for trigger #1 (inverted!), rising for trigger #2
+0x600F, 2, 3
+# Pullup for trigger #2
+0x600F, 3, 2
+# Enable EXT1, EXT2
+0x600F, 1, 3
+
+# save settings
+#0x2007, 0, 2
diff --git a/commandline/cfg/endswitchesON.cfg b/commandline/cfg/endswitchesON.cfg
new file mode 100644
index 0000000..fff1c4f
--- /dev/null
+++ b/commandline/cfg/endswitchesON.cfg
@@ -0,0 +1,2 @@
+# Enable EXT1, EXT2
+0x600F, 1, 3
diff --git a/commandline/cfg/move b/commandline/cfg/move
new file mode 100755
index 0000000..f063d2a
--- /dev/null
+++ b/commandline/cfg/move
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+while true; do
+ ./steppermove -m100 -w -I4 -r 32
+ ./steppermove -m200 -w -I4 -r -64
+ sleep 1
+done
+
diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c
index 2648b45..77fc57d 100644
--- a/commandline/cmdlnopts.c
+++ b/commandline/cmdlnopts.c
@@ -77,6 +77,7 @@ static myoption cmdlnopts[] = {
{"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")},
+ {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), _("wait while motor is busy")},
end_option
};
diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h
index 9bbee87..2462688 100644
--- a/commandline/cmdlnopts.h
+++ b/commandline/cmdlnopts.h
@@ -50,6 +50,7 @@ typedef struct{
int disable; // disable motor
int showpars; // show values of some parameters
int enableESW; // send signal to enable end-switches
+ int wait; // wait while device is busy
} glob_pars;
diff --git a/commandline/dicentries.in b/commandline/dicentries.in
index a058e76..1413d93 100644
--- a/commandline/dicentries.in
+++ b/commandline/dicentries.in
@@ -22,6 +22,51 @@
// heartbeat time
DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time")
+
+// receive PDO parameter 0
+// largest subindex supported
+DICENTRY(RPDOP0LS, 0x1400, 0, 1, 0, "receive PDO parameter 0, largest subindex supported")
+// COB-ID used by PDO
+DICENTRY(RPDOP0CI, 0x1400, 1, 4, 0, "receive PDO parameter 0, COB-ID used by PDO")
+// transmission type
+DICENTRY(RPDOP0TT, 0x1400, 2, 1, 0, "receive PDO parameter 0, transmission type")
+// inhibit time
+DICENTRY(RPDOP0IT, 0x1400, 3, 2, 0, "receive PDO parameter 0, inhibit time")
+// compatibility entry
+DICENTRY(RPDOP0CE, 0x1400, 4, 1, 0, "receive PDO parameter 0, compatibility entry")
+// event timer
+DICENTRY(RPDOP0ET, 0x1400, 5, 2, 0, "receive PDO parameter 0, event timer")
+
+// receive PDO mapping 0
+// number of mapped application objects
+DICENTRY(RPDOM0N, 0x1600, 0, 1, 0, "receive PDO mapping 0, number of objects")
+// first map
+DICENTRY(RPDOM0O1, 0x1600, 1, 4, 0, "receive PDO mapping 0, mapping for 1st object")
+
+// transmit PDO parameter 0
+// largest subindex supported
+DICENTRY(TPDOP0LS, 0x1800, 0, 1, 0, "transmit PDO parameter 0, largest subindex supported")
+// COB-ID used by PDO
+DICENTRY(TPDOP0CI, 0x1800, 1, 4, 0, "transmit PDO parameter 0, COB-ID used by PDO")
+// transmission type
+DICENTRY(TPDOP0TT, 0x1800, 2, 1, 0, "transmit PDO parameter 0, transmission type")
+// inhibit time
+DICENTRY(TPDOP0IT, 0x1800, 3, 2, 0, "transmit PDO parameter 0, inhibit time")
+// reserved
+DICENTRY(TPDOP0R, 0x1800, 4, 1, 0, "transmit PDO parameter 0, reserved")
+// event timer
+DICENTRY(TPDOP0ET, 0x1800, 5, 2, 0, "transmit PDO parameter 0, event timer")
+
+// transmit PDO mapping 0
+// number of mapped application objects
+DICENTRY(TPDOM0N, 0x1A00, 0, 1, 0, "transmit PDO mapping 0, number of objects")
+// first map
+DICENTRY(TPDOM0O1, 0x1A00, 1, 4, 0, "transmit PDO mapping 0, mapping for 1st object")
+DICENTRY(TPDOM0O2, 0x1A00, 2, 4, 0, "transmit PDO mapping 0, mapping for 2nd object")
+DICENTRY(TPDOM0O3, 0x1A00, 3, 4, 0, "transmit PDO mapping 0, mapping for 3rd object")
+DICENTRY(TPDOM0O4, 0x1A00, 4, 4, 0, "transmit PDO mapping 0, mapping for 4th object")
+DICENTRY(TPDOM0O5, 0x1A00, 5, 4, 0, "transmit PDO mapping 0, mapping for 5th object")
+
// node ID
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID")
// baudrate
diff --git a/commandline/main.c b/commandline/main.c
index 0f2a386..803345a 100644
--- a/commandline/main.c
+++ b/commandline/main.c
@@ -16,7 +16,6 @@
* along with this program. If not, see .
*/
-#include "dataparser.h"
#include
#include
#include
@@ -27,11 +26,13 @@
#include "canbus.h"
#include "canopen.h"
#include "cmdlnopts.h"
+#include "dataparser.h"
#include "pusirobot.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){
@@ -66,15 +67,15 @@ static inline void chkerr(int64_t es){
static inline void chkstat(int64_t es){
if(es) red("DEVSTATUS=%d\n", (int)es);
else green("DEVSTATUS=0\n");
- uint8_t s = (uint8_t)es;
- if(s){
+ devstat = (uint8_t)es;
+ if(devstat){
for(uint8_t i = 0; i < 8; ++i){
- const char *msg = devstatus(s, i);
+ const char *msg = devstatus(devstat, i);
if(msg) red("\t%s\n", msg);
}
- if(s != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, s) || 0 != (es = SDO_read(&DEVSTATUS, ID)))){
+ if(devstat != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, devstat) || 0 != (es = SDO_read(&DEVSTATUS, ID)))){
ERRX("Can't clean device status");
- }
+ }else devstat = (uint8_t)es;
if(es && es != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy
}
}
@@ -122,7 +123,7 @@ static inline void gpioval(int64_t es){
// show values of all parameters from dicentries.in
static inline void showAllPars(){
- green("\nParameters' values:");
+ green("\nParameters' values:\n");
for(int i = 0; i < DEsz; ++i){
const SDO_dic_entry *entry = allrecords[i];
int64_t val = SDO_read(entry, GP->NodeID);
@@ -131,12 +132,37 @@ static inline void showAllPars(){
entry->index, entry->subindex, entry->name);
continue;
}
- printf("\n# %s\n0x%04X, %d, %ld", entry->name, entry->index,
+ printf("# %s\n0x%04X, %d, %ld\n", entry->name, entry->index,
entry->subindex, val);
+ fflush(stdout);
}
printf("\n\n");
}
+// wait while device is in busy state
+static inline void wait_busy(){
+ int errctr = 0;
+ int64_t ans;
+ int i = 0;
+ const char symbols[] = "|/-\\";
+ printf("Waiting... ");
+ do{
+ printf("%c\b", symbols[i]);
+ fflush(stdout);
+ if(++i > 3) i = 0;
+ if(INT64_MIN == (ans = SDO_read(&DEVSTATUS, ID))){
+ if(++errctr > 10) ERRX("Can't read device status");
+ }else errctr = 0;
+ if(ans == BUSY_STATE){
+ usleep(50000);
+ }else{
+ chkstat(ans);
+ break;
+ }
+ }while(1);
+ printf("\n");
+}
+
int main(int argc, char *argv[]){
initial_setup();
char *self = strdup(argv[0]);
@@ -151,6 +177,7 @@ int main(int argc, char *argv[]){
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");
}
@@ -175,6 +202,7 @@ int main(int argc, char *argv[]){
putlog("Can't set CAN speed %d. Exit.", GP->canspeed);
signals(2);
}
+ DBG("here");
//setup_con();
// print current position and state
@@ -185,45 +213,39 @@ int main(int argc, char *argv[]){
#define Mesg(...)
//#define Mesg(...) green(__VA_ARGS__)
- //double d0 = dtime();
+//double d0 = dtime();
getSDOe(ERRSTATE, chkerr, "Can't get error status");
Mesg("ERRSTATE: %g\n", dtime() - d0);
+
+ if(GP->stop){
+ if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
+ Mesg("STOP: %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->zeropos){
+ if(SDO_write(&POSITION, ID, 0))
+ ERRX("Can't clear position counter");
+ Mesg("POSITION: %g\n", dtime() - d0);
}
+ getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
+ Mesg("MICROSTEPS: %g\n", dtime() - d0);
+
+ if(INT64_MIN != (i64 = SDO_read(&POSITION, ID)))
+ green("CURPOS=%d\n", (int)i64/microstepping);
+ else WARNX("Can't read current position");
+ Mesg("CURPOS: %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 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");
+ getSDOe(GPIOVAL, gpioval, "Can't read GPIO values");
Mesg("GPIOVAL: %g\n", dtime() - d0);
if(GP->disable){
@@ -237,18 +259,34 @@ int main(int argc, char *argv[]){
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(devstat == BUSY_STATE && GP->wait) wait_busy();
+
+ 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->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(MAXSPEED, setmaxspd, "Can't read max speed");
+ Mesg("MAXSPEED: %g\n", dtime() - d0);
+
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*microstepping))
ERRX("Can't move to absolute position %d", GP->absmove);
Mesg("AbsMove: %g\n", dtime() - d0);
}
if(GP->relmove != INT_MIN && GP->relmove){
+ if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state");
uint8_t dir = 1;
SDO_write(&ENABLE, ID, 1);
if(GP->relmove < 0){ // negative direction