eddy/pusirobot
1
0
Fork 0
mirror of https://github.com/eddyem/pusirobot.git synced 2025-12-06 02:25:10 +03:00
Code Issues Packages Projects Releases Wiki Activity

Add config-file parser (read SDOs from file and send them to driver)

Browse Source
This commit is contained in:
eddyem 2020-05-07 01:34:11 +03:00
parent 4efa87448d
commit ef21a5f826
10 changed files with 294 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
commandline/Start_settings.cfg Normal file
View File

@ -0,0 +1,24 @@
# Transmit SDO to driver
# Format: index, subindex, data
# Set Node ID to 1
0x2002, 0, 1
# Set microstepping to 32
0x600A, 0, 32
# Set max phase current to 600mA
0x600B, 0, 600
# Set acceleration (0 - no, 1 - highest, 8 - lowest)
0x6008, 0, 4
# Set deceleration
0x6009, 0, 5
# EXT1 emergency stop
# Rising edge trigger
0x600F, 2, 1
# Turn on pullup
0x600F, 3, 1
# Enable EXT1
0x600F, 1, 1
# Save parameters
0x2007, 0, 2

2
commandline/canbus.c
View File

@ -54,7 +54,7 @@ static char *read_string();
* @return amount of bytes read
*/
static int read_ttyX(TTY_descr *d){
if(d->comfd < 0) return 0;
if(!d || d->comfd < 0) return -1;
size_t L = 0;
ssize_t l;
size_t length = d->bufsz;

4
commandline/cmdlnopts.c
View File

@ -64,7 +64,7 @@ static myoption cmdlnopts[] = {
{"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)")},
{"nodeid", NEED_ARG, NULL, 'I', arg_int, APTR(&G.NodeID), _("node ID (1..127)")},
{"microsteps", NEED_ARG,NULL, 'u', arg_int, APTR(&G.microsteps),_("microstepping (0..256)")},
{"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (steps)")},
{"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (steps)")},
@ -72,6 +72,8 @@ static myoption cmdlnopts[] = {
{"stop", NO_ARGS, NULL, 'S', arg_int, APTR(&G.stop), _("stop motor")},
{"clearerr",NO_ARGS, NULL, 'c', arg_int, APTR(&G.clearerr), _("clear errors")},
{"zeropos", NO_ARGS, NULL, '0', arg_int, APTR(&G.zeropos), _("set current position to zero")},
{"parse", NEED_ARG, NULL, 'p', arg_string, APTR(&G.parsefile), _("file with SDO data to send to device")},
{"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")},
end_option
};

2
commandline/cmdlnopts.h
View File

