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

add checking limit switches state; their work is still weird...

Browse Source
This commit is contained in:
eddyem 2020-05-09 21:19:37 +03:00
parent ef21a5f826
commit f709c1d68d
9 changed files with 128 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
commandline/Start_settings.cfg
View File

@ -3,6 +3,8 @@
# Set Node ID to 1 # Set Node ID to 1
0x2002, 0, 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 # Set microstepping to 32
0x600A, 0, 32 0x600A, 0, 32
# Set max phase current to 600mA # Set max phase current to 600mA
@ -12,13 +14,13 @@
# Set deceleration # Set deceleration
0x6009, 0, 5 0x6009, 0, 5
# EXT1 emergency stop # EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0)
# Rising edge trigger # Rising edge trigger for #2, falling for #1 (inverted!)
0x600F, 2, 1 0x600F, 2, 3
# Turn on pullup # Turn on pullup for #2 (EXT1 is always pulled down)
0x600F, 3, 1 0x600F, 3, 2
# Enable EXT1 # Enable EXT1, EXT2
0x600F, 1, 1 0x600F, 1, 3
# Save parameters # Save parameters
0x2007, 0, 2 0x2007, 0, 2

15
commandline/canbus.c
View File

@ -112,6 +112,11 @@ void setserialspeed(int speed){
serialspeed = speed; serialspeed = speed;
} }
static void clearRXbuf(){
double t0 = dtime();
while(read_string() && dtime() - t0 < T_POLLING_TMOUT){usleep(1000);}
}
int canbus_open(const char *devname){ int canbus_open(const char *devname){
if(!devname){ if(!devname){
WARNX("canbus_open(): need device name"); WARNX("canbus_open(): need device name");
@ -138,7 +143,7 @@ int canbus_setspeed(int speed){
} }
int len = snprintf(buff, BUFLEN, "b %d", speed); int len = snprintf(buff, BUFLEN, "b %d", speed);
if(len < 1) return 2; if(len < 1) return 2;
while(read_string()); // clear buffer clearRXbuf();
return ttyWR(buff, len); return ttyWR(buff, len);
} }
@ -153,7 +158,7 @@ int canbus_write(CANmesg *mesg){
rem -= l; len += l; rem -= l; len += l;
if(rem < 0) return 2; if(rem < 0) return 2;
} }
while(read_string()); // clear buffer clearRXbuf();
return ttyWR(buf, len); return ttyWR(buf, len);
} }
@ -220,6 +225,7 @@ CANmesg *parseCANmesg(const char *str){
return &m; return &m;
} }
#if 0
void showM(CANmesg *m){ void showM(CANmesg *m){
printf("TS=%d, ID=0x%X", m->timemark, m->ID); printf("TS=%d, ID=0x%X", m->timemark, m->ID);
int l = m->len; int l = m->len;
@ -227,6 +233,7 @@ void showM(CANmesg *m){
for(int i = 0; i < l; ++i) printf(" 0x%02X", m->data[i]); for(int i = 0; i < l; ++i) printf(" 0x%02X", m->data[i]);
printf("\n"); printf("\n");
} }
#endif
int canbus_read(CANmesg *mesg){ int canbus_read(CANmesg *mesg){
FNAME(); FNAME();
@ -239,8 +246,8 @@ int canbus_read(CANmesg *mesg){
while(dtime() - t0 < T_POLLING_TMOUT){ // read answer while(dtime() - t0 < T_POLLING_TMOUT){ // read answer
if((ans = read_string())){ // parse new data if((ans = read_string())){ // parse new data
if((m = parseCANmesg(ans))){ if((m = parseCANmesg(ans))){
DBG("Got canbus message:"); //DBG("Got canbus message:");
showM(m); //showM(m);
if(ID && m->ID == ID){ if(ID && m->ID == ID){
memcpy(mesg, m, sizeof(CANmesg)); memcpy(mesg, m, sizeof(CANmesg));
DBG("All OK"); DBG("All OK");

2
commandline/canopen.c
View File

@ -110,7 +110,7 @@ static CANmesg *mkMesg(SDO *sdo){
mesg.data[1] = sdo->index & 0xff; // l mesg.data[1] = sdo->index & 0xff; // l
mesg.data[2] = (sdo->index >> 8) & 0xff; // h mesg.data[2] = (sdo->index >> 8) & 0xff; // h
mesg.data[3] = sdo->subindex; mesg.data[3] = sdo->subindex;
#ifdef EBUG #if 0
FNAME(); FNAME();
green("Make message to 0x%X: ", mesg.ID); green("Make message to 0x%X: ", mesg.ID);
for(uint8_t i = 0; i < 8; ++i) printf("0x%02X ", mesg.data[i]); for(uint8_t i = 0; i < 8; ++i) printf("0x%02X ", mesg.data[i]);

1
commandline/cmdlnopts.c
View File

@ -74,6 +74,7 @@ static myoption cmdlnopts[] = {
{"zeropos", NO_ARGS, NULL, '0', arg_int, APTR(&G.zeropos), _("set current position to zero")}, {"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")}, {"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")}, {"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")},
{"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")},
end_option end_option
}; };

3
commandline/cmdlnopts.h
View File

@ -36,6 +36,7 @@ typedef struct{
char *pidfile; // name of PID file char *pidfile; // name of PID file
char *logfile; // logging to this file char *logfile; // logging to this file
char *parsefile; // file to parse char *parsefile; // file to parse
char *checkfile; // SDO data filename to check
int canspeed; // CAN bus speed int canspeed; // CAN bus speed
int serialspeed; // serial device speed (CAN-bus to USB) int serialspeed; // serial device speed (CAN-bus to USB)
int NodeID; // node ID to work with int NodeID; // node ID to work with
@ -46,7 +47,7 @@ typedef struct{
int stop; // stop motor int stop; // stop motor
int clearerr; // try to clear errors int clearerr; // try to clear errors
int zeropos; // set position to zero int zeropos; // set position to zero
char *checkfile; // SDO data filename to check int disable; // disable motor
} glob_pars; } glob_pars;

5
commandline/dataparser.c
View File

@ -125,7 +125,10 @@ int parse_data_file(const char *fname, uint8_t nid){
continue; continue;
} }
} }
if(nid) SDO_write(entry, nid, data); 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);
}
}while(0); }while(0);
if(!isgood){ if(!isgood){
WARNX("Bad syntax in line #%d: %s\nFormat: index, subindex, data (all may be hex, dec, oct or bin)", lineno, str); WARNX("Bad syntax in line #%d: %s\nFormat: index, subindex, data (all may be hex, dec, oct or bin)", lineno, str);

2
commandline/dicentries.h
View File

@ -60,6 +60,8 @@ DICENTRY(EXTENABLE, 0x600F, 1, 1, 0)
DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0) DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0)
// EXT emergency sensor type // EXT emergency sensor type
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0) DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0)
// GPIO value
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0)
// absolute displacement // absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1) DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1)
// stop motor // stop motor

