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

start adding simple stepper

Browse Source
This commit is contained in:
Edward Emelianov 2020-11-19 18:46:18 +03:00
parent 2502c071cc
commit b0f487ce3f
12 changed files with 421 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
canserver/4tests.txt
View File

@ -8,3 +8,13 @@ mesg x 10 0x6001 0
#emul
register new 3333 emulation
#stepper
register x 0x58a stepper
#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");

1
canserver/aux.c
View File

@ -196,5 +196,6 @@ int str2long(char *str, long* l){
long n = strtol(str, &eptr, 0);
if(*eptr) return 2; // wrong symbols in number
if(l) *l = n;
//DBG("str '%s' to long %ld", str, n);
return 0;
}

283
canserver/canopen.c
View File

@ -24,12 +24,6 @@
#include "canopen.h"
typedef struct{
uint32_t code;
const char *errmsg;
} abortcodes;
static const abortcodes AC[] = {
//while read l; do N=$(echo $l|awk '{print $1 $2}'); R=$(echo $l|awk '{$1=$2=""; print substr($0,3)}'|sed 's/\.//'); echo -e "{0x$N, \"$R\"},"; done < codes.b
{0x05030000, "Toggle bit not alternated"},
@ -65,21 +59,19 @@ static const abortcodes AC[] = {
static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1;
// used
/**
* @brief abortcode_text - explanation of abort code
* @brief abortcode_search - explanation of abort code
* @param abortcode - code
* @return text for error or NULL
* @return abortcode found or NULL
*/
const char *abortcode_text(uint32_t abortcode){ //, int *n){
static const abortcodes *abortcode_search(uint32_t abortcode){ //, int *n){
int idx = ACmax/2, min_ = 0, max_ = ACmax, newidx = 0, iter=0;
do{
++iter;
uint32_t c = AC[idx].code;
//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;
return &AC[idx];
}else if(c > abortcode){
newidx = (idx + min_)/2;
max_ = idx;
@ -89,37 +81,43 @@ const char *abortcode_text(uint32_t abortcode){ //, int *n){
min_ = idx;
}
if(newidx == idx || min_ < 0 || max_ > ACmax){
//if(n) *n = 0;
return NULL;
}
idx = newidx;
}while(1);
}
// make CAN message from sdo object; don't support more then one block/packet
static CANmesg *mkMesg(SDO *sdo){
static CANmesg mesg;
mesg.ID = RSDO_COBID + sdo->NID;
mesg.len = 8;
memset(mesg.data, 0, 8);
mesg.data[0] = SDO_CCS(sdo->ccs);
// used
/**
* @brief mkMesg - make CAN message from sdo object
* @param sdo (i) - sdo object to transform
* @param mesg (o) - output CANmesg
* @return pointer to mesg
*/
CANmesg *mkMesg(SDO *sdo, CANmesg *mesg){
if(!sdo || !mesg) return NULL;
mesg->ID = RSDO_COBID + sdo->NID;
mesg->len = 8;
memset(mesg->data, 0, 8);
mesg->data[0] = SDO_CCS(sdo->ccs);
if(sdo->datalen){ // send N bytes of data
mesg.data[0] |= SDO_N(sdo->datalen) | SDO_E | SDO_S;
for(uint8_t i = 0; i < sdo->datalen; ++i) mesg.data[4+i] = sdo->data[i];
mesg->data[0] |= SDO_N(sdo->datalen) | SDO_E | SDO_S;
for(uint8_t i = 0; i < sdo->datalen; ++i) mesg->data[4+i] = sdo->data[i];
}
mesg.data[1] = sdo->index & 0xff; // l
mesg.data[2] = (sdo->index >> 8) & 0xff; // h
mesg.data[3] = sdo->subindex;
return &mesg;
mesg->data[1] = sdo->index & 0xff; // l
mesg->data[2] = (sdo->index >> 8) & 0xff; // h
mesg->data[3] = sdo->subindex;
return mesg;
}
// used
/**
* @brief parseSDO - transform CAN-message to SDO
* @param mesg (i) - message
* @param sdo (o) - SDO
* @return sdo or NULL depending on result
*/
SDO *parseSDO(CANmesg *mesg, SDO *sdo){
SDO *parseSDO(const CANmesg *mesg, SDO *sdo){
if(mesg->len != 8){
WARNX("Wrong SDO data length");
return NULL;
@ -137,7 +135,7 @@ SDO *parseSDO(CANmesg *mesg, SDO *sdo){
if((spec & SDO_E) && (spec & SDO_S)) sdo->datalen = SDO_datalen(spec);
else if(sdo->ccs == CCS_ABORT_TRANSFER) sdo->datalen = 4; // error code
else sdo->datalen = 0; // no data in message
for(uint8_t i = 0; i < sdo->datalen; ++i) sdo->data[i] = mesg->data[4+i];
for(uint8_t i = 0; i < 4; ++i) sdo->data[i] = mesg->data[4+i];
DBG("Got TSDO from NID=%d, ccs=%u, index=0x%X, subindex=0x%X, datalen=%d",
sdo->NID, sdo->ccs, sdo->index, sdo->subindex, sdo->datalen);
return sdo;
@ -151,8 +149,8 @@ static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){
sdo.datalen = 0;
sdo.index = idx;
sdo.subindex = subidx;
CANmesg *mesg = mkMesg(&sdo);
return canbus_write(mesg);
CANmesg mesg;
return canbus_write(mkMesg(&sdo, &mesg));
}
static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
@ -195,74 +193,162 @@ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo){
return getSDOans(idx, subidx, NID, sdo);
}
static inline uint32_t mku32(uint8_t data[4]){
// mk... are all used
static inline uint32_t mku32(const uint8_t data[4]){
return (uint32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
}
static inline uint16_t mku16(uint8_t data[4]){
static inline uint16_t mku16(const uint8_t data[4]){
return (uint16_t)(data[0] | (data[1]<<8));
}
static inline uint8_t mku8(uint8_t data[4]){
static inline uint8_t mku8(const uint8_t data[4]){
return data[0];
}
static inline int32_t mki32(uint8_t data[4]){
static inline int32_t mki32(const uint8_t data[4]){
return (int32_t)(data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24));
}
static inline int16_t mki16(uint8_t data[4]){
static inline int16_t mki16(const uint8_t data[4]){
return (int16_t)(data[0] | (data[1]<<8));
}
static inline int8_t mki8(uint8_t data[4]){
static inline int8_t mki8(const uint8_t data[4]){
return (int8_t)data[0];
}
// read SDO value, if error - return INT64_MIN
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
FNAME();
SDO sdo;
if(!readSDOvalue(e->index, e->subindex, NID, &sdo)){
return INT64_MIN;
}
if(sdo.ccs == CCS_ABORT_TRANSFER){ // error
// used
/**
* @brief getSDOval - get value from SDO
* @param sdo (i) - SDO
* @param e (i) - its dictionary entry
* @param ac (o) - pointer for abort code (in case of error) or NULL
* @return value, INT64_MIN in case of error or INT64_MAX in case of zero-length
*/
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac){
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) WARNX("Abort code 0x%X: %s", ac, etxt);
uint32_t c = mku32(sdo->data);
const abortcodes *abc = abortcode_search(c);
if(abc) WARNX("Abort code 0x%X: %s", ac, abc->errmsg);
if(ac) *ac = abc;
return INT64_MIN;
}
if(sdo.datalen != e->datasize){
WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo.datalen, e->datasize);
if(sdo->datalen == 0) return INT_MAX;
if(sdo->datalen != e->datasize){
WARNX("Got SDO with length %d instead of %d (as in dictionary)", sdo->datalen, e->datasize);
}
int64_t ans = 0;
if(e->issigned){
switch(sdo.datalen){
switch(sdo->datalen){
case 1:
ans = mki8(sdo.data);
ans = mki8(sdo->data);
break;
case 4:
ans = mki32(sdo.data);
ans = mki32(sdo->data);
break;
default: // can't be 3! 3->2
ans = mki16(sdo.data);
ans = mki16(sdo->data);
}
}else{
switch(sdo.datalen){
switch(sdo->datalen){
case 1:
ans = mku8(sdo.data);
ans = mku8(sdo->data);
break;
case 4:
ans = mku32(sdo.data);
ans = mku32(sdo->data);
break;
default: // can't be 3! 3->2
ans = mku16(sdo.data);
ans = mku16(sdo->data);
}
}
return ans;
}
// used
/**
* @brief SDO_read - form CANmesg to read SDO entry `e`
* @param e (i) - SDO dictionary entry to read
* @param NID - target node ID
* @param cm (o) - pointer to CANmesg which to modify
* @return `cm` or NULL if failed
*/
CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm){
if(!e || !cm) return NULL;
SDO sdo = {
.NID = NID,
.ccs = CCS_INIT_UPLOAD,
.index = e->index,
.subindex = e->subindex
};
/*sdo.NID = NID;
sdo.ccs = CCS_INIT_UPLOAD;
sdo.index = e->index;
sdo.subindex = e->subindex;*/
return mkMesg(&sdo, cm);
}
// used
/**
* @brief SDO_write - form CANmesg to write `data` to SDO entry `e`
* @param e
* @param NID
* @param cm
* @return
*/
CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *cm){
if(!e || !cm) return NULL;
uint32_t U;
int32_t I;
uint16_t U16;
int16_t I16;
SDO sdo;
sdo.NID = NID;
sdo.ccs = CCS_INIT_DOWNLOAD;
sdo.datalen = e->datasize;
sdo.index = e->index;
sdo.subindex = e->subindex;
DBG("datalen=%d, signed=%d", e->datasize, e->issigned);
if(e->issigned){
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
I = (int32_t) data;
sdo.data[0] = I&0xff;
sdo.data[1] = (I>>8)&0xff;
sdo.data[2] = (I>>16)&0xff;
sdo.data[3] = (I>>24)&0xff;
break;
default: // can't be 3! 3->2
I16 = (int16_t) data;
sdo.data[0] = I16&0xff;
sdo.data[1] = (I16>>8)&0xff;
}
}else{
switch(e->datasize){
case 1:
sdo.data[0] = (uint8_t) data;
break;
case 4:
U = (uint32_t) data;
sdo.data[0] = U&0xff;
sdo.data[1] = (U>>8)&0xff;
sdo.data[2] = (U>>16)&0xff;
sdo.data[3] = (U>>24)&0xff;
break;
default: // can't be 3! 3->2
U16 = (uint16_t) data;
sdo.data[0] = U16&0xff;
sdo.data[1] = (U16>>8)&0xff;
}
}
mkMesg(&sdo, cm);
return cm;
}
// write SDO data, return 0 if all OK
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
FNAME();
@ -277,9 +363,10 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
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);
CANmesg mesgp;
mkMesg(&sdo, &mesgp);
DBG("Canbus write..");
if(canbus_write(mesgp)){
if(canbus_write(&mesgp)){
WARNX("SDO_write(): Can't initiate download");
return 2;
}
@ -292,8 +379,8 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
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);
const abortcodes *e = abortcode_search(ac);
if(e) WARNX("Abort code 0x%X: %s", ac, e->errmsg);
return 4;
}
if(sdop.datalen != 0){
@ -306,75 +393,3 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
}
return 0;
}
int SDO_write(const SDO_dic_entry *e, 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){
SDO *sdo = readSDOvalue(idx, subidx, NID);
if(!sdo || sdo->datalen != 1){
WARNX("Got SDO with wrong data length: %d instead of 1", sdo->datalen);
return 1;
}
if(data) *data = sdo->data[0];
return 0;
}*/
#if 0
// write uint8_t to SDO with index idx & subindex subidx to NID
void SDO_writeU8(uint16_t idx, uint8_t subidx, uint8_t data, uint8_t NID){
SDO sdo;
sdo.NID = NID;
sdo.ccs = CCS_INIT_DOWNLOAD;
}
#endif

18
canserver/canopen.h
View File

@ -77,14 +77,18 @@ typedef struct{
uint8_t ccs; // Client command specifier
} SDO;
const char *abortcode_text(uint32_t abortcode);
SDO *parseSDO(CANmesg *mesg, SDO *sdo);
typedef struct{
uint32_t code;
const char *errmsg;
} abortcodes;
CANmesg *mkMesg(SDO *sdo, CANmesg *mesg);
SDO *parseSDO(const CANmesg *mesg, SDO *sdo);
SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID, SDO *sdo);
int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID);
int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data);
int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data);
int64_t getSDOval(const SDO *sdo, const SDO_dic_entry *e, const abortcodes **ac);
CANmesg *SDO_read(const SDO_dic_entry *e, uint8_t NID, CANmesg *cm);
CANmesg *SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data, CANmesg *cm);
//int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
#endif // CANOPEN_H__

121
canserver/dicentries.in
View File

@ -21,123 +21,78 @@
// variable name / index / subindex / datasize / issigned / name
// 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")
DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0, "heartbeat time", "hearbt")
// node ID
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID")
DICENTRY(NODEID, 0x2002, 0, 1, 0, "node ID", "nodeid")
// baudrate
DICENTRY(BAUDRATE, 0x2003, 0, 1, 0, "baudrate")
DICENTRY(BAUDRATE, 0x2003, 0, 1, 0, "baudrate", "baudrate")
// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings
DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0, "system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings")
DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0, "system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings", "syscontrol")
// error status
DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status")
DICENTRY(ERRSTATE, 0x6000, 0, 1, 0, "error status", "errstatus")
// controller status
DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status")
DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0, "controller status", "devstatus")
// rotation direction
DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction")
DICENTRY(ROTDIR, 0x6002, 0, 1, 0, "rotation direction", "rotdir")
// maximal speed
DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed")
DICENTRY(MAXSPEED, 0x6003, 0, 4, 1, "maximal speed", "maxspeed")
// relative displacement
DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement")
DICENTRY(RELSTEPS, 0x6004, 0, 4, 0, "relative displacement", "relsteps")
// operation mode
DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode")
DICENTRY(OPMODE, 0x6005, 0, 1, 0, "operation mode", "opmode")
// start speed
DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed")
DICENTRY(STARTSPEED, 0x6006, 0, 2, 0, "start speed", "startspd")
// stop speed
DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed")
DICENTRY(STOPSPEED, 0x6007, 0, 2, 0, "stop speed", "stopspd")
// acceleration coefficient
DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient")
DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0, "acceleration coefficient", "acccoef")
// deceleration coefficient
DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient")
DICENTRY(DECELCOEF, 0x6009, 0, 1, 0, "deceleration coefficient", "deccoef")
// microstepping
DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping")
DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0, "microstepping", "microsteps")
// max current
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current")
DICENTRY(MAXCURNT, 0x600B, 0, 2, 0, "maximum phase current", "maxcurnt")
// current position
DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position")
DICENTRY(POSITION, 0x600C, 0, 4, 0, "current position", "curpos")
// current reduction
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction")
DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction", "curred")
// motor enable
DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable")
DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable", "enable")
// EXT emergency stop enable
DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable")
DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable", "extenable")
// EXT emergency stop trigger mode
DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0, "EXT emergency stop trigger mode")
DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0, "EXT emergency stop trigger mode", "exttrigmod")
// EXT emergency sensor type
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type")
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0, "EXT emergency sensor type", "extsenstype")
// GPIO direction
DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction")
DICENTRY(GPIODIR, 0x6011, 1, 2, 0, "GPIO direction", "gpiodir")
// GPIO configuration
DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration")
DICENTRY(GPIOCONF, 0x6011, 2, 4, 0, "GPIO configuration", "gpioconf")
// GPIO value
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value")
DICENTRY(GPIOVAL, 0x6012, 0, 2, 0, "GPIO value", "gpioval")
// stall parameters
DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)")
DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)", "stallpars")
// offline operation
DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command")
DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable")
DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command", "offlnmbr")
DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable", "offlenbl")
// stall set
DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)")
DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)", "stallset")
// absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement")
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1, "absolute displacement", "abssteps")
// stop motor
DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor")
DICENTRY(STOP, 0x6020, 0, 1, 0, "stop motor", "stop")
// encoder resolution
DICENTRY(ENCRESOL, 0x6021, 0, 2, 0, "encoder resolution (closed loop)")
DICENTRY(ENCRESOL, 0x6021, 0, 2, 0, "encoder resolution (closed loop)", "encresol")
// stall length parameter
DICENTRY(STALLLEN, 0x6028, 0, 2, 0, "stall length parameter (closed loop)")
DICENTRY(STALLLEN, 0x6028, 0, 2, 0, "stall length parameter (closed loop)", "stallen")
// torque ring enable
DICENTRY(TORQRING, 0x6029, 0, 1, 0, "torque ring enable (closed loop)")
DICENTRY(TORQRING, 0x6029, 0, 1, 0, "torque ring enable (closed loop)", "torqring")
// autosave position
DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)")
DICENTRY(POSAUTOSAVE, 0x602A, 0, 1, 0, "autosave position (closed loop)", "autosave")
// real time speed
DICENTRY(REALTIMESPD, 0x6030, 0, 2, 1, "real time speed (closed loop)")
DICENTRY(REALTIMESPD, 0x6030, 0, 2, 1, "real time speed (closed loop)", "realtimespd")
// calibration zero
DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero")
DICENTRY(CALIBZERO, 0x6034, 0, 4, 1, "calibration zero", "calibzero")
// encoder position
DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position")
DICENTRY(ENCPOS, 0x6035, 0, 4, 1, "encoder position", "encpos")