@ -35,6 +35,7 @@ typedef struct{
char *device; // serial device name
char *pidfile; // name of PID file
char *logfile; // logging to this file
char *parsefile; // file to parse
int canspeed; // CAN bus speed
int serialspeed; // serial device speed (CAN-bus to USB)
int NodeID; // node ID to work with
@ -45,6 +46,7 @@ typedef struct{
int stop; // stop motor
int clearerr; // try to clear errors
int zeropos; // set position to zero
char *checkfile; // SDO data filename to check
} glob_pars;

138
commandline/dataparser.c Normal file
View File

@ -0,0 +1,138 @@
/*
* 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 "canopen.h"
#include "dataparser.h"
#include "pusirobot.h"
#include <libgen.h> // basename
#include <stdio.h> // fopen
#include <string.h> // strchr
#include <usefull_macros.h>
#define BUFSZ 256
char *getl(char *str, long long *lp){
long long l;
char *eptr;
int base = 0;
if(str[0] == '0' && (str[1] == 'b' || str[1] == 'B')){ // binary number format
base = 2;
str += 2;
}
l = strtoll(str, &eptr, base);
if(str == eptr) return NULL;
if(lp) *lp = l;
return eptr;
}
// get next value - after ',' and spaces
char *getnxt(char *str){
char *comma = strchr(str, ',');
if(!comma || !*comma) return NULL;
for(; str < comma; ++str) if(*str > ' ') return NULL;
++comma;
while(*comma && *comma <= ' ') ++comma;
return comma;
}
/**
* @brief parse_data_file - read SDO & data from file and send them to node `nid`
* @param fname - file name
* @param nid - node ID or 0 to check file
* @return 0 if no errors found
*/
int parse_data_file(const char *fname, uint8_t nid){
int ret = 0;
if(!fname){
WARNX("No filename for parsing");
return 1;
}
FILE *f = fopen(fname, "r");
if(!f){
WARN("Can't open %s for parsing", basename((char*)fname));
return 2;
}
char str[BUFSZ];
int lineno = 0;
while(fgets(str, BUFSZ, f)){
char *num = strchr(str, '#');
if(num) *num = 0;
long long l;
uint16_t idx;
uint8_t sidx;
int64_t data;
int isgood = 1;
do{
char *ptr;
if(!(ptr = getl(str, &l))) break;
idx = (uint16_t) l;
if(!(ptr = getnxt(ptr))){
isgood = 0;
break;
}
if(!(ptr = getl(ptr, &l))){
isgood = 0;
break;
}
sidx = (uint8_t) l;
if(!(ptr = getnxt(ptr))){
isgood = 0;
break;
}
if(!(ptr = getl(ptr, &l))){
isgood = 0;
break;
}
data = (int64_t) l;
DBG("Got: idx=0x%04X, subidx=0x%02X, data=0x%lX", idx, sidx, data);
if(nid == 0) printf("line #%d: read SDO with index=0x%04X, subindex=0x%02X, data=0x%lX (dec: %ld)\n", lineno, idx, sidx, data, data);
SDO_dic_entry *entry = dictentry_search(idx, sidx);
if(!entry){
WARNX("SDO 0x%04X/0x%02X isn't in dictionary", idx, sidx);
continue;
}
if(data<0 && !entry->issigned){
WARNX("SDO 0x%04X/0x%02X is only positive", idx, sidx);
continue;
}
uint64_t u64 = (uint64_t) (l > 0) ? l : -l;
if(u64 > UINT32_MAX){
WARNX("The data size of SDO 0x%04X/0x%02X out of possible range for uint32_t", idx, sidx);
continue;
}
uint32_t u = (uint32_t) (l > 0) ? l : -l;
if(entry->datasize != 4){
uint32_t mask = 0xff;
if(entry->datasize == 2) mask = 0xffff;
if((u & mask) != u){
WARNX("The data size of SDO 0x%04X/0x%02X is larger than possible (%d byte[s])", idx, sidx, entry->datasize);
continue;
}
}
if(nid) SDO_write(entry, nid, data);
}while(0);
if(!isgood){
WARNX("Bad syntax in line #%d: %s\nFormat: index, subindex, data (all may be hex, dec, oct or bin)", lineno, str);
}
++lineno;
}
fclose(f);
return ret;
}

27
commandline/dataparser.h Normal file
View File

@ -0,0 +1,27 @@
/*
* 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 DATAPARSER_H__
#define DATAPARSER_H__
#include <stdint.h>
int parse_data_file(const char *fname, uint8_t nid);
#endif // DATAPARSER_H__

67
commandline/dicentries.h Normal file
View File

@ -0,0 +1,67 @@
/*
* 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/>.
*/
// this file can be included more than once!
// 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)
// EXT emergency stop enable
DICENTRY(EXTENABLE, 0x600F, 1, 1, 0)
// EXT emergency stop trigger mode
DICENTRY(EXTTRIGMODE, 0x600F, 2, 1, 0)
// EXT emergency sensor type
DICENTRY(EXTSENSTYPE, 0x600F, 3, 1, 0)
// absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1)
// stop motor
DICENTRY(STOP, 0x6020, 0, 1, 0)

9
commandline/main.c
View File

@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "dataparser.h"
#include <signal.h>
#include <stdio.h>
#include <string.h>
@ -48,6 +49,9 @@ int main(int argc, char *argv[]){
initial_setup();
char *self = strdup(argv[0]);
GP = parse_args(argc, argv);
if(GP->checkfile){ // just check and exit
return parse_data_file(GP->checkfile, 0);
}
check4running(self, GP->pidfile);
free(self);
signal(SIGTERM, signals); // kill (-15) - quit
@ -80,6 +84,11 @@ int main(int argc, char *argv[]){
signals(2);
}
if(GP->parsefile){
green("Try to parse %s and send SDOs to device\n", GP->parsefile);
parse_data_file(GP->parsefile, GP->NodeID);
}
//setup_con();
// print current position and state
int64_t i64;

31
commandline/pusirobot.c
View File

@ -18,10 +18,21 @@
#include <stdlib.h> // for NULL
// we should init constants here!
#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s};
#include "pusirobot.h"
// we should init constants here!
#undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s) const SDO_dic_entry name = {idx, sidx, sz, s};
#include "dicentries.h"
// now init array with all dictionary
#undef DICENTRY
#define DICENTRY(name, idx, sidx, sz, s) {idx, sidx, sz, s},
static const SDO_dic_entry allrecords[] = {
#include "dicentries.h"
};
const int DEsz = sizeof(allrecords) / sizeof(SDO_dic_entry);
// controller status for bits
static const char *DevStatus[] = {
"External stop 1",
@ -58,12 +69,12 @@ const char *errname(uint8_t error, uint8_t bit){
return NULL;
}
/*
// 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;
// search if the object exists in dictionary - for config file parser
SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex){
// the search is linear as dictionary can be unsorted!!!
for(int i = 0; i < DEsz; ++i){
const SDO_dic_entry *entry = &allrecords[i];
if(entry->index == index && entry->subindex == subindex) return (SDO_dic_entry*)entry;
}
return NULL;
}
*/

42
commandline/pusirobot.h
View File

@ -33,46 +33,7 @@ typedef struct{
#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)
// absolute displacement
DICENTRY(ABSSTEPS, 0x601C, 0, 4, 1)
// stop motor
DICENTRY(STOP, 0x6020, 0, 1, 0)
#include "dicentries.h"
#define MAX_SPEED_MIN -200000
#define MAX_SPEED_MAX 200000
@ -81,6 +42,7 @@ DICENTRY(STOP, 0x6020, 0, 1, 0)
#define BUSY_STATE (1<<3)
const char *devstatus(uint8_t status, uint8_t bit);
const char *errname(uint8_t error, uint8_t bit);
SDO_dic_entry *dictentry_search(uint16_t index, uint8_t subindex);
//int get_current_position(uint8_t NID);
#endif // PUSIROBOT_H__