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

Initial commit

Browse Source
This commit is contained in:
eddyem 2020-05-04 18:35:15 +03:00
parent 99f37b9c17
commit dddd80a613
13 changed files with 1122 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
.gitignore vendored Normal file
View File

@ -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

3
Readme.md Normal file
View File

@ -0,0 +1,3 @@
commandline - Command line motor management

69
commandline/CMakeLists.txt Normal file
View File

@ -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")

5
commandline/Readme.md Normal file
View File

@ -0,0 +1,5 @@
Command line motor management
=============================
CAN controller: my CAN-USB sniffer

255
commandline/canbus.c Normal file
View File

@ -0,0 +1,255 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <pthread.h>
#include <stdio.h>
#include <string.h>
#include <sys/select.h>
#include <usefull_macros.h>
#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;
}

47
commandline/canbus.h Normal file
View File

@ -0,0 +1,47 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef CANBUS_H__
#define CANBUS_H__
#include <stdint.h>
#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__

273
commandline/canopen.c Normal file
View File

@ -0,0 +1,273 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usefull_macros.h>
#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

87
commandline/canopen.h Normal file
View File

@ -0,0 +1,87 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#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__

96
commandline/cmdlnopts.c Normal file
View File

@ -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 <eddy@sao.ru>
*
* 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 <assert.h>
#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <usefull_macros.h>
#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;
}

48
commandline/cmdlnopts.h Normal file
View File

@ -0,0 +1,48 @@
/* geany_encoding=koi8-r
* cmdlnopts.h - comand line options for parceargs
*
* Copyright 2013 Edward V. Emelianoff <eddy@sao.ru>
*
* 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__

106
commandline/main.c Normal file
View File

@ -0,0 +1,106 @@
/*
* This file is part of the usefull_macros project.
* Copyright 2018 Edward V. Emelianoff <eddy@sao.ru>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <usefull_macros.h>
#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;
}

33
commandline/pusirobot.c Normal file
View File

@ -0,0 +1,33 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
//#include <limits.h>
// 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;
}
*/

77
commandline/pusirobot.h Normal file
View File

@ -0,0 +1,77 @@
/*
* This file is part of the stepper project.
* Copyright 2020 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef PUSIROBOT_H__
#define PUSIROBOT_H__
#include <stdint.h>
// 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__