183
canserver/processmotors.c
View File

@ -24,6 +24,7 @@
#include "socket.h"
#include <fcntl.h> // open
#include <inttypes.h> // PRId64
#include <stdio.h> // printf
#include <string.h> // strcmp
#include <sys/stat.h> // open
@ -37,18 +38,29 @@ static message CANbusMessages = {0}; // CANserver thread is master
#define CANBUSPUSH(mesg) mesgAddObj(&CANbusMessages, mesg, sizeof(CANmesg))
#define CANBUSPOP() mesgGetObj(&CANbusMessages, NULL)
// commands sent to threads
// each threadCmd array should be terminated with NULLs; default command `help` shows all names/descriptions
typedef struct{
char *name; // command name
int nargs; // max arguments number
long *args; // pointer to arguments
char *descr; // description for help
} threadCmd;
// basic threads
// messages: master - thread, slave - caller
static void *stpemulator(void *arg);
static void *rawcommands(void *arg);
static void *canopencmds(void *arg);
static void *simplestp(void *arg);
// handlers for standard types
thread_handler CANhandlers[] = {
{"emulation", stpemulator},
{"raw", rawcommands},
{"canopen", canopencmds},
{NULL, NULL}
{"canopen", canopencmds, "NodeID index subindex [data] - raw CANOpen commands with `index` and `subindex` to `NodeID`"},
{"emulation", stpemulator, "(list) - stepper emulation"},
{"raw", rawcommands, "ID [DATA] - raw CANbus commands to raw `ID` with `DATA`"},
{"stepper", simplestp, "(list) - simple stepper motor: no limit switches, only goto"},
{NULL, NULL, NULL}
};
thread_handler *get_handler(const char *name){
@ -59,6 +71,51 @@ thread_handler *get_handler(const char *name){
return NULL;
}
/**
* @brief cmdParser - parser of user's comands
* @param cmdlist - NULL-terminated array with possible commands
* @param cmd - user's command (default command `list` shows all names/descriptions)
* @return index of command in `cmdlist` or -1 if something wrong or not found
*/
static int cmdParser(threadCmd *cmdlist, char *s){
if(!cmdlist || !s) return -1;
char *saveptr, *command;
int idx = 0;
command = strtok_r(s, " \t,;\r\n", &saveptr);
if(!command) return -1;
if(strcmp(command, "list") == 0){ // just show help
char buf[128];
while(cmdlist->name){
snprintf(buf, 128, "params> %s (%d arguments) - %s",
cmdlist->name, cmdlist->nargs, cmdlist->descr);
mesgAddText(&ServerMessages, buf);
++cmdlist;
}
return 10000;
}
while(cmdlist->name){
if(strcmp(cmdlist->name, command) == 0) break;
++idx; ++cmdlist;
}
if(!cmdlist->name) return -1; // command not found
if(cmdlist->nargs == 0) return idx; // simple command
int nargsplus1 = cmdlist->nargs + 1, N = 0;
long *args = MALLOC(long, nargsplus1);
for(; N < nargsplus1; ++N){
char *nxt = strtok_r(NULL, " \t,;\r\n", &saveptr);
if(!nxt) break;
if(str2long(nxt, &args[N])) break;
}
if(N != cmdlist->nargs){
FREE(args);
return -1; // bad arguments number
}
for(int i = 0; i < N; ++i)
cmdlist->args[i] = args[i];
FREE(args);
return idx;
}
/**
* @brief parsePacket - convert text into can packet data
* @param packet (o) - pointer to CANpacket or NULL (just to check)
@ -116,29 +173,10 @@ static void reopen_device(){
static void processCANmessage(CANmesg *mesg){
threadinfo *ti = findThreadByID(0);
if(ti){
/*char buf[64], *ptr = buf;
int l = 64, x;
x = snprintf(ptr, l, "#0x%03X ", cm.ID);
l -= x; ptr += x;
for(int i = 0; i < cm.len; ++i){
x = snprintf(ptr, l, "0x%02X ", cm.data[i]);
l -= x; ptr += x;
}*/
mesgAddObj(&ti->answers, (void*)mesg, sizeof(CANmesg));
}
ti = findThreadByID(mesg->ID);
if(ti){
/* DBG("Found");
char buf[64], *ptr = buf;
int l = 64, x;
x = snprintf(ptr, l, "#0x%03X ", mesg->ID);
l -= x; ptr += x;
for(int i = 0; i < mesg->len; ++i){
x = snprintf(ptr, l, "0x%02X ", mesg->data[i]);
l -= x; ptr += x;
}
mesgAddText(&ti->answers, buf);
*/
mesgAddObj(&ti->answers, (void*) mesg, sizeof(CANmesg));
}
}
@ -161,7 +199,7 @@ void *CANserver(_U_ void *data){
FREE(msg);
}
usleep(1000);
CANmesg cm;
CANmesg cm = {0};
if(!canbus_read(&cm)){ // got raw message from CAN bus - parce it
DBG("Got CAN message from %d, len: %d", cm.ID, cm.len);
processCANmessage(&cm);
@ -254,7 +292,7 @@ static void sendSDO(char *mesg){
CANmesg comesg;
uint8_t datalen = (uint8_t) N - 3;
comesg.data[0] = SDO_CCS(CCS_INIT_DOWNLOAD);
comesg.data[0] = (datalen) ? SDO_CCS(CCS_INIT_DOWNLOAD) : SDO_CCS(CCS_INIT_UPLOAD); // write or read
comesg.len = 8;
if(datalen){ // there's data
comesg.data[0] |= SDO_N(datalen) | SDO_E | SDO_S;
@ -284,8 +322,8 @@ static void *canopencmds(void *arg){
if(parseSDO(ans, &sdo)){
char buf[128], *ptr = buf;
int rest = 128;
int l = snprintf(ptr, rest, "SDO={nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d",
sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen);
int l = snprintf(ptr, rest, "%s nid=0x%02X, idx=0x%04X, subidx=%d, ccs=0x%02X, datalen=%d",
ti->name, sdo.NID, sdo.index, sdo.subindex, sdo.ccs, sdo.datalen);
ptr += l; rest -= l;
if(sdo.datalen){
l = snprintf(ptr, rest, ", data=[");
@ -298,7 +336,6 @@ static void *canopencmds(void *arg){
l = snprintf(ptr, rest, "]");
ptr += l; rest -= l;
}
snprintf(ptr, rest, "}");
mesgAddText(&ServerMessages, buf);
}
FREE(ans);
@ -309,6 +346,96 @@ static void *canopencmds(void *arg){
return NULL;
}
// check incoming SDO and send data to all (thrname - thread name)
static void chkSDO(const SDO *sdo, const char *thrname){
char buf[128];
if(!sdo) return;
SDO_dic_entry *de = dictentry_search(sdo->index, sdo->subindex);
if(!de) return; // SDO not from dictionary
const abortcodes *ac = NULL;
int64_t val = getSDOval(sdo, de, &ac);
if(val == INT_MAX) // zero-length SDO - last command acknowledgement
snprintf(buf, 128, "%s %s=OK", thrname, de->varname);
else if(val == INT64_MIN) // error
snprintf(buf, 128, "%s abortcode='0x%X' error='%s'", thrname, ac->code, ac->errmsg);
else // got value
snprintf(buf, 128, "%s %s=%" PRId64, thrname, de->varname, val);
mesgAddText(&ServerMessages, buf);
}
/**
* @brief simplestp - simplest stepper motor
* @param arg - thread identifier
* @return not used
* Commands:
* maxspeed x: set maximal speed to x pulses per second
* microsteps x: set microstepping to x pulses per step
* move x: move for x pulses (+-)
* status: current position & state
* stop: stop motor
*/
static void *simplestp(void *arg){
threadinfo _U_ *ti = (threadinfo*)arg;
CANmesg can;
char buf[128];
long parameters[2];
threadCmd commands[] = {
[0] = {"test", 2, parameters, "show test values"},
[1] = {"status", 0, NULL, "get current position and status"},
[2] = {"stop", 0, NULL, "stop motor"},
[3] = {"absmove", 1, parameters, "absolute move to position x"},
{NULL, 0, NULL, NULL}
};
int NID = ti->ID & NODEID_MASK; // node ID
// prepare all
CANBUSPUSH(SDO_write(&MAXSPEED, NID, 3200, &can));
while(1){
char *mesg = mesgGetText(&ti->commands);
if(mesg) do{
DBG("Got command: %s", mesg);
int idx = cmdParser(commands, mesg);
DBG("idx = %d", idx);
if(-1 == idx){
snprintf(buf, 128, "%s wrong command '%s' or bad arguments number", ti->name, mesg);
mesgAddText(&ServerMessages, buf);
}
switch(idx){
case 0:
snprintf(buf, 128, "%s get test params: %ld, %ld", ti->name, parameters[0], parameters[1]);
mesgAddText(&ServerMessages, buf);
break;
case 1: // status, curpos
CANBUSPUSH(SDO_read(&DEVSTATUS, NID, &can));
CANBUSPUSH(SDO_read(&POSITION, NID, &can));
CANBUSPUSH(SDO_read(&ERRSTATE, NID, &can));
break;
case 2: // stop
CANBUSPUSH(SDO_write(&STOP, NID, 1, &can));
break;
case 3: // absmove
/*if(parameters[0] < 0){
CANBUSPUSH(SDO_write(&RELSTEPS, NID, parameters[0], &can));
parameters[0] = -parameters[0];
}*/
CANBUSPUSH(SDO_write(&ABSSTEPS, NID, parameters[0], &can));
break;
default:
break;
}
FREE(mesg);
}while(0);
CANmesg *ans = (CANmesg*)mesgGetObj(&ti->answers, NULL);
if(ans) do{
SDO sdo;
if(!parseSDO(ans, &sdo)) break;
chkSDO(&sdo, ti->name);
FREE(ans);
}while(0);
usleep(1000);
}
LOGERR("simplestp(): UNREACHABLE CODE REACHED!");
return NULL;
}
/**
* @brief setCANspeed - set new speed of CANbus

54
canserver/proto.c
View File

@ -32,7 +32,10 @@ static const char *ANS_OK = "OK";
static const char *ANS_WRONGCANID = "Wrong CANID";
static const char *ANS_NOTFOUND = "Thread not found";
static const char *ANS_CANTSEND = "Can't send message";
static const char *ANS_WRONGMESG = "Wrong message";
static const char *shelp(_U_ char *par1, _U_ char *par2);
static const char *sthrds(_U_ char *par1, _U_ char *par2);
static const char *listthr(_U_ char *par1, _U_ char *par2);
static const char *regthr(char *thrname, char *data);
static const char *unregthr(char *thrname, char *data);
@ -49,20 +52,47 @@ static const char *setspd(char *speed, _U_ char *data);
* you can get full list of functions by function `help`
*/
typedef struct{
const char *fname; // function name
const char *fname; // function name
const char *(*handler)(char *arg1, char *arg2); // data handler (arg1 and arg2 could be changed)
const char *helpmesg; // help message
} cmditem;
// array with known functions
static cmditem functions[] = {
{"list", listthr}, // list threads
{"mesg", sendmsg}, // "mesg NAME ID [data]"
{"register", regthr}, // "register NAME ID", ID - RAW CAN ID (not canopen ID)!!!
{"speed", setspd}, // set CANbus speed
{"unregister", unregthr}, // "unregister NAME"
{NULL, NULL}
{"help", shelp, "- show help"},
{"list", listthr, "- list all threads"},
{"mesg", sendmsg, "NAME MESG - send message `MESG` to thread `NAME`"},
{"register", regthr, "NAME ID ROLE - register new thread with `NAME`, raw receiving `ID` running thread `ROLE`"},
{"speed", setspd, "SPD - set CANbus speed to `SPD`"},
{"threads", sthrds, "- list all possible threads with their message format"},
{"unregister", unregthr, "NAME - kill thread `NAME`"},
{NULL, NULL, NULL}
};
// show help
static const char *shelp(_U_ char *par1, _U_ char *par2){
char buf[128];
cmditem *ptr = functions;
while(ptr->fname){
snprintf(buf, 128, "help> %s %s", ptr->fname, ptr->helpmesg);
mesgAddText(&ServerMessages, buf);
++ptr;
}
return NULL;
}
// show all possible threads
static const char *sthrds(_U_ char *par1, _U_ char *par2){
char buf[128];
thread_handler *h = CANhandlers;
while(h->name){
snprintf(buf, 128, "thread> %s %s", h->name, h->helpmesg);
mesgAddText(&ServerMessages, buf);
++h;
}
return NULL;
}
// list all threads
static const char *listthr(_U_ char *par1, _U_ char *par2){
FNAME();
@ -72,10 +102,11 @@ static const char *listthr(_U_ char *par1, _U_ char *par2){
do{
list = nextThread(list);
if(!list) break;
snprintf(msg, 256, "thread name='%s' role='%s' ID=0x%X", list->ti.name, list->ti.handler.name, list->ti.ID);
snprintf(msg, 256, "thread> name='%s' role='%s' ID=0x%X", list->ti.name, list->ti.handler.name, list->ti.ID);
mesgAddText(&ServerMessages, msg);
empty = 0;
}while(1);
mesgAddText(&ServerMessages, "thread> Send message 'list' to thread marked with (list) to get commands list");
if(empty) return "No threads";
return NULL;
}
@ -130,6 +161,13 @@ static const char *unregthr(char *thrname, _U_ char *data){
*/
static const char *sendmsg(char *thrname, char *data){
FNAME();
if(!data || strlen(data) == 0) return ANS_WRONGMESG;
char *ptr = data, c = 0;
while(*ptr){ // check that message not empty
c = *ptr++;
if(c > ' ') break;
}
if(c <= ' ') return ANS_WRONGMESG;
threadinfo *ti = findThreadByName(thrname);
if(!ti) return ANS_NOTFOUND;
if(!mesgAddText(&ti->commands, data)) return ANS_CANTSEND;

6
canserver/pusirobot.c
View File

@ -22,14 +22,12 @@
// we should init constants here!
#undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s, n) const SDO_dic_entry name = {idx, sidx, sz, s, n};
#define DICENTRY(name, idx, sidx, sz, s, n, v) const SDO_dic_entry name = {idx, sidx, sz, s, n, v};
#include "dicentries.in"
// now init array with all dictionary
#undef DICENTRY
#define nnn(nm) nm
#define lnk(nm) & ## nnn(nm)
#define DICENTRY(name, idx, sidx, sz, s, n) &name,
#define DICENTRY(name, idx, sidx, sz, s, n, v) &name,
const SDO_dic_entry* allrecords[] = {
#include "dicentries.in"
};

6
canserver/pusirobot.h
View File

@ -28,11 +28,11 @@ typedef struct{
uint8_t datasize; // data size: 1,2,3 or 4 bytes
uint8_t issigned; // signess: if issigned==1, then signed, else unsigned
const char *name; // dictionary entry name
const char *varname;// variable name for output
} SDO_dic_entry;
#ifndef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s, n) extern const SDO_dic_entry name;
#endif
#undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s, n, v) extern const SDO_dic_entry name;
#include "dicentries.in"

6
canserver/socket.c
View File

@ -122,8 +122,10 @@ static void *server(void *asock){
close(fd);
DBG("Client with fd %d closed", fd);
LOGMSG("Client %d disconnected", fd);
for(int i = fdidx; i < nfd; ++i)
poll_set[i] = poll_set[i + 1];
// move last to free space
poll_set[fdidx] = poll_set[nfd - 1];
//for(int i = fdidx; i < nfd-1; ++i)
// poll_set[i] = poll_set[i + 1];
--nfd;
}
}else{ // server

2
canserver/threadlist.c
View File

@ -146,7 +146,7 @@ void *mesgAddObj(message *msg, void *data, size_t size){
*/
char *mesgAddText(message *msg, char *txt){
if(!txt) return NULL;
DBG("mesg add text '%s'", txt);
DBG("mesgAddText(%s)", txt);
size_t l = strlen(txt) + 1;
return mesgAddObj(msg, (void*)txt, l);
}

1
canserver/threadlist.h
View File

@ -42,6 +42,7 @@ typedef struct{
typedef struct{
const char *name; // handler name
void *(*handler)(void *); // handler function
const char *helpmesg; // help message
} thread_handler;
// thread information