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
pusirobot/commandline/main.c

238 lines
8.2 KiB
C
Raw Blame History

/*
* 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 "dataparser.h"
#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`
static uint8_t ID = 0;
static uint16_t microstepping = 0;
void signals(int sig){
putlog("Exit with status %d", sig);
DBG("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);
}
static inline void chkerr(int64_t es){
if(!es) return;
red("ERRSTATE=%d\n", es);
uint8_t s = (uint8_t)es;
for(uint8_t i = 0; i < 8; ++i){
const char *msg = errname(s, i);
if(msg) red("\t%s\n", msg);
}
if(!GP->clearerr) ERRX("Error status is not zero");
if(SDO_write(&ERRSTATE, ID, s) || 0 != (es = SDO_read(&ERRSTATE, ID))){
ERRX("Can't clean error status");
}
}
static inline void chkstat(int64_t es){
if(es) red("DEVSTATUS=%d\n", (int)es);
else green("DEVSTATUS=0\n");
uint8_t s = (uint8_t)es;
if(s){
for(uint8_t i = 0; i < 8; ++i){
const char *msg = devstatus(s, i);
if(msg) red("\t%s\n", msg);
}
if(s != BUSY_STATE && GP->clearerr && (SDO_write(&DEVSTATUS, ID, s) || 0 != (es = SDO_read(&DEVSTATUS, ID)))){
ERRX("Can't clean device status");
}
if(es && es != BUSY_STATE) ERRX("Can't work in this state"); // DIE if !busy
}
}
static inline void setusteps(int64_t es){
if(GP->microsteps > -1 && GP->microsteps != (int) es){
DBG("Try to change microsteps");
if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID)))
ERRX("Can't change microstepping");
}
microstepping = (uint16_t) es;
green("MICROSTEPPING=%u\n", microstepping);
}
static inline void setmaxspd(int64_t es){
DBG("abs=%d, rel=%d", GP->absmove, GP->relmove);
if(es == 0 && (GP->absmove != INT_MIN || GP->relmove != INT_MIN) && (GP->maxspeed == INT_MIN || GP->maxspeed == 0))
ERRX("Can't move when MAXSPEED==0");
if(GP->maxspeed != INT_MIN){
GP->maxspeed *= microstepping;
if(GP->maxspeed < MAX_SPEED_MIN || GP->maxspeed > MAX_SPEED_MAX)
ERRX("MAXSPEED should be from %d to %d", MAX_SPEED_MIN/microstepping, MAX_SPEED_MAX/microstepping);
DBG("Try to change max speed");
if(SDO_write(&MAXSPEED, ID, GP->maxspeed) || INT64_MIN == (es = SDO_read(&MAXSPEED, ID)))
ERRX("Can't change max speed");
}
if(es) green("MAXSPEED=%d\n", (int)es/microstepping);
else red("MAXSPEED=0\n");
}
static inline void gpioval(int64_t es){
uint16_t v = (uint16_t) es;
if(INT64_MIN == (es = SDO_read(&EXTENABLE, ID))){
WARNX("Can't read limit switches configuration");
return;
}
for(int i = 1; i < 4; ++i){
if(0 == (es & 1<<(i-1))) continue; // endswitch disabled
if(EXTACTIVE(i, v)) red("LIM%d=1\n", i);
}
}
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
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->microsteps > 0 && (1 != __builtin_popcount(GP->microsteps) || GP->microsteps == 1)) // __builtin_popcount - amount of non-zero bits in uint
ERRX("Wrong microstepping settings, should be 0 or 2^(1..8)");
if(GP->absmove != INT_MIN || GP->relmove != INT_MIN){ // wanna move
if(GP->absmove != INT_MIN && GP->relmove != INT_MIN)
ERRX("ABSMOVE and RELMOVE can't be used together");
if(GP->maxspeed == 0)
ERRX("Set non-zero MAXSPEED");
}
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);
}
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;
ID = GP->NodeID;
#define getSDOe(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
#define getSDOw(SDO, fn, e) do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else WARNX(e);}while(0)
getSDOe(ERRSTATE, chkerr, "Can't get error status");
getSDOe(DEVSTATUS, chkstat, "Can't get device status");
getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
if(GP->zeropos){
if(SDO_write(&POSITION, ID, 0))
ERRX("Can't clear position counter");
}
if(INT64_MIN != (i64 = SDO_read(&POSITION, ID)))
green("CURPOS=%d\n", (int)i64/microstepping);
else ERRX("Can't read current position");
getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
getSDOw(GPIOVAL, gpioval, "Can't read GPIO values");
if(GP->disable){
if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor");
}
if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
if(i64) green("ENABLE=1\n");
else red("ENABLE=0\n");
}
if(GP->stop){
if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
}
if(GP->absmove != INT_MIN){
SDO_write(&ENABLE, ID, 1);
if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping))
ERRX("Can't move to absolute position %d", GP->absmove);
}
if(GP->relmove != INT_MIN && GP->relmove){
uint8_t dir = 1;
SDO_write(&ENABLE, ID, 1);
if(GP->relmove < 0){ // negative direction
dir = 0;
GP->relmove = -GP->relmove;
}
if(SDO_write(&ROTDIR, ID, dir) || INT64_MIN == (i64 = SDO_read(&ROTDIR, ID)))
ERRX("Can't change rotation direction");
DBG("i64=%ld, dir=%d", i64, dir);
if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping))
ERRX("Can't move to relative position %d", GP->relmove);
}
#undef getSDOe
#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;
}
Reference in New Issue View Git Blame Copy Permalink