From f3023b1cc08a5ad128001c7198d2fd949d0f5ac1 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Tue, 18 Feb 2025 17:36:25 +0300 Subject: [PATCH] some changes: bug fixes, working with PMC007 --- commandline/CMakeLists.txt | 4 +- commandline/canbus.c | 6 ++- commandline/canbus.h | 2 +- commandline/canopen.c | 33 +++++++++---- commandline/canopen.h | 4 +- commandline/cfg/2dirturret/Readme | 32 +++++++++++++ commandline/cfg/2dirturret/Rgotoangle | 46 ++++++++++++++++++ commandline/cfg/2dirturret/Start_settings.cfg | 35 ++++++++++++++ commandline/cfg/2dirturret/Tgotopos | 43 +++++++++++++++++ commandline/cfg/2dirturret/Tinit | 5 ++ commandline/cfg/3axiscorr/Readme | 1 + commandline/cfg/3axiscorr/loccorr_init | 19 ++++++++ commandline/cfg/3axiscorr/loccorr_telepalka | 24 ++++++++++ commandline/cfg/Start_settings.cfg | 10 +++- commandline/cfg/Start_settingsA.cfg | 20 ++++++++ commandline/cfg/endswitchesOFF.cfg | 2 + commandline/cfg/endswitches_NC_Pullup.cfg | 13 +++++ commandline/dicentries.in | 4 ++ commandline/main.c | 48 +++++++++---------- commandline/pusirobot.h | 2 +- 20 files changed, 308 insertions(+), 45 deletions(-) create mode 100644 commandline/cfg/2dirturret/Readme create mode 100755 commandline/cfg/2dirturret/Rgotoangle create mode 100644 commandline/cfg/2dirturret/Start_settings.cfg create mode 100755 commandline/cfg/2dirturret/Tgotopos create mode 100755 commandline/cfg/2dirturret/Tinit create mode 100644 commandline/cfg/3axiscorr/Readme create mode 100755 commandline/cfg/3axiscorr/loccorr_init create mode 100755 commandline/cfg/3axiscorr/loccorr_telepalka create mode 100644 commandline/cfg/Start_settingsA.cfg create mode 100644 commandline/cfg/endswitchesOFF.cfg create mode 100644 commandline/cfg/endswitches_NC_Pullup.cfg diff --git a/commandline/CMakeLists.txt b/commandline/CMakeLists.txt index 5cd0e2d..13cf023 100644 --- a/commandline/CMakeLists.txt +++ b/commandline/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.0) set(PROJ steppermove) -set(MINOR_VERSION "1") +set(MINOR_VERSION "2") set(MID_VERSION "0") set(MAJOR_VERSION "0") set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}") -project(${PROJ} VERSION ${PROJ_VERSION} LANGUAGES C) +project(${PROJ} VERSION ${VERSION} LANGUAGES C) #enable_language(C) message("VER: ${VERSION}") diff --git a/commandline/canbus.c b/commandline/canbus.c index 1974640..7da45c1 100644 --- a/commandline/canbus.c +++ b/commandline/canbus.c @@ -98,6 +98,7 @@ static int ttyWR(const char *buff, int len){ int w = write_tty(dev->comfd, buff, (size_t)len); if(!w) w = write_tty(dev->comfd, "\n", 1); DBG("Written, dt=%g", dtime() - t0); + /* int errctr = 0; while(1){ char *s = read_string(); // clear echo & check @@ -108,7 +109,7 @@ static int ttyWR(const char *buff, int len){ break; } }else break; - } + }*/ pthread_mutex_unlock(&mutex); DBG("Success, dt=%g", dtime() - t0); return w; @@ -206,7 +207,7 @@ static char *read_string(){ memcpy(ptr, dev->buf, dev->buflen); r += l; LL -= l; ptr += l; if(ptr[-1] == '\n'){ - //DBG("Newline detected"); + DBG("Newline detected"); break; } d0 = dtime(); @@ -218,6 +219,7 @@ static char *read_string(){ if(optr){ *optr = 0; ++optr; + if(*optr) DBG("optr=%s", optr); }else{ WARNX("read_string(): no newline found"); DBG("buf: %s", buf); diff --git a/commandline/canbus.h b/commandline/canbus.h index f2c2bc8..742badf 100644 --- a/commandline/canbus.h +++ b/commandline/canbus.h @@ -22,7 +22,7 @@ #include #ifndef T_POLLING_TMOUT -#define T_POLLING_TMOUT (0.5) +#define T_POLLING_TMOUT (0.1) #endif typedef struct{ diff --git a/commandline/canopen.c b/commandline/canopen.c index 18bd5da..a9a8286 100644 --- a/commandline/canopen.c +++ b/commandline/canopen.c @@ -164,17 +164,21 @@ static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ CANmesg mesg; SDO *sdo = NULL; double t0 = dtime(); - while(dtime() - t0 < SDO_ANS_TIMEOUT){ + while(dtime() - t0 < T_POLLING_TMOUT){ mesg.ID = TSDO_COBID | NID; // read only from given ID if(canbus_read(&mesg)){ + DBG("NO CAN data"); continue; } sdo = parseSDO(&mesg); - if(!sdo) continue; + if(!sdo){ + DBG("Not SDO"); + continue; + } if(sdo->index == idx && sdo->subindex == subidx) break; } if(!sdo || sdo->index != idx || sdo->subindex != subidx){ - WARNX("No answer from SDO 0x%X/0x%X", idx, subidx); + DBG("No answer from SDO 0x%X/0x%X", idx, subidx); return NULL; } return sdo; @@ -189,11 +193,18 @@ static SDO *getSDOans(uint16_t idx, uint8_t subidx, uint8_t NID){ */ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID){ FNAME(); - if(ask2read(idx, subidx, NID)){ - WARNX("readSDOvalue(): Can't initiate upload"); - return NULL; + double t0 = dtime(); + SDO *sdo = NULL; + for(int i = 0; i < NTRIES && dtime() - t0 < SDO_ANS_TIMEOUT; ++i){ + DBG("Try %d ...", i); + if(ask2read(idx, subidx, NID)){ + DBG("Can't initiate upload"); + continue; + } + if((sdo = getSDOans(idx, subidx, NID))) break; + DBG("got no answer"); } - return getSDOans(idx, subidx, NID); + return sdo; } static inline uint32_t mku32(uint8_t data[4]){ @@ -228,10 +239,12 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){ return INT64_MIN; } if(sdo->ccs == CCS_ABORT_TRANSFER){ // error - WARNX("Got error for SDO 0x%X", e->index); uint32_t ac = mku32(sdo->data); - const char *etxt = abortcode_text(ac); - if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); + if(ac != 0x06020000){ // don't warn for unexistant objects + WARNX("Got error for SDO 0x%X", e->index); + const char *etxt = abortcode_text(ac); + if(etxt) WARNX("Abort code 0x%X: %s", ac, etxt); + } return INT64_MIN; } if(sdo->datalen != e->datasize){ diff --git a/commandline/canopen.h b/commandline/canopen.h index 1e26a49..029b660 100644 --- a/commandline/canopen.h +++ b/commandline/canopen.h @@ -24,9 +24,9 @@ #include "pusirobot.h" // timeout for answer from the SDO, seconds -#define SDO_ANS_TIMEOUT (2.5) +#define SDO_ANS_TIMEOUT (0.2) // N tries to write CAN message -#define NTRIES (15) +#define NTRIES (3) // COB-ID base: #define NMT_COBID 0 diff --git a/commandline/cfg/2dirturret/Readme b/commandline/cfg/2dirturret/Readme new file mode 100644 index 0000000..976d1f6 --- /dev/null +++ b/commandline/cfg/2dirturret/Readme @@ -0,0 +1,32 @@ +scripts for two-direction turret + +lambda/4 +rotates in negative direction +speed=400 +full circle = 400 steps (12800 microsteps for 1/32) +35.(5) microsteps per 1 degree + +turrret +rotates in positive direction +speed=1000 +full circle = 200 steps (6400 mircosteps for 1/32) +six positions, so distance between positions is 1066.(6) microsteps + + +scripts: + +Tinit - init stepper driver (run just after power on) +Tgotopos - move turret to given position (0..5) +Rgotoangle - rotate rotator to given angle (0..359 degrees) + +WARNING! If LIM1=1 (i.e. one of limit switches active) but lsw activated by other device than you want move, +you won't be able to move it! + +So, if you need to move turret to some position when rotator is at 0 angle, you should first remove rotator from +limit switch (at least to 30 degrees), then rotate turret and after all move rotator to given angle. +The same is for turret position 2: it always on LIM1, so to move rotator when turret is on 2 you should move +turret to any other position first, after that you will be able to rotate rotator + +To check just run +steppermove -vvvw -E3 -I11 +(twice, if first run won't show you limit switches), and you will see value of LIM1. \ No newline at end of file diff --git a/commandline/cfg/2dirturret/Rgotoangle b/commandline/cfg/2dirturret/Rgotoangle new file mode 100755 index 0000000..54b6fc7 --- /dev/null +++ b/commandline/cfg/2dirturret/Rgotoangle @@ -0,0 +1,46 @@ +#!/bin/bash + +CANID=11 +STPPERROT=12800 + +function onlim(){ + eval $(steppermove -I11 -E3 | grep LIM1) + echo $LIM1 +} + + +if [[ $# != 1 ]]; then + echo -e "Usage: $(basename $0) position\n\twhere 'position' is an angle [integer number!!!] from 0 to 360\t0 - limit switch position" >&2 + exit 1 +fi + +if (( $1 > 360 || $1 < 0 )); then + echo "'position' should be from 0 360" >&2 + exit 1 +fi + +steppermove -I${CANID} -m1000 +if [[ $(onlim) == "1" ]]; then + echo "Go out from limit switch" + steppermove -I${CANID} -cA + steppermove -I${CANID} -r -$((STPPERROT / 4)) -w + steppermove -I${CANID} -E3 -m1000 + if (( $(onlim) == 1 )); then + echo "TURRET is on limit switch, rotate it first!" >&2 + exit 1 + fi +fi + +echo "Go to limit switch" +steppermove -I${CANID} -E3 -r -$STPPERROT -w +steppermove -I${CANID} -cA -w + +if (( $1 != 0 )); then + STP=$(printf %.0f $(echo "12800/360*$1 + 31" | bc -l)) + STP=$(( STP/32*32 )) + echo "Go to angle $1" + steppermove -I${CANID} -r -$STP -w +fi + +echo "OK, at place!" +steppermove -I${CANID} -E3 diff --git a/commandline/cfg/2dirturret/Start_settings.cfg b/commandline/cfg/2dirturret/Start_settings.cfg new file mode 100644 index 0000000..f4a5f78 --- /dev/null +++ b/commandline/cfg/2dirturret/Start_settings.cfg @@ -0,0 +1,35 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set heartbeat time to 0 +0x1017, 0, 0 + +# Turn off offline programming +0x6018, 1, 0 +0x6018, 2, 0 + +# Set Node ID to 11 +0x2002, 0, 11 +# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) +0x2003, 0, 5 + +# Set microstepping to 32 +0x600A, 0, 32 +# Set max phase current to 1.0A +0x600B, 0, 1000 +# Set acceleration (0 - no, 1 - highest, 8 - lowest) +0x6008, 0, 2 +# Set deceleration +0x6009, 0, 2 + +# EXT1&EXT2 emergency stop @ rising edge (normal state == 0) +# Rising edge for triggers +0x600F, 2, 3 +# Pullup for triggers on rising edge +0x600F, 3, 3 +# Enable +0x600F, 1, 3 + + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/2dirturret/Tgotopos b/commandline/cfg/2dirturret/Tgotopos new file mode 100755 index 0000000..ff1fa56 --- /dev/null +++ b/commandline/cfg/2dirturret/Tgotopos @@ -0,0 +1,43 @@ +#!/bin/bash + +CANID=11 +STPPERROT=6400 +# positions: amount of steps for each turret position +POSITIONS=(4224 5291 6357 7424 8491 9557) + +function onlim(){ + eval $(steppermove -I11 -E3 | grep LIM1) + echo $LIM1 +} + +if [[ $# != 1 ]]; then + echo -e "Usage: $(basename $0) position\n\twhere 'position' is a number from 0 to 5\t0 is closed diaphragm" >&2 + exit 1 +fi + +if (( $1 > 5 || $1 < 0 )); then + echo "'position' should be from 0 (closed) to 5" >&2 + exit 1 +fi + +steppermove -I${CANID} -m400 +if [[ $(onlim) == 1 ]]; then + echo "Go out from limit switch" + steppermove -I${CANID} -cA + steppermove -I${CANID} -m1000 -r $((STPPERROT / 4)) -w + steppermove -I${CANID} -m400 + if (( $(onlim) == 1 )); then + echo "ROTATOR is on limit switch, rotate it first!" >&2 + exit 1 + fi +fi + +echo "Go to limit switch" +steppermove -I${CANID} -E3 -r $STPPERROT -w +steppermove -I${CANID} -cA -w + +echo "Go to position $1" +steppermove -I${CANID} -r ${POSITIONS[$1]} -w + +echo "OK, at place!" +steppermove -I${CANID} -E3 \ No newline at end of file diff --git a/commandline/cfg/2dirturret/Tinit b/commandline/cfg/2dirturret/Tinit new file mode 100755 index 0000000..b97975e --- /dev/null +++ b/commandline/cfg/2dirturret/Tinit @@ -0,0 +1,5 @@ +#!/bin/sh + +CANID=11 + +steppermove -vvv -I${CANID} -cE3 -s250 -m400 -w diff --git a/commandline/cfg/3axiscorr/Readme b/commandline/cfg/3axiscorr/Readme new file mode 100644 index 0000000..2542231 --- /dev/null +++ b/commandline/cfg/3axiscorr/Readme @@ -0,0 +1 @@ +scripts for 3-motor local corrector diff --git a/commandline/cfg/3axiscorr/loccorr_init b/commandline/cfg/3axiscorr/loccorr_init new file mode 100755 index 0000000..64f9eb8 --- /dev/null +++ b/commandline/cfg/3axiscorr/loccorr_init @@ -0,0 +1,19 @@ +#!/bin/sh + +# init 3-point local corrector + +for I in 2 10 11; do + ./steppermove -I$I -m8000 -r-10000 +done + +for I in 2 10 11; do + ./steppermove -I$I -w +done + +for I in 2 10 11; do + ./steppermove -I$I -0 +done + +for I in 2 10 11; do + ./steppermove -I$I -a 7200 +done diff --git a/commandline/cfg/3axiscorr/loccorr_telepalka b/commandline/cfg/3axiscorr/loccorr_telepalka new file mode 100755 index 0000000..5d5d287 --- /dev/null +++ b/commandline/cfg/3axiscorr/loccorr_telepalka @@ -0,0 +1,24 @@ +#!/bin/sh + +# random move 3-point local corrector around middle point + +function rmove(){ # $1 - current motor index, $2 - index of motor to correct into+, $3 - relative steps + arr=(2 10 11) # ID's of motors + ID=${arr[$1]} + #./steppermove -I$ID -w + if [[ $1 -ne $2 ]]; then # move to minus + ./steppermove -I$ID -a $(( 7200 - S )) + else # move to plus + ./steppermove -I$ID -a $(( 7200 + S )) + fi +} + +trap "echo Exited!; exit;" SIGINT SIGTERM + +while true; do + I=$(( RANDOM % 3 )) + S=$(( RANDOM % 1280 )) + for i in 0 1 2; do + rmove $i $I $S + done +done diff --git a/commandline/cfg/Start_settings.cfg b/commandline/cfg/Start_settings.cfg index 1b21d96..3b5cc74 100644 --- a/commandline/cfg/Start_settings.cfg +++ b/commandline/cfg/Start_settings.cfg @@ -8,13 +8,19 @@ 0x6018, 1, 0 0x6018, 2, 0 -# Set Node ID to 10 -0x2002, 0, 10 +# Set Node ID to 11 +0x2002, 0, 11 # Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) 0x2003, 0, 5 +# Set microstepping to 32 +0x600A, 0, 32 # Set max phase current to 1.5A 0x600B, 0, 1500 +# Set acceleration (0 - no, 1 - highest, 8 - lowest) +0x6008, 0, 2 +# Set deceleration +0x6009, 0, 2 # EXT1&EXT2 emergency stop: EXT1@falling edge (normal state == 1), EXT2@rising edge (normal state == 0) # Falling edge for trigger #1 diff --git a/commandline/cfg/Start_settingsA.cfg b/commandline/cfg/Start_settingsA.cfg new file mode 100644 index 0000000..869d0cf --- /dev/null +++ b/commandline/cfg/Start_settingsA.cfg @@ -0,0 +1,20 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# Set heartbeat time to 0 +0x1017, 0, 0 + +# Turn off offline programming +0x6018, 1, 0 +0x6018, 2, 0 + +# Set Node ID +0x2002, 0, 11 +# Set speed to 250 (0-20, 1-25, 2-50, 3-100, 4-125, 5-250, 6-500, 7-800, 8-1000) +0x2003, 0, 5 + +# Set max phase current to x mA +0x600B, 0, 1000 + +# Save parameters +0x2007, 0, 2 diff --git a/commandline/cfg/endswitchesOFF.cfg b/commandline/cfg/endswitchesOFF.cfg new file mode 100644 index 0000000..eacca84 --- /dev/null +++ b/commandline/cfg/endswitchesOFF.cfg @@ -0,0 +1,2 @@ +# Disable EXT1, EXT2 +0x600F, 1, 0 diff --git a/commandline/cfg/endswitches_NC_Pullup.cfg b/commandline/cfg/endswitches_NC_Pullup.cfg new file mode 100644 index 0000000..8757077 --- /dev/null +++ b/commandline/cfg/endswitches_NC_Pullup.cfg @@ -0,0 +1,13 @@ +# Transmit SDO to driver +# Format: index, subindex, data + +# EXT1 emergency stop @ rising edge (normal state == 0) +# Rising edge for trigger #1 +0x600F, 2, 3 +# Pullup for triggers on rising edge +0x600F, 3, 3 +# Enable EXT1 +0x600F, 1, 3 + +# save settings +#0x2007, 0, 2 diff --git a/commandline/dicentries.in b/commandline/dicentries.in index 0e5fce1..97ebbca 100644 --- a/commandline/dicentries.in +++ b/commandline/dicentries.in @@ -103,6 +103,8 @@ DICENTRY(POSITION, 0x600C, 0, 4, 1, "current position") DICENTRY(CURRREDUCT, 0x600D, 0, 1, 0, "current reduction") // motor enable DICENTRY(ENABLE, 0x600E, 0, 1, 0, "motor enable") +// EXT emergency stop Npar +DICENTRY(EXTNPAR, 0x600F, 0, 1, 0, "EXT emergency stop number of parameters") // EXT emergency stop enable DICENTRY(EXTENABLE, 0x600F, 1, 1, 0, "EXT emergency stop enable") // EXT emergency stop trigger mode @@ -120,6 +122,8 @@ DICENTRY(STALLPARS, 0x6017, 0, 2, 0, "stall parameters (open loop)") // offline operation DICENTRY(OFFLNMBR, 0x6018, 1, 1, 0, "Number of offline programming command") DICENTRY(OFFLENBL, 0x6018, 2, 1, 0, "Offline automatic operation enable") +// EXT stabilize delay +DICENTRY(EXTSTABDELAY, 0x601A, 0, 2, 0, "EXT stabilize delay (ms)") // stall set DICENTRY(STALLSET, 0x601B, 0, 1, 0, "stall set (open loop)") // absolute displacement diff --git a/commandline/main.c b/commandline/main.c index 24198eb..d8b1b5f 100644 --- a/commandline/main.c +++ b/commandline/main.c @@ -123,7 +123,7 @@ static inline void gpioval(int64_t es){ } 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); + printf("LIM%d=%d\n", i, (EXTACTIVE(i, v))); } } @@ -138,8 +138,8 @@ static inline void showAllPars(){ entry->index, entry->subindex, entry->name); continue; } - printf("# %s\n0x%04X, %d, %ld (0x%lX)\n", entry->name, entry->index, - entry->subindex, val, val); + printf("# %s\n0x%04X, %d, %ld (0x%X)\n", entry->name, entry->index, + entry->subindex, val, (uint32_t)val); fflush(stdout); } printf("\n\n"); @@ -171,7 +171,6 @@ static inline void wait_busy(){ int main(int argc, char *argv[]){ initial_setup(); - char *self = strdup(argv[0]); GP = parse_args(argc, argv); if(GP->verblevel) maxmesglevl(GP->verblevel); if(GP->checkfile){ // just check and exit @@ -198,8 +197,7 @@ int main(int argc, char *argv[]){ } if(GP->enableESW && GP->disableESW) ERRX("Enable & disable ESW can't meet together"); - check4running(self, GP->pidfile); - free(self); + check4running(NULL, GP->pidfile); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit @@ -236,6 +234,23 @@ int main(int argc, char *argv[]){ getSDOe(MICROSTEPS, setusteps, "Can't get microstepping"); Mesg("MICROSTEPS: %g\n", dtime() - d0); }*/ + // enable limit switches + if(GP->enableESW){ + 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); if(!GP->quick){ // check error status getSDOe(ERRSTATE, chkerr, "Can't get error status"); @@ -265,7 +280,7 @@ int main(int argc, char *argv[]){ else message(2, "MICROSTEPS=%u", i64); } i64 = SDO_read(&ENCRESOL, ID); - if(i64 == INT64_MIN) LogAndWarn("Can't get encoder resolution value"); + if(i64 == INT64_MIN){ /* LogAndWarn("Can't get encoder resolution value"); */} else message(2, "ENCRESOL=%u", 1 << i64); if(GP->absmove != INT_MIN || GP->relmove != INT_MIN || !GP->quick || GP->wait){ // check device status @@ -304,24 +319,6 @@ int main(int argc, char *argv[]){ ++p; } } - // enable limit switches - if(GP->enableESW){ - 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); - // 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); @@ -342,6 +339,7 @@ int main(int argc, char *argv[]){ ERRX("Can't move to relative position %d", GP->relmove); Mesg("RelMove: %g\n", dtime() - d0); } + if(GP->wait) wait_busy(); #undef getSDOe #if 0 diff --git a/commandline/pusirobot.h b/commandline/pusirobot.h index 6ad6589..9d25613 100644 --- a/commandline/pusirobot.h +++ b/commandline/pusirobot.h @@ -46,7 +46,7 @@ extern const SDO_dic_entry* allrecords[]; #define SPEED_MULTIPLIER (1.6) // limit switches mask in GPIO status register (x=1,2,3) -#define EXTMASK(x) (1<<(6+x)) +#define EXTMASK(x) (1<<(7+x)) #define EXTACTIVE(x, reg) ((reg&EXTMASK(x)) ? 1:0)