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
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 600mA
@ -12,13 +14,13 @@
# Set deceleration
0x6009, 0, 5
# EXT1 emergency stop
# Rising edge trigger
0x600F, 2, 1
# Turn on pullup
0x600F, 3, 1
# Enable EXT1
0x600F, 1, 1
# 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, 3
# Turn on pullup for #2 (EXT1 is always pulled down)
0x600F, 3, 2
# Enable EXT1, EXT2
0x600F, 1, 3
# Save parameters
0x2007, 0, 2

15
commandline/canbus.c
View File

@ -112,6 +112,11 @@ void setserialspeed(int 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){
if(!devname){
WARNX("canbus_open(): need device name");
@ -138,7 +143,7 @@ int canbus_setspeed(int speed){
}
int len = snprintf(buff, BUFLEN, "b %d", speed);
if(len < 1) return 2;
while(read_string()); // clear buffer
clearRXbuf();
return ttyWR(buff, len);
}
@ -153,7 +158,7 @@ int canbus_write(CANmesg *mesg){
rem -= l; len += l;
if(rem < 0) return 2;
}
while(read_string()); // clear buffer
clearRXbuf();
return ttyWR(buf, len);
}
@ -220,6 +225,7 @@ CANmesg *parseCANmesg(const char *str){
return &m;
}
#if 0
void showM(CANmesg *m){
printf("TS=%d, ID=0x%X", m->timemark, m->ID);
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]);
printf("\n");
}
#endif
int canbus_read(CANmesg *mesg){
FNAME();
@ -239,8 +246,8 @@ int canbus_read(CANmesg *mesg){
while(dtime() - t0 < T_POLLING_TMOUT){ // read answer
if((ans = read_string())){ // parse new data
if((m = parseCANmesg(ans))){
DBG("Got canbus message:");
showM(m);
//DBG("Got canbus message:");
//showM(m);
if(ID && m->ID == ID){
memcpy(mesg, m, sizeof(CANmesg));
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[2] = (sdo->index >> 8) & 0xff; // h
mesg.data[3] = sdo->subindex;
#ifdef EBUG
#if 0
FNAME();
green("Make message to 0x%X: ", mesg.ID);
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")},
{"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")},
{"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")},
end_option
};

3
commandline/cmdlnopts.h
View File

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

5
commandline/dataparser.c
View File

@ -125,7 +125,10 @@ int parse_data_file(const char *fname, uint8_t nid){
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);
if(!isgood){
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)
// EXT emergency sensor type
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0)
// GPIO value
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0)
// absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1)
// stop motor

150
commandline/main.c
View File

@ -30,6 +30,8 @@
#include "pusirobot.h"
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){
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);
}
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[]){
initial_setup();
char *self = strdup(argv[0]);
@ -92,79 +162,43 @@ int main(int argc, char *argv[]){
//setup_con();
// print current position and state
int64_t i64;
uint8_t ID = GP->NodeID;
if(INT64_MIN != (i64 = SDO_read(&ERRSTATE, ID))){
if(i64){
red("ERRSTATE=%d\n", i64);
uint8_t s = (uint8_t)i64;
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 != (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");
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)
getSDOe(ERRSTATE, chkerr, "Can't get error status");
getSDOe(DEVSTATUS, chkstat, "Can't get device status");
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
if(GP->zeropos){
i64 = 0;
if(SDO_write(&POSITION, ID, i64))
if(SDO_write(&POSITION, ID, 0))
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)))
green("CURPOS=%d\n", (int)i64/microstepping);
else ERRX("Can't read current position");
if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, ID))){
DBG("abs=%d, rel=%d", GP->absmove, GP->relmove);
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->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 == (i64 = SDO_read(&MAXSPEED, ID)))
ERRX("Can't change max speed");
}
green("MAXSPEED=%d\n", (int)i64/microstepping);
}else ERRX("Can't read max speed");
getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
getSDOw(GPIOVAL, gpioval, "Can't read GPIO values");
if(GP->disable){
if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor");
}
if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
if(i64) green("ENABLE=1\n");
else red("ENABLE=0\n");
}
if(GP->stop){
if(SDO_write(&STOP, ID, 1))
ERRX("Can't stop motor");
if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
}
if(GP->absmove != INT_MIN){
SDO_write(&ENABLE, ID, 1);
if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping))
ERRX("Can't move to absolute position %d", GP->absmove);
}
if(GP->relmove != INT_MIN && GP->relmove){
uint8_t dir = 1;
SDO_write(&ENABLE, ID, 1);
if(GP->relmove < 0){ // negative direction
dir = 0;
GP->relmove = -GP->relmove;
@ -175,6 +209,8 @@ int main(int argc, char *argv[]){
if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping))
ERRX("Can't move to relative position %d", GP->relmove);
}
#undef getSDOe
#if 0
CANmesg m;
double t = dtime() - 10.;

5
commandline/pusirobot.h
View File

@ -38,6 +38,11 @@ typedef struct{
#define MAX_SPEED_MIN -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
#define BUSY_STATE (1<<3)
const char *devstatus(uint8_t status, uint8_t bit);