From 6ef30d6d2aba85acc7ef0c802aa4a4a7f14f7c59 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Mon, 19 Oct 2020 15:20:06 +0300 Subject: [PATCH] Add better reaction to -q keyword --- commandline/canbus.c | 3 -- commandline/cfg/Oscill | 22 +++++++++++++ commandline/cfg/SetCurnt0.8.cfg | 8 +++++ commandline/cfg/SetCurnt1.0.cfg | 8 +++++ commandline/cfg/SetCurnt1.5.cfg | 8 +++++ commandline/cfg/SetCurnt2.0.cfg | 8 +++++ commandline/cfg/TurretMove | 37 +++++++++++++++++++++ commandline/cmdlnopts.c | 10 +++--- commandline/cmdlnopts.h | 2 ++ commandline/main.c | 57 +++++++++++++++++++++++---------- 10 files changed, 139 insertions(+), 24 deletions(-) create mode 100755 commandline/cfg/Oscill create mode 100644 commandline/cfg/SetCurnt0.8.cfg create mode 100644 commandline/cfg/SetCurnt1.0.cfg create mode 100644 commandline/cfg/SetCurnt1.5.cfg create mode 100644 commandline/cfg/SetCurnt2.0.cfg create mode 100755 commandline/cfg/TurretMove diff --git a/commandline/canbus.c b/commandline/canbus.c index 896588d..1974640 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -54,7 +54,6 @@ static char *read_string(); * @return amount of bytes read */ static int read_ttyX(TTY_descr *d){ - FNAME(); if(!d || d->comfd < 0) return -1; size_t L = 0; ssize_t l; @@ -179,7 +178,6 @@ int canbus_write(CANmesg *mesg){ * @return NULL if nothing was read or pointer to static buffer */ static char *read_string(){ - FNAME(); static char buf[1024]; int LL = 1023, r = 0, l; char *ptr = NULL; @@ -252,7 +250,6 @@ void showM(CANmesg *m){ #endif int canbus_read(CANmesg *mesg){ - FNAME(); if(!mesg) return 1; pthread_mutex_lock(&mutex); double t0 = dtime(); diff --git a/commandline/cfg/Oscill b/commandline/cfg/Oscill new file mode 100755 index 0000000..5774a2d --- /dev/null +++ b/commandline/cfg/Oscill @@ -0,0 +1,22 @@ +#!/bin/bash + +Ampl=$1 + +./steppermove -s250 +./steppermove -I1 -m4000 +./steppermove -I2 -m4000 + +#if false; then +./steppermove -I2 -qr-3000 +./steppermove -I1 -qr-3000 +./steppermove -I2 -qw -r500 +./steppermove -I1 -qr500 +./steppermove -I2 -qw +#fi + +while true; do +./steppermove -I2 -qwr${Ampl} +./steppermove -I1 -qr${Ampl} +./steppermove -I2 -qwr-${Ampl} +./steppermove -I1 -qr-${Ampl} +done diff --git a/commandline/cfg/SetCurnt0.8.cfg b/commandline/cfg/SetCurnt0.8.cfg new file mode 100644 index 0000000..f7527fd --- /dev/null +++ b/commandline/cfg/SetCurnt0.8.cfg @@ -0,0 +1,8 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set max phase current to 800mA +0x600B, 0, 800 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/SetCurnt1.0.cfg b/commandline/cfg/SetCurnt1.0.cfg new file mode 100644 index 0000000..4a4b0b9 --- /dev/null +++ b/commandline/cfg/SetCurnt1.0.cfg @@ -0,0 +1,8 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set max phase current to 600mA +0x600B, 0, 1000 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/SetCurnt1.5.cfg b/commandline/cfg/SetCurnt1.5.cfg new file mode 100644 index 0000000..84e7db1 --- /dev/null +++ b/commandline/cfg/SetCurnt1.5.cfg @@ -0,0 +1,8 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set max phase current to 600mA +0x600B, 0, 1500 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/SetCurnt2.0.cfg b/commandline/cfg/SetCurnt2.0.cfg new file mode 100644 index 0000000..2284b8e --- /dev/null +++ b/commandline/cfg/SetCurnt2.0.cfg @@ -0,0 +1,8 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set max phase current to 2A +0x600B, 0, 2000 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/TurretMove b/commandline/cfg/TurretMove new file mode 100755 index 0000000..2280971 --- /dev/null +++ b/commandline/cfg/TurretMove @@ -0,0 +1,37 @@ +#!/bin/bash + +function move(){ + ./steppermove -I4 $* +} + +if [ $# -ne 1 ]; then + echo "Usage: $0 Nposition (enumeration from 0)" >&2 + exit 1 +fi + +if [ $1 -lt 0 -o $1 -gt 5 ]; then + echo "Turet position should be from 0 to 5" >&2 + exit 2 +fi + +LAST=-1 +[ -f lastpos ] && LAST=$(cat lastpos) + +eval "$(move -s250 -m50)" + +DIFF=$((CURENCODER - LAST)) +ABS=${DIFF#-} # absoulte value + +if [ $ABS -gt 3 ]; then # goto zero if last position differs more then for 3 from current + move -cE1 -r-1000 + move -wcA + move -0 +fi + +move -Fa $((683*$1)) +move -w + +sleep 1 # wait for stopping +eval "$(move)" +echo $CURENCODER > lastpos +echo "Current encoder position: $CURENCODER" diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c index 79dab58..fde34e3 100644 --- a/commandline/cmdlnopts.c +++ b/commandline/cmdlnopts.c @@ -66,9 +66,9 @@ static myoption cmdlnopts[] = { {"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)")}, {"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)")}, - {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (steps per second)")}, + {"rel", NEED_ARG, NULL, 'r', arg_int, APTR(&G.relmove), _("move to relative position (full steps)")}, + {"abs", NEED_ARG, NULL, 'a', arg_int, APTR(&G.absmove), _("move to absolute position (full steps)")}, + {"maxspd", NEED_ARG, NULL, 'm', arg_int, APTR(&G.maxspeed), _("maximal motor speed (full steps per second)")}, {"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")}, @@ -76,9 +76,11 @@ static myoption cmdlnopts[] = { {"check", NEED_ARG, NULL, 'k', arg_string, APTR(&G.checkfile), _("check SDO data file")}, {"disable", NO_ARGS, NULL, 'D', arg_int, APTR(&G.disable), _("disable motor")}, {"readvals",NO_ARGS, NULL, 'R', arg_int, APTR(&G.showpars), _("read values of used parameters")}, - {"enablesw",NO_ARGS, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches 1 and 2")}, + {"enablesw",NEED_ARG, NULL, 'E', arg_int, APTR(&G.enableESW), _("enable end-switches with given mask")}, + {"disablesw",NO_ARGS, NULL, 'A', arg_int, APTR(&G.disableESW),_("disable end-switches")}, {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), _("wait while motor is busy")}, {"quick", NO_ARGS, NULL, 'q', arg_int, APTR(&G.quick), _("directly send command without getting status")}, + {"fracsteps",NO_ARGS, NULL, 'F', arg_int, APTR(&G.fracsteps), _("move to steps fractions amount instead of full steps")}, end_option }; diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h index 1882de9..be51f27 100644 --- a/commandline/cmdlnopts.h +++ b/commandline/cmdlnopts.h @@ -50,8 +50,10 @@ typedef struct{ int disable; // disable motor int showpars; // show values of some parameters int enableESW; // send signal to enable end-switches + int disableESW; // --//-- disable int wait; // wait while device is busy int quick; // directly send command without getting status + int fracsteps; // move in steps fractions instead of full steps } glob_pars; diff --git a/commandline/main.c b/commandline/main.c index f5d996b..c599bbe 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -82,12 +82,17 @@ static inline void chkstat(int64_t es){ // setup microstepping static inline void setusteps(int64_t es){ - if(GP->microsteps > -1 && GP->microsteps != (int) es){ + DBG("es=%zd", es); + int us = GP->microsteps; + if(us > 0){ + if(us == 1) us = 0; // PusiRobot driver needs value 0 for microstepping 1! + if(us != (int) es){ // change microstepping DBG("Try to change microsteps"); - if(SDO_write(&MICROSTEPS, ID, GP->microsteps) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID))) + if(SDO_write(&MICROSTEPS, ID, us) || INT64_MIN == (es = SDO_read(&MICROSTEPS, ID))) ERRX("Can't change microstepping"); + } } - microstepping = (uint16_t) es; + microstepping = es > 0 ? (uint16_t) es : 1; green("MICROSTEPPING=%u\n", microstepping); } @@ -181,8 +186,9 @@ int main(int argc, char *argv[]){ 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)"); + DBG("builtin: %d", __builtin_popcount(GP->microsteps)); + if(GP->microsteps > 0 && (1 != __builtin_popcount(GP->microsteps))) // __builtin_popcount - amount of non-zero bits in uint + ERRX("Wrong microstepping settings, should be 2^(0..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"); @@ -214,15 +220,21 @@ int main(int argc, char *argv[]){ //double d0 = dtime(); // get mircostepping (need this to properly calculate current position and move - getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); - Mesg("MICROSTEPS: %g\n", dtime() - d0); + if(GP->fracsteps) microstepping = 1; // Don't calculate microstepping in this case + else{ + getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); + Mesg("MICROSTEPS: %g\n", dtime() - d0); + } if(!GP->quick){ // check error status getSDOe(ERRSTATE, chkerr, "Can't get error status"); Mesg("ERRSTATE: %g\n", dtime() - d0); // get current position - if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))) - green("CURPOS=%d\n", (int)i64/microstepping); + if(INT64_MIN != (i64 = SDO_read(&POSITION, ID))){ + int enc = (int)i64; + green("CURENCODER=%d\n", enc); + green("CURSTEPS=%.2f\n", enc / ((double)microstepping)); + } else WARNX("Can't read current position"); Mesg("CURPOS: %g\n", dtime() - d0); // get limit switches values @@ -238,10 +250,12 @@ int main(int argc, char *argv[]){ getSDOe(MAXSPEED, setmaxspd, "Can't read max speed"); Mesg("MAXSPEED: %g\n", dtime() - d0); } - // check device status - getSDOe(DEVSTATUS, chkstat, "Can't get device status"); - Mesg("DEVSTATUS: %g\n", dtime() - d0); - if(devstat == BUSY_STATE && GP->wait) wait_busy(); + if(GP->absmove != INT_MIN || GP->relmove != INT_MIN || !GP->quick || GP->wait){ + // check device status + getSDOe(DEVSTATUS, chkstat, "Can't get device status"); + Mesg("DEVSTATUS: %g\n", dtime() - d0); + if(devstat == BUSY_STATE && GP->wait) wait_busy(); + } // stop motor if(GP->stop){ if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor"); @@ -271,19 +285,28 @@ int main(int argc, char *argv[]){ } // enable limit switches if(GP->enableESW){ - if(SDO_write(&EXTENABLE, ID, 3)){ + if(SDO_write(&EXTENABLE, ID, GP->enableESW)){ WARNX("Error when trying to enable limit switches"); if(GP->absmove || GP->relmove) signals(-1); } Mesg("EXTENABLE: %g\n", dtime() - d0); } + // disable limit switches + if(GP->disableESW){ + if(SDO_write(&EXTENABLE, ID, 0)){ + WARNX("Error when trying to disable limit switches"); + if(GP->absmove || GP->relmove) signals(-1); + } + } + //int64_t es = SDO_read(&EXTENABLE, ID); + //green("LIMITSW=%lld\n", es); + int multiplier = GP->fracsteps ? 1 : microstepping; // move to absolute position if(GP->absmove != INT_MIN){ if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state"); SDO_write(&ENABLE, ID, 1); - if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping)) + if(SDO_write(&ABSSTEPS, ID, GP->absmove * multiplier)) ERRX("Can't move to absolute position %d", GP->absmove); - Mesg("AbsMove: %g\n", dtime() - d0); } if(GP->relmove != INT_MIN && GP->relmove){ if(devstat == BUSY_STATE) ERRX("Can't move in BUSY state"); @@ -296,7 +319,7 @@ int main(int argc, char *argv[]){ 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)) + if(SDO_write(&RELSTEPS, ID, GP->relmove * multiplier)) ERRX("Can't move to relative position %d", GP->relmove); Mesg("RelMove: %g\n", dtime() - d0); }