150
commandline/main.c
View File

@ -30,6 +30,8 @@
#include "pusirobot.h" #include "pusirobot.h"
static glob_pars *GP = NULL; // for GP->pidfile need in `signals` static glob_pars *GP = NULL; // for GP->pidfile need in `signals`
static uint8_t ID = 0;
static uint16_t microstepping = 0;
void signals(int sig){ void signals(int sig){
putlog("Exit with status %d", sig); putlog("Exit with status %d", sig);
@ -45,6 +47,74 @@ void iffound_default(pid_t pid){
ERRX("Another copy of this process found, pid=%d. Exit.", pid); ERRX("Another copy of this process found, pid=%d. Exit.", pid);
} }
static inline void chkerr(int64_t es){
if(!es) return;
red("ERRSTATE=%d\n", es);
uint8_t s = (uint8_t)es;
for(uint8_t i = 0; i < 8; ++i){
const char *msg = errname(s, i);
if(msg) red("\t%s\n", msg);
}
if(!GP->clearerr) ERRX("Error status is not zero");
if(SDO_write(&ERRSTATE, ID, s) || 0 != (es = SDO_read(&ERRSTATE, ID))){
ERRX("Can't clean error status");
}
}
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){
for(uint8_t i = 0; i < 8; ++i){
const char *msg = devstatus(s, 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)))){
ERRX("Can't clean device status");
}
if(es && es != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy
}
}
static inline void setusteps(int64_t es){
if(GP->microsteps > -1 && GP->microsteps != (int) es){
DBG("Try to change microsteps");
if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID)))
ERRX("Can't change microstepping");
}
microstepping = (uint16_t) es;
green("MICROSTEPPING=%u\n", microstepping);
}
static inline void setmaxspd(int64_t es){
DBG("abs=%d, rel=%d", GP->absmove, GP->relmove);
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);
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);
else red("MAXSPEED=0\n");
}
static inline void gpioval(int64_t es){
uint16_t v = (uint16_t) es;
if(INT64_MIN == (es = SDO_read(&EXTENABLE, ID))){
WARNX("Can't read limit switches configuration");
return;
}
for(int i = 1; i < 4; ++i){
if(0 == (es & 1<<(i-1))) continue; // endswitch disabled
if(EXTACTIVE(i, v)) red("LIM%d=1\n", i);
}
}
int main(int argc, char *argv[]){ int main(int argc, char *argv[]){
initial_setup(); initial_setup();
char *self = strdup(argv[0]); char *self = strdup(argv[0]);
@ -92,79 +162,43 @@ int main(int argc, char *argv[]){
//setup_con(); //setup_con();
// print current position and state // print current position and state
int64_t i64; int64_t i64;
uint8_t ID = GP->NodeID; ID = GP->NodeID;
if(INT64_MIN != (i64 = SDO_read(&ERRSTATE, ID))){ #define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
if(i64){ #define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else WARNX(e);}while(0)
red("ERRSTATE=%d\n", i64); getSDOe(ERRSTATE, chkerr, "Can't get error status");
uint8_t s = (uint8_t)i64; getSDOe(DEVSTATUS, chkstat, "Can't get device status");
for(uint8_t i = 0; i < 8; ++i){ getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
const char *msg = errname(s, i);
if(msg) red("\t%s\n", msg);
}
if(!GP->clearerr) ERRX("Error status is not zero");
if(SDO_write(&ERRSTATE, ID, s) || 0 != (i64 = SDO_read(&ERRSTATE, ID))){
ERRX("Can't clean error status");
}
}
}
if(INT64_MIN != (i64 = SDO_read(&DEVSTATUS, ID))){
green("DEVSTATUS=%d\n", (int)i64);
uint8_t s = (uint8_t)i64;
if(s){
for(uint8_t i = 0; i < 8; ++i){
const char *msg = devstatus(s, i);
if(msg) red("\t%s\n", msg);
}
if(s != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, s) || 0 != (i64 = SDO_read(&DEVSTATUS, ID)))){
ERRX("Can't clean device status");
}
if(i64 && i64 != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy
}
}else ERRX("Can't get device status");
if(GP->zeropos){ if(GP->zeropos){
i64 = 0; if(SDO_write(&POSITION, ID, 0))
if(SDO_write(&POSITION, ID, i64))
ERRX("Can't clear position counter"); ERRX("Can't clear position counter");
} }
uint16_t microstepping = 0;
if(INT64_MIN != (i64 = SDO_read(&MICROSTEPS, ID))){
if(GP->microsteps > -1 && GP->microsteps != (int) i64){
DBG("Try to change microsteps");
if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (i64 = SDO_read(&MICROSTEPS, ID)))
ERRX("Can't change microstepping");
}
microstepping = (uint16_t) i64;
green("MICROSTEPPING=%u\n", microstepping);
}else ERRX("Can't get microstepping");
if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))) if(INT64_MIN != (i64 = SDO_read(&POSITION, ID)))
green("CURPOS=%d\n", (int)i64/microstepping); green("CURPOS=%d\n", (int)i64/microstepping);
else ERRX("Can't read current position"); else ERRX("Can't read current position");
if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, ID))){ getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
DBG("abs=%d, rel=%d", GP->absmove, GP->relmove); getSDOw(GPIOVAL, gpioval, "Can't read GPIO values");
if(i64 == 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->disable){
if(GP->maxspeed != INT_MIN){ if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor");
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); if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
DBG("Try to change max speed"); if(i64) green("ENABLE=1\n");
if(SDO_write(&MAXSPEED, ID, GP->maxspeed) || INT64_MIN == (i64 = SDO_read(&MAXSPEED, ID))) else red("ENABLE=0\n");
ERRX("Can't change max speed"); }
}
green("MAXSPEED=%d\n", (int)i64/microstepping);
}else ERRX("Can't read max speed");
if(GP->stop){ if(GP->stop){
if(SDO_write(&STOP, ID, 1)) if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
ERRX("Can't stop motor");
} }
if(GP->absmove != INT_MIN){ if(GP->absmove != INT_MIN){
SDO_write(&ENABLE, ID, 1);
if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping)) if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping))
ERRX("Can't move to absolute position %d", GP->absmove); ERRX("Can't move to absolute position %d", GP->absmove);
} }
if(GP->relmove != INT_MIN && GP->relmove){ if(GP->relmove != INT_MIN && GP->relmove){
uint8_t dir = 1; uint8_t dir = 1;
SDO_write(&ENABLE, ID, 1);
if(GP->relmove < 0){ // negative direction if(GP->relmove < 0){ // negative direction
dir = 0; dir = 0;
GP->relmove = -GP->relmove; GP->relmove = -GP->relmove;
@ -175,6 +209,8 @@ int main(int argc, char *argv[]){
if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping)) if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping))
ERRX("Can't move to relative position %d", GP->relmove); ERRX("Can't move to relative position %d", GP->relmove);
} }
#undef getSDOe
#if 0 #if 0
CANmesg m; CANmesg m;
double t = dtime() - 10.; double t = dtime() - 10.;

5
commandline/pusirobot.h
View File

@ -38,6 +38,11 @@ typedef struct{
#define MAX_SPEED_MIN -200000 #define MAX_SPEED_MIN -200000
#define MAX_SPEED_MAX 200000 #define MAX_SPEED_MAX 200000
// limit switches mask in GPIO status register (x=1,2,3)
#define EXTMASK(x) (1<<(6+x))
#define EXTACTIVE(x, reg) ((reg&EXTMASK(x)) ? 1:0)
// unclearable status // unclearable status
#define BUSY_STATE (1<<3) #define BUSY_STATE (1<<3)
const char *devstatus(uint8_t status, uint8_t bit); const char *devstatus(uint8_t status, uint8_t bit);