From dddd80a613a404ca15578adf449bfbc202212c2d Mon Sep 17 00:00:00 2001 From: eddyem Date: Mon, 4 May 2020 18:35:15 +0300 Subject: [PATCH] Initial commit --- .gitignore | 23 ++++ Readme.md | 3 + commandline/CMakeLists.txt | 69 ++++++++++ commandline/Readme.md | 5 + commandline/canbus.c | 255 ++++++++++++++++++++++++++++++++++ commandline/canbus.h | 47 +++++++ commandline/canopen.c | 273 +++++++++++++++++++++++++++++++++++++ commandline/canopen.h | 87 ++++++++++++ commandline/cmdlnopts.c | 96 +++++++++++++ commandline/cmdlnopts.h | 48 +++++++ commandline/main.c | 106 ++++++++++++++ commandline/pusirobot.c | 33 +++++ commandline/pusirobot.h | 77 +++++++++++ 13 files changed, 1122 insertions(+) create mode 100644 .gitignore create mode 100644 Readme.md create mode 100644 commandline/CMakeLists.txt create mode 100644 commandline/Readme.md create mode 100644 commandline/canbus.c create mode 100644 commandline/canbus.h create mode 100644 commandline/canopen.c create mode 100644 commandline/canopen.h create mode 100644 commandline/cmdlnopts.c create mode 100644 commandline/cmdlnopts.h create mode 100644 commandline/main.c create mode 100644 commandline/pusirobot.c create mode 100644 commandline/pusirobot.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..60663b5 --- /dev/null +++ b/.gitignore @@ -0,0 +1,23 @@ +# Prerequisites +*.d + +# Object files +*.o + +# Libraries +*.lib +*.a +*.la +*.lo + +# Shared objects (inc. Windows DLLs) +*.so +*.so.* + +# qt-creator +*.config +*.cflags +*.cxxflags +*.creator* +*.files +*.includes diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..e85cb7b --- /dev/null +++ b/Readme.md @@ -0,0 +1,3 @@ +commandline - Command line motor management + + diff --git a/commandline/CMakeLists.txt b/commandline/CMakeLists.txt new file mode 100644 index 0000000..c356a80 --- /dev/null +++ b/commandline/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.0) +set(PROJ steppermove) +set(MINOR_VERSION "1") +set(MID_VERSION "0") +set(MAJOR_VERSION "0") +set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}") + +project(${PROJ} VERSION ${PROJ_VERSION} LANGUAGES C) +#enable_language(C) + +message("VER: ${VERSION}") + +# default flags +set(CMAKE_C_FLAGS_RELEASE "") +set(CMAKE_C_FLAGS_DEBUG "") +set(CMAKE_C_FLAGS "-O2 -std=gnu99") + +set(CMAKE_COLOR_MAKEFILE ON) + +# here is one of two variants: all .c in directory or .c files in list +aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES) + +# cmake -DDEBUG=1 -> debugging +if(DEFINED EBUG) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Wall -Werror -W") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wextra -Wall -Werror -W") + set(CMAKE_BUILD_TYPE DEBUG) + set(CMAKE_VERBOSE_MAKEFILE "ON") + add_definitions(-DEBUG) +else() + set(CMAKE_BUILD_TYPE RELEASE) +endif() + +###### pkgconfig ###### +# pkg-config modules (for pkg-check-modules) +set(MODULES usefull_macros) + +# find packages: +find_package(PkgConfig REQUIRED) +pkg_check_modules(${PROJ} REQUIRED ${MODULES}) + +###### additional flags ###### +#list(APPEND ${PROJ}_LIBRARIES "-lfftw3_threads") + +# change wrong behaviour with install prefix +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT AND CMAKE_INSTALL_PREFIX MATCHES "/usr/local") +else() + message("Change default install path to /usr/local") + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +message("Install dir prefix: ${CMAKE_INSTALL_PREFIX}") + +# exe file +add_executable(${PROJ} ${SOURCES}) +# -I +include_directories(${${PROJ}_INCLUDE_DIRS}) +# -L +link_directories(${${PROJ}_LIBRARY_DIRS}) +# -D +add_definitions(${CFLAGS} -DLOCALEDIR=\"${LOCALEDIR}\" + -DPACKAGE_VERSION=\"${VERSION}\" -DGETTEXT_PACKAGE=\"${PROJ}\" + -DMINOR_VERSION=\"${MINOR_VERSION}\" -DMID_VERSION=\"${MID_VERSION}\" + -DMAJOR_VERSION=\"${MAJOR_VESION}\") + +# -l +target_link_libraries(${PROJ} ${${PROJ}_LIBRARIES} -lm) + +# Installation of the program +INSTALL(TARGETS ${PROJ} DESTINATION "bin") diff --git a/commandline/Readme.md b/commandline/Readme.md new file mode 100644 index 0000000..2c4a9c7 --- /dev/null +++ b/commandline/Readme.md @@ -0,0 +1,5 @@ +Command line motor management +============================= + +CAN controller: my CAN-USB sniffer + diff --git a/commandline/canbus.c b/commandline/canbus.c new file mode 100644 index 0000000..a96ae98 --- /dev/null +++ b/commandline/canbus.c @@ -0,0 +1,255 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include + +#include "canbus.h" + +#ifndef BUFLEN +#define BUFLEN 80 +#endif + +#ifndef WAIT_TMOUT +#define WAIT_TMOUT 0.01 +#endif + +/* +This file should provide next functions: + int canbus_open(const char *devname) - calls @the beginning, return 0 if all OK + int canbus_setspeed(int speed) - set given speed (in Kbaud) @ CAN bus (return 0 if all OK) + void canbus_close() - calls @the end + int canbus_write(CANmesg *mesg) - write `data` with length `len` to ID `ID`, return 0 if all OK + int canbus_read(CANmesg *mesg) - blocking read (broadcast if ID==0 or only from given ID) from can bus, return 0 if all OK +*/ + +static TTY_descr *dev = NULL; // shoul be global to restore if die +static int serialspeed = 115200; // speed to open serial device +static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; + +static char *read_string(); + +/** + * @brief read_ttyX- read data from TTY with 10ms timeout WITH disconnect detection + * @param buff (o) - buffer for data read + * @param length - buffer len + * @return amount of bytes read + */ +static int read_ttyX(TTY_descr *d){ + if(d->comfd < 0) return 0; + size_t L = 0; + ssize_t l; + size_t length = d->bufsz; + char *ptr = d->buf; + fd_set rfds; + struct timeval tv; + int retval; + do{ + l = 0; + FD_ZERO(&rfds); + FD_SET(d->comfd, &rfds); + // wait for 10ms + tv.tv_sec = 0; tv.tv_usec = 50000; + retval = select(d->comfd + 1, &rfds, NULL, NULL, &tv); + if (!retval) break; + if(FD_ISSET(d->comfd, &rfds)){ + if((l = read(d->comfd, ptr, length)) < 1){ + return -1; // disconnect or other error - close TTY & die + } + ptr += l; L += l; + length -= l; + } + }while(l && length); + d->buflen = L; + d->buf[L] = 0; + return (size_t)L; +} + + +// thread-safe writing, add trailing '\n' +static int ttyWR(const char *buff, int len){ + pthread_mutex_lock(&mutex); + DBG("Write 2tty %d bytes: ", len); +#ifdef EBUG + int _U_ n = write(STDERR_FILENO, buff, len); + fprintf(stderr, "\n"); +#endif + int w = write_tty(dev->comfd, buff, (size_t)len); + if(!w) w = write_tty(dev->comfd, "\n", 1); + char *s = read_string(); // clear echo + if(!s || strcmp(s, buff) != 0){ + WARNX("wrong answer! Got '%s' instead of '%s'", s, buff); + return 1; + } + pthread_mutex_unlock(&mutex); + return w; +} + +void canbus_close(){ + if(dev) close_tty(&dev); +} + +void setserialspeed(int speed){ + serialspeed = speed; +} + +int canbus_open(const char *devname){ + if(!devname){ + WARNX("canbus_open(): need device name"); + return 1; + } + if(dev) close_tty(&dev); + dev = new_tty((char*)devname, serialspeed, BUFLEN); + if(dev){ + if(!tty_open(dev, 1)) // blocking open + close_tty(&dev); + } + if(!dev){ + return 1; + } + return 0; +} + +int canbus_setspeed(int speed){ + if(speed == 0) return 0; // default - not change + char buff[BUFLEN]; + if(speed<10 || speed>3000){ + WARNX("Wrong CAN bus speed value: %d", speed); + return 1; + } + int len = snprintf(buff, BUFLEN, "b %d", speed); + if(len < 1) return 2; + while(read_string()); // clear buffer + return ttyWR(buff, len); +} + +int canbus_write(CANmesg *mesg){ + char buf[BUFLEN]; + if(!mesg || mesg->len > 8) return 1; + int rem = BUFLEN, len = 0; + int l = snprintf(buf, rem, "s %d", mesg->ID); + rem -= l; len += l; + for(uint8_t i = 0; i < mesg->len; ++i){ + l = snprintf(&buf[len], rem, " %d", mesg->data[i]); + rem -= l; len += l; + if(rem < 0) return 2; + } + while(read_string()); // clear buffer + return ttyWR(buf, len); +} + +/** + * read strings from terminal (ending with '\n') with timeout + * @return NULL if nothing was read or pointer to static buffer + */ +static char *read_string(){ + static char buf[1024]; + int LL = 1023, r = 0, l; + char *ptr = NULL; + static char *optr = NULL; + if(optr && *optr){ + ptr = optr; + optr = strchr(optr, '\n'); + if(optr){ + *optr = 0; + ++optr; + } + return ptr; + } + ptr = buf; + double d0 = dtime(); + do{ + if((l = read_ttyX(dev))){ + if(l < 0){ + ERR("tty disconnected"); + } + if(l > LL){ // buffer overflow + WARNX("read_string(): buffer overflow"); + optr = NULL; + return NULL; + } + memcpy(ptr, dev->buf, dev->buflen); + r += l; LL -= l; ptr += l; + if(ptr[-1] == '\n') break; + d0 = dtime(); + } + }while(dtime() - d0 < WAIT_TMOUT && LL); + if(r){ + buf[r] = 0; + optr = strchr(buf, '\n'); + if(optr){ + *optr = 0; + ++optr; + }else{ + WARNX("read_string(): no newline found"); + DBG("buf: %s", buf); + optr = NULL; + return NULL; + } + DBG("buf: %s", buf); + return buf; + } + return NULL; +} + +CANmesg *parseCANmesg(const char *str){ + static CANmesg m; + int l = sscanf(str, "%d #0x%hx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx 0x%hhx", &m.timemark, &m.ID, + &m.data[0], &m.data[1], &m.data[2], &m.data[3], &m.data[4], &m.data[5], &m.data[6], &m.data[7]); + if(l < 2) return NULL; + m.len = l - 2; + return &m; +} + +void showM(CANmesg *m){ + printf("TS=%d, ID=0x%X", m->timemark, m->ID); + int l = m->len; + if(l) printf(", data="); + for(int i = 0; i < l; ++i) printf(" 0x%02X", m->data[i]); + printf("\n"); +} + +int canbus_read(CANmesg *mesg){ + if(!mesg) return 1; + pthread_mutex_lock(&mutex); + double t0 = dtime(); + int ID = mesg->ID; + char *ans; + 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); + if(ID && m->ID == ID){ + memcpy(mesg, m, sizeof(CANmesg)); + DBG("All OK"); + pthread_mutex_unlock(&mutex); + return 0; + } + } + } + } + pthread_mutex_unlock(&mutex); + return 1; +} + diff --git a/commandline/canbus.h b/commandline/canbus.h new file mode 100644 index 0000000..f390cdc --- /dev/null +++ b/commandline/canbus.h @@ -0,0 +1,47 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef CANBUS_H__ +#define CANBUS_H__ + +#include + +#ifndef T_POLLING_TMOUT +#define T_POLLING_TMOUT (0.5) +#endif + +typedef struct{ + uint32_t timemark; // time since MCU run (ms) + uint16_t ID; // 11-bit identifier + uint8_t data[8]; // up to 8 bit data, data[0] is lowest + uint8_t len; // data length +} CANmesg; + +// main (necessary) functions of canbus.c: +void canbus_close(); +int canbus_open(const char *devname); +int canbus_write(CANmesg *mesg); +int canbus_read(CANmesg *mesg); +int canbus_setspeed(int speed); + +// auxiliary (not necessary) functions +void setserialspeed(int speed); +void showM(CANmesg *m); +CANmesg *parseCANmesg(const char *str); + +#endif // CANBUS_H__ diff --git a/commandline/canopen.c b/commandline/canopen.c new file mode 100644 index 0000000..e2d9eff --- /dev/null +++ b/commandline/canopen.c @@ -0,0 +1,273 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include + +#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"}, + {0x05040000, "SDO protocol timed out"}, + {0x05040001, "Client/server command specifier not valid or unknown"}, + {0x05040002, "Invalid block size (block mode only)"}, + {0x05040003, "Invalid sequence number (block mode only)"}, + {0x05040004, "CRC error (block mode only)"}, + {0x05040005, "Out of memory"}, + {0x06010000, "Unsupported access to an object"}, + {0x06010001, "Attempt to read a write only object"}, + {0x06010002, "Attempt to write a read only object"}, + {0x06020000, "Object does not exist in the object dictionary"}, + {0x06040041, "Object cannot be mapped to the PDO"}, + {0x06040042, "The number and length of the objects to be mapped would exceed PDO length"}, + {0x06040043, "General parameter incompatibility reason"}, + {0x06040047, "General internal incompatibility in the device"}, + {0x06060000, "Access failed due to a hardware error"}, + {0x06070010, "Data type does not match; length of service parameter does not match"}, + {0x06070012, "Data type does not match; length of service parameter too high"}, + {0x06070013, "Data type does not match; length of service parameter too low"}, + {0x06090011, "Sub-index does not exist"}, + {0x06090030, "Value range of parameter exceeded (only for write access)"}, + {0x06090031, "Value of parameter written too high"}, + {0x06090032, "Value of parameter written too low"}, + {0x06090036, "Maximum value is less than minimum value"}, + {0x08000000, "General error"}, + {0x08000020, "Data cannot be transferred or stored to the application"}, + {0x08000021, "Data cannot be transferred or stored to the application because of local control"}, + {0x08000022, "Data cannot be transferred or stored to the application because of the present device state"}, + {0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present"}, +}; + +static const int ACmax = sizeof(AC)/sizeof(abortcodes) - 1; + +/** + * @brief abortcode_text - explanation of abort code + * @param abortcode - code + * @return text for error or NULL + */ +const char *abortcode_text(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; + return AC[idx].errmsg; + }else if(c > abortcode){ + newidx = (idx + min_)/2; + max_ = idx; + + }else{ + newidx = (idx + max_ + 1)/2; + 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); + 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[1] = sdo->index & 0xff; // l + mesg.data[2] = (sdo->index >> 8) & 0xff; // h + mesg.data[3] = sdo->subindex; +#ifdef EBUG + FNAME(); + green("Make message to 0x%X: ", mesg.ID); + for(uint8_t i = 0; i < 8; ++i) printf("0x%02X ", mesg.data[i]); + printf("\n"); +#endif + return &mesg; +} + +// transform CAN-message to SDO +SDO *parseSDO(CANmesg *mesg){ + static SDO sdo; + if(mesg->len != 8){ + WARNX("Wrong SDO data length"); + return NULL; + } + uint16_t cobid = mesg->ID & COBID_MASK; + if(cobid != TSDO_COBID){ + DBG("cobid=0x%X, not a TSDO!", cobid); + return NULL; // not a transmit SDO + } + sdo.NID = mesg->ID & NODEID_MASK; + uint8_t spec = mesg->data[0]; + sdo.ccs = GET_CCS(spec); + sdo.index = (uint16_t)mesg->data[1] | ((uint16_t)mesg->data[2] << 8); + sdo.subindex = mesg->data[3]; + 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]; + 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; +} + +// send request to read SDO +static int ask2read(uint16_t idx, uint8_t subidx, uint8_t NID){ + SDO sdo; + sdo.NID = NID; + sdo.ccs = CCS_INIT_UPLOAD; + sdo.datalen = 0; + sdo.index = idx; + sdo.subindex = subidx; + CANmesg *mesg = mkMesg(&sdo); + return canbus_write(mesg); +} + +/** + * @brief readSDOvalue - send request to SDO read + * @param idx - SDO index + * @param subidx - SDO subindex + * @param NID - target node ID + * @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; +} + +static inline uint32_t mku32(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]){ + return (uint16_t)(data[0] | (data[1]<<8)); +} + +static inline uint8_t mku8(uint8_t data[4]){ + return data[0]; +} + +static inline int32_t mki32(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]){ + return (int16_t)(data[0] | (data[1]<<8)); +} + +static inline int8_t mki8(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){ + SDO *sdo = readSDOvalue(e->index, e->subindex, NID); + if(!sdo){ + WARNX("SDO read error"); + return INT64_MIN; + } + if(sdo->datalen == 0){ + 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); + return INT64_MIN; + } + 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){ + case 1: + ans = mki8(sdo->data); + break; + case 4: + ans = mki32(sdo->data); + break; + default: // can't be 3! 3->2 + ans = mki16(sdo->data); + } + }else{ + switch(sdo->datalen){ + case 1: + ans = mku8(sdo->data); + break; + case 4: + ans = mku32(sdo->data); + break; + default: // can't be 3! 3->2 + ans = mku16(sdo->data); + } + } + return ans; +} + +// 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 diff --git a/commandline/canopen.h b/commandline/canopen.h new file mode 100644 index 0000000..df4e8fc --- /dev/null +++ b/commandline/canopen.h @@ -0,0 +1,87 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#ifndef CANOPEN_H__ +#define CANOPEN_H__ + +#include "canbus.h" +#include "pusirobot.h" + +// timeout for answer from the SDO, seconds +#define SDO_ANS_TIMEOUT (1.) + +// COB-ID base: +#define NMT_COBID 0 +#define EMERG_COBID 0x80 +#define TIMESTAMP_COBID 0x100 +#define TPDO1_COBID 0x180 +#define RPDO1_COBID 0x200 +#define TPDO2_COBID 0x280 +#define RPDO2_COBID 0x300 +#define TPDO3_COBID 0x380 +#define RPDO3_COBID 0x400 +#define TPDO4_COBID 0x480 +#define RPDO4_COBID 0x500 +#define TSDO_COBID 0x580 +#define RSDO_COBID 0x600 +#define HEARTB_COBID 0x700 +// mask to select COB-ID base from ID +#define COBID_MASK 0x780 +// mask to select node ID from ID +#define NODEID_MASK 0x7F + +// SDO client command specifier field +typedef enum{ + CCS_SEG_DOWNLOAD = 0, + CCS_INIT_DOWNLOAD, + CCS_INIT_UPLOAD, + CCS_SEG_UPLOAD, + CCS_ABORT_TRANSFER, + CCS_BLOCK_UPLOAD, + CCS_BLOCK_DOWNLOAD +} SDO_CCS_fields; +// make number from CSS field +#define SDO_CCS(f) (f<<5) +// make CCS from number +#define GET_CCS(f) (f>>5) +// make number from data amount +#define SDO_N(n) ((4-n)<<2) +// make data amount from number +#define SDO_datalen(n) (4-((n&0xC)>>2)) +// SDO e & s fields: +#define SDO_E (1<<1) +#define SDO_S (1<<0) + +typedef struct{ + uint8_t NID; // node ID in CANopen + uint16_t index; // SDO index + uint8_t subindex; // SDO subindex + uint8_t data[4]; // 1..4 bytes of data (data[0] is lowest) + uint8_t datalen; // length of data + uint8_t ccs; // Client command specifier +} SDO; + +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); + +#endif // CANOPEN_H__ diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c new file mode 100644 index 0000000..cb4c2af --- /dev/null +++ b/commandline/cmdlnopts.c @@ -0,0 +1,96 @@ +/* geany_encoding=koi8-r + * cmdlnopts.c - the only function that parse cmdln args and returns glob parameters + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#include +#include +#include +#include +#include + +#include "cmdlnopts.h" + +/* + * here are global parameters initialisation + */ +static int help; +static glob_pars G; + +// default PID filename: +#define DEFAULT_PIDFILE "/tmp/steppersmng.pid" +#define DEFAULT_PORTDEV "/dev/ttyUSB0" +#define DEFAULT_SER_SPEED 115200 +// DEFAULTS +// default global parameters +static glob_pars const Gdefault = { + .device = DEFAULT_PORTDEV, + .pidfile = DEFAULT_PIDFILE, + .serialspeed = DEFAULT_SER_SPEED, + .canspeed = 0, // don't set + .logfile = NULL, // don't save logs + .NodeID = 1, // default node ID = 1 + .absmove = INT_MIN, + .relmove = INT_MIN +}; + +/* + * Define command line options by filling structure: + * name has_arg flag val type argptr help +*/ +static myoption cmdlnopts[] = { +// common options + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")}, + {"device", NEED_ARG, NULL, 'd', arg_string, APTR(&G.device), _("serial device name (default: " DEFAULT_PORTDEV ")")}, + {"canspd", NEED_ARG, NULL, 's', arg_int, APTR(&G.canspeed), _("CAN bus speed (default: " STR(DEFAULT_SPEED) ")")}, + {"serialspd",NEED_ARG, NULL, 't', arg_int, APTR(&G.serialspeed),_("serial (tty) device speed (default: " STR(DEFAULT_SPEED) ")")}, + {"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")}, + end_option +}; + +/** + * Parse command line options and return dynamically allocated structure + * to global parameters + * @param argc - copy of argc from main + * @param argv - copy of argv from main + * @return allocated structure with global parameters + */ +glob_pars *parse_args(int argc, char **argv){ + int i; + void *ptr; + ptr = memcpy(&G, &Gdefault, sizeof(G)); assert(ptr); + size_t hlen = 1024; + char helpstring[1024], *hptr = helpstring; + snprintf(hptr, hlen, "Usage: %%s [args]\n\n\tWhere args are:\n"); + // format of help: "Usage: progname [args]\n" + change_helpstring(helpstring); + // parse arguments + parseargs(&argc, &argv, cmdlnopts); + if(help) showhelp(-1, cmdlnopts); + if(argc > 0){ + WARNX("Extra arguments: "); + for (i = 0; i < argc; i++) fprintf(stderr, "\t%s\n", argv[i]); + showhelp(-1, cmdlnopts); + } + return &G; +} + diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h new file mode 100644 index 0000000..e33a516 --- /dev/null +++ b/commandline/cmdlnopts.h @@ -0,0 +1,48 @@ +/* geany_encoding=koi8-r + * cmdlnopts.h - comand line options for parceargs + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#pragma once +#ifndef CMDLNOPTS_H__ +#define CMDLNOPTS_H__ + +#ifndef STR +#define STR(x) _S_(x) +#define _S_(x) #x +#endif + +/* + * here are some typedef's for global data + */ +typedef struct{ + char *device; // serial device name + char *pidfile; // name of PID file + char *logfile; // logging to this file + int canspeed; // CAN bus speed + int serialspeed; // serial device speed (CAN-bus to USB) + int NodeID; // node ID to work with + int absmove; // absolute position to move + int relmove; // relative position to move +} glob_pars; + + +glob_pars *parse_args(int argc, char **argv); + +#endif // CMDLNOPTS_H__ diff --git a/commandline/main.c b/commandline/main.c new file mode 100644 index 0000000..c9084e3 --- /dev/null +++ b/commandline/main.c @@ -0,0 +1,106 @@ +/* + * This file is part of the usefull_macros project. + * Copyright 2018 Edward V. Emelianoff . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include + +#include "canbus.h" +#include "canopen.h" +#include "cmdlnopts.h" +#include "pusirobot.h" + +static glob_pars *GP = NULL; // for GP->pidfile need in `signals` + +void signals(int sig){ + putlog("Exit with status %d", sig); + restore_console(); + if(GP->pidfile) // remove unnesessary PID file + unlink(GP->pidfile); + canbus_close(); + exit(sig); +} + +void iffound_default(pid_t pid){ + ERRX("Another copy of this process found, pid=%d. Exit.", pid); +} + +int main(int argc, char *argv[]){ + initial_setup(); + char *self = strdup(argv[0]); + GP = parse_args(argc, argv); + check4running(self, GP->pidfile); + free(self); + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + 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"); + } + if(GP->logfile) openlogfile(GP->logfile); + putlog(("Start application...")); + putlog("Try to open CAN bus device %s", GP->device); + setserialspeed(GP->serialspeed); + if(canbus_open(GP->device)){ + putlog("Can't open %s @ speed %d. Exit.", GP->device, GP->serialspeed); + signals(1); + } + if(canbus_setspeed(GP->canspeed)){ + putlog("Can't set CAN speed %d. Exit.", GP->canspeed); + signals(2); + } + + //setup_con(); + // print current position and state + int64_t i64; + if(INT64_MIN != (i64 = SDO_read(&DEVSTATUS, GP->NodeID))) + 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); + +#if 0 + CANmesg m; + double t = dtime() - 10.; + while(1){ + m.ID = 0; // read all + if(!canbus_read(&m)){ + showM(&m); + SDO *x = parseSDO(&m); + if(x){ + printf("Get SDO, NID=%d, CCS=%d, idx=%d, subidx=%d, datalen=%d\n", x->NID, x->ccs, x->index, x->subindex, x->datalen); + } + } + if(dtime() - t > 5.){ + t = dtime(); + green("Try to get status.. "); + uint8_t s; + if(SDO_readByte(0x6001, 0, &s, 1)) red("Err\n"); + else printf("got: 0x%X\n", s); + } + } +#endif + signals(0); + return 0; +} diff --git a/commandline/pusirobot.c b/commandline/pusirobot.c new file mode 100644 index 0000000..acbf556 --- /dev/null +++ b/commandline/pusirobot.c @@ -0,0 +1,33 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +//#include + +// we should init constants here! +#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s}; +#include "pusirobot.h" + +/* +// get current position for node ID `NID`, @return INT_MIN if error +int get_current_position(uint8_t NID){ + int64_t val = SDO_read(&POSITION, NID); + if(val == INT64_MIN) return INT_MIN; + return (int) val; +} +*/ + diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h new file mode 100644 index 0000000..4822b99 --- /dev/null +++ b/commandline/pusirobot.h @@ -0,0 +1,77 @@ +/* + * This file is part of the stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef PUSIROBOT_H__ +#define PUSIROBOT_H__ + +#include + +// entry of SDO dictionary +typedef struct{ + uint16_t index; // SDO index + uint8_t subindex; // SDO subindex + uint8_t datasize; // data size: 1,2,3 or 4 bytes + uint8_t issigned; // signess: if issigned==1, then signed, else unsigned +} SDO_dic_entry; + +#ifndef DICENTRY +#define DICENTRY(name, idx, sidx, sz, s) extern const SDO_dic_entry name; +#endif + +// heartbeat time +DICENTRY(HEARTBTTIME, 0x1017, 0, 2, 0) +// node ID +DICENTRY(NODEID, 0x2002, 0, 1, 0) +// baudrate +DICENTRY(BAUDRATE, 0x2003, 0, 1, 0) +// system control: 1- bootloader, 2 - save parameters, 3 - reset factory settings +DICENTRY(SYSCONTROL, 0x2007, 0, 1, 0) +// error state +DICENTRY(ERRSTATE, 0x6000, 0, 1, 0) +// controller status +DICENTRY(DEVSTATUS, 0x6001, 0, 1, 0) +// rotation direction +DICENTRY(ROTDIR, 0x6002, 0, 1, 0) +// maximal speed +DICENTRY(MAXSPEED, 0x6003, 0, 4, 1) +// relative displacement +DICENTRY(RELSTEPS, 0x6004, 0, 4, 0) +// operation mode +DICENTRY(OPMODE, 0x6005, 0, 1, 0) +// start speed +DICENTRY(STARTSPEED, 0x6006, 0, 2, 0) +// stop speed +DICENTRY(STOPSPEED, 0x6007, 0, 2, 0) +// acceleration coefficient +DICENTRY(ACCELCOEF, 0x6008, 0, 1, 0) +// deceleration coefficient +DICENTRY(DECELCOEF, 0x6009, 0, 1, 0) +// microstepping +DICENTRY(MICROSTEPS, 0x600A, 0, 2, 0) +// max current +DICENTRY(MAXCURNT, 0x600B, 0, 2, 0) +// current position +DICENTRY(POSITION, 0x600C, 0, 4, 0) +// motor enable +DICENTRY(ENABLE, 0x600E, 0, 1, 0) + + + +//int get_current_position(uint8_t NID); + +#endif // PUSIROBOT_H__