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

a first approach to CLI

Browse Source
This commit is contained in:
eddyem 2020-05-05 01:12:09 +03:00
parent dddd80a613
commit 57f4b9c1be
9 changed files with 298 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
commandline/Readme.md
View File

@ -3,3 +3,22 @@ Command line motor management
CAN controller: my CAN-USB sniffer
Usage: steppermove [args]
Where args are:
-0, --zeropos set current position to zero
-P, --pidfile=arg pidfile (default: /tmp/steppersmng.pid)
-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
-i, --nodeid=arg node ID (1..127)
-l, --logfile=arg file to save logs
-m, --maxspd=arg maximal motor speed (steps per second)
-r, --rel=arg move to relative position (steps)
-s, --canspd=arg CAN bus speed (default: DEFAULT_SPEED)
-t, --serialspd=arg serial (tty) device speed (default: DEFAULT_SPEED)
-u, --microsteps=arg microstepping (0..256)

3
commandline/canbus.c
View File

@ -100,6 +100,7 @@ static int ttyWR(const char *buff, int len){
return 1;
}
pthread_mutex_unlock(&mutex);
DBG("Success");
return w;
}
@ -228,6 +229,7 @@ void showM(CANmesg *m){
}
int canbus_read(CANmesg *mesg){
FNAME();
if(!mesg) return 1;
pthread_mutex_lock(&mutex);
double t0 = dtime();
@ -236,7 +238,6 @@ int canbus_read(CANmesg *mesg){
CANmesg *m;
while(dtime() - t0 < T_POLLING_TMOUT){ // read answer
if((ans = read_string())){ // parse new data
pthread_mutex_unlock(&mutex);
if((m = parseCANmesg(ans))){
DBG("Got canbus message:");
showM(m);

138
commandline/canopen.c
View File

@ -75,9 +75,10 @@ const char *abortcode_text(uint32_t abortcode){ //, int *n){
do{
++iter;
uint32_t c = AC[idx].code;
printf("idx=%d, min=%d, max=%d\n", idx, min_, max_);
//printf("idx=%d, min=%d, max=%d\n", idx, min_, max_);
if(c == abortcode){
//if(n) *n = iter;
//DBG("got : %s", AC[idx].errmsg);
return AC[idx].errmsg;
}else if(c > abortcode){
newidx = (idx + min_)/2;
@ -155,6 +156,26 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
return canbus_write(mesg);
}
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){
CANmesg mesg;
SDO *sdo = NULL;
double t0 = dtime();
while(dtime() - t0 < SDO_ANS_TIMEOUT){
mesg.ID = TSDO_COBID | NID; // read only from given ID
if(canbus_read(&mesg)){
continue;
}
sdo = parseSDO(&mesg);
if(!sdo) continue;
if(sdo->index == idx && sdo->subindex == subidx) break;
}
if(!sdo || sdo->index != idx || sdo->subindex != subidx){
WARNX("No answer from SDO 0x%X/0x%X", idx, subidx);
return NULL;
}
return sdo;
}
/**
* @brief readSDOvalue - send request to SDO read
* @param idx - SDO index
@ -163,25 +184,11 @@ static int ask2read(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){
SDO *sdo = NULL;
if(ask2read(idx, subidx, NID)){
WARNX("Can't initiate upload");
return NULL;
}
CANmesg mesg;
double t0 = dtime();
while(dtime() - t0 < SDO_ANS_TIMEOUT){
mesg.ID = TSDO_COBID | NID; // read only from given ID
if(canbus_read(&mesg)) continue;
sdo = parseSDO(&mesg);
if(!sdo) continue;
if(sdo->index == idx && sdo->subindex == subidx) break;
}
if(!sdo || sdo->index != idx || sdo->subindex != subidx){
WARNX("No answer for SDO reading");
return NULL;
}
return sdo;
return getSDOans(idx, subidx, NID);
}
static inline uint32_t mku32(uint8_t data[4]){
@ -215,7 +222,7 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
WARNX("SDO read error");
return INT64_MIN;
}
if(sdo->datalen == 0){
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);
@ -252,8 +259,101 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
return ans;
}
// write SDO data
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){
if(!e || !data || e->datasize < 1 || e->datasize > 4){
WARNX("SDO_write(): bad datalen");
return 1;
}
SDO sdo;
sdo.NID = NID;
sdo.ccs = CCS_INIT_DOWNLOAD;
sdo.datalen = e->datasize;
for(uint8_t i = 0; i < e->datasize; ++i) sdo.data[i] = data[i];
sdo.index = e->index;
sdo.subindex = e->subindex;
CANmesg *mesgp = mkMesg(&sdo);
if(canbus_write(mesgp)){
WARNX("SDO_write(): Can't initiate download");
return 2;
}
SDO *sdop = getSDOans(e->index, e->subindex, NID);
if(!sdop){
WARNX("SDO_write(): SDO read error");
return 3;
}
if(sdop->ccs == CCS_ABORT_TRANSFER){ // error
WARNX("SDO_write(): Got error for SDO 0x%X", e->index);
uint32_t ac = mku32(sdop->data);
const char *etxt = abortcode_text(ac);
if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt);
return 4;
}
if(sdop->datalen != 0){
WARNX("SDO_write(): got answer with non-zero length");
return 5;
}
if(sdop->ccs != CCS_SEG_UPLOAD){
WARNX("SDO_write(): got wrong answer");
return 6;
}
return 0;
}
int SDO_write(const SDO_dic_entry *e, _U_ uint8_t NID, int64_t data){
if(!e) return 1;
uint8_t arr[4] = {0};
uint32_t U;
int32_t I;
uint16_t U16;
int16_t I16;
if(e->issigned){
switch(e->datasize){
case 1:
arr[0] = (uint8_t) data;
break;
case 4:
I = (int32_t) data;
arr[0] = I&0xff;
arr[1] = (I>>8)&0xff;
arr[2] = (I>>16)&0xff;
arr[3] = (I>>24)&0xff;
break;
default: // can't be 3! 3->2
I16 = (int16_t) data;
arr[0] = I16&0xff;
arr[1] = (I16>>8)&0xff;
}
}else{
switch(e->datasize){
case 1:
arr[0] = (uint8_t) data;
break;
case 4:
U = (uint32_t) data;
arr[0] = U&0xff;
arr[1] = (U>>8)&0xff;
arr[2] = (U>>16)&0xff;
arr[3] = (U>>24)&0xff;
break;
default: // can't be 3! 3->2
U16 = (uint16_t) data;
arr[0] = U16&0xff;
arr[1] = (U16>>8)&0xff;
}
}
/*
DBG("DATA:");
for(int i = 0; i < e->datasize; ++i) printf("0x%X ", arr[i]);
printf("\n");
return 0;*/
return SDO_writeArr(e, NID, arr);
}
// read one byte of data
int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){
/*int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){
SDO *sdo = readSDOvalue(idx, subidx, NID);
if(!sdo || sdo->datalen != 1){
WARNX("Got SDO with wrong data length: %d instead of 1", sdo->datalen);
@ -261,7 +361,7 @@ int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID){
}
if(data) *data = sdo->data[0];
return 0;
}
}*/
#if 0
// write uint8_t to SDO with index idx & subindex subidx to NID

5
commandline/canopen.h
View File

@ -81,7 +81,10 @@ const char *abortcode_text(uint32_t abortcode);
SDO *parseSDO(CANmesg *mesg);
SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID);
int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID);
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data);
int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data);
//int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
#endif // CANOPEN_H__

14
commandline/cmdlnopts.c
View File

@ -25,6 +25,7 @@
#include <usefull_macros.h>
#include "cmdlnopts.h"
#include "pusirobot.h"
/*
* here are global parameters initialisation
@ -46,7 +47,9 @@ static glob_pars const Gdefault = {
.logfile = NULL, // don't save logs
.NodeID = 1, // default node ID = 1
.absmove = INT_MIN,
.relmove = INT_MIN
.relmove = INT_MIN,
.microsteps = -1,
.maxspeed = INT_MIN
};
/*
@ -62,8 +65,13 @@ 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)")},
{"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position")},
{"abs", NEED_ARG, NULL, 'r', arg_int, APTR(&G.absmove), _("move to absolute position")},
{"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 (steps)")},
{"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (steps)")},
{"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (steps 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")},
end_option
};

5
commandline/cmdlnopts.h
View File

@ -40,6 +40,11 @@ typedef struct{
int NodeID; // node ID to work with
int absmove; // absolute position to move
int relmove; // relative position to move
int microsteps; // set microstepping
int maxspeed; // max speed
int stop; // stop motor
int clearerr; // try to clear errors
int zeropos; // set position to zero
} glob_pars;

96
commandline/main.c
View File

@ -32,6 +32,7 @@ static glob_pars *GP = NULL; // for GP->pidfile need in `signals`
void signals(int sig){
putlog("Exit with status %d", sig);
DBG("Exit with status %d", sig);
restore_console();
if(GP->pidfile) // remove unnesessary PID file
unlink(GP->pidfile);
@ -57,6 +58,15 @@ int main(int argc, char *argv[]){
if(GP->NodeID != 1){
if(GP->NodeID < 1 || GP->NodeID > 127) ERRX("Node ID should be a number from 1 to 127");
}
if(GP->microsteps > 0 && (1 != __builtin_popcount(GP->microsteps) || GP->microsteps == 1)) // __builtin_popcount - amount of non-zero bits in uint
ERRX("Wrong microstepping settings, should be 0 or 2^(1..8)");
if(GP->absmove != INT_MIN || GP->relmove != INT_MIN){ // wanna move
if(GP->absmove != INT_MIN && GP->relmove != INT_MIN)
ERRX("ABSMOVE and RELMOVE can't be used together");
if(GP->maxspeed == 0)
ERRX("Set non-zero MAXSPEED");
}
if(GP->logfile) openlogfile(GP->logfile);
putlog(("Start application..."));
putlog("Try to open CAN bus device %s", GP->device);
@ -73,13 +83,89 @@ int main(int argc, char *argv[]){
//setup_con();
// print current position and state
int64_t i64;
if(INT64_MIN != (i64 = SDO_read(&DEVSTATUS, GP->NodeID)))
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);
if(INT64_MIN != (i64 = SDO_read(&POSITION, GP->NodeID)))
green("CURPOS=%d\n", (int)i64);
if(INT64_MIN != (i64 = SDO_read(&MAXSPEED, GP->NodeID)))
green("MAXSPEED=%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){
i64 = 0;
if(SDO_write(&POSITION, ID, i64))
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");
if(GP->stop){
if(SDO_write(&STOP, ID, 1))
ERRX("Can't stop motor");
}
if(GP->absmove != INT_MIN){
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;
if(GP->relmove < 0){ // negative direction
dir = 0;
GP->relmove = -GP->relmove;
}
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*microstepping))
ERRX("Can't move to relative position %d", GP->relmove);
}
#if 0
CANmesg m;
double t = dtime() - 10.;

38
commandline/pusirobot.c
View File

@ -16,12 +16,48 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//#include <limits.h>
#include <stdlib.h> // for NULL
// we should init constants here!
#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s};
#include "pusirobot.h"
// controller status for bits
static const char *DevStatus[] = {
"External stop 1",
"External stop 2",
"Stall state",
"Busy state",
"External stop 3",
"The FIFO of PVT Mode 3 is empty",
"FIFO Lower bound of PVT Mode 3",
"FIFO upper limit of PVT mode 3"
};
// controller error statuses
static const char *DevError[] = {
"TSD, over temperature shutdown",
"AERR, coil A error",
"BERR, coil B error",
"AOC, A over current",
"BOC, B over current",
"UVLO, low voltage fault"
};
// return status message for given bit in status
const char *devstatus(uint8_t status, uint8_t bit){
if(bit > 7) return NULL;
if(status & (1<<bit)) return DevStatus[bit];
return NULL;
}
// error codes explanation
const char *errname(uint8_t error, uint8_t bit){
if(bit > 5) return NULL;
if(error & (1<<bit)) return DevError[bit];
return NULL;
}
/*
// get current position for node ID `NID`, @return INT_MIN if error
int get_current_position(uint8_t NID){

11
commandline/pusirobot.h
View File

@ -69,9 +69,18 @@ DICENTRY(MAXCURNT, 0x600B, 0, 2, 0)
DICENTRY(POSITION, 0x600C, 0, 4, 0)
// motor enable
DICENTRY(ENABLE, 0x600E, 0, 1, 0)
// absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1)
// stop motor
DICENTRY(STOP, 0x6020, 0, 1, 0)
#define MAX_SPEED_MIN -200000
#define MAX_SPEED_MAX 200000
// unclearable status
#define BUSY_STATE (1<<3)
const char *devstatus(uint8_t status, uint8_t bit);
const char *errname(uint8_t error, uint8_t bit);
//int get_current_position(uint8_t NID);
#endif // PUSIROBOT_H__