mirror of
https://github.com/eddyem/pusirobot.git
synced 2025-12-06 02:25:10 +03:00
Initial commit
This commit is contained in:
eddyem
parent
99f37b9c17
commit
dddd80a613
23
.gitignore
vendored
Normal file
23
.gitignore
vendored
Normal 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
|
||||||
69
commandline/CMakeLists.txt
Normal file
69
commandline/CMakeLists.txt
Normal 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
5
commandline/Readme.md
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
Command line motor management
|
||||||
|
=============================
|
||||||
|
|
||||||
|
CAN controller: my CAN-USB sniffer
|
||||||
|
|
||||||
255
commandline/canbus.c
Normal file
255
commandline/canbus.c
Normal 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
47
commandline/canbus.h
Normal 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
273
commandline/canopen.c
Normal 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
87
commandline/canopen.h
Normal 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
96
commandline/cmdlnopts.c
Normal 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
48
commandline/cmdlnopts.h
Normal 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
106
commandline/main.c
Normal 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
33
commandline/pusirobot.c
Normal 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
77
commandline/pusirobot.h
Normal 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__
|
||||||
Loading…
x
Reference in New Issue
Block a user