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 bugs

Browse Source
This commit is contained in:
Edward Emelianov 2020-09-23 17:31:01 +03:00
parent 43de7a0259
commit 240b5f8677
14 changed files with 222 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
commandline/Start_settings.cfg
View File

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

29
commandline/canbus.c
View File

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

1
commandline/canbus.h
View File

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

11
commandline/canopen.c
View File

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

13
commandline/cfg/Default_settings.cfg Normal file
View File

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

19
commandline/cfg/Start_settings.cfg Normal file
View File

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

11
commandline/cfg/Start_settings_onlySpeed.cfg Normal file
View File

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

13
commandline/cfg/endswitches.cfg Normal file
View File

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

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

@ -0,0 +1,2 @@
# Enable EXT1, EXT2
0x600F, 1, 3

8
commandline/cfg/move Executable file
View File

@ -0,0 +1,8 @@
#!/bin/bash
while true; do
./steppermove -m100 -w -I4 -r 32
./steppermove -m200 -w -I4 -r -64
sleep 1
done

1
commandline/cmdlnopts.c
View File

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

1
commandline/cmdlnopts.h
View File

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

45
commandline/dicentries.in
View File

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

114
commandline/main.c
View File

@ -16,7 +16,6 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "dataparser.h"
#include <signal.h>
#include <stdio.h>
#include <string.h>
@ -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