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

some changes: bug fixes, working with PMC007

Browse Source
This commit is contained in:
Edward Emelianov 2025-02-18 17:36:25 +03:00
parent 1453398f08
commit f3023b1cc0
20 changed files with 308 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
commandline/CMakeLists.txt
View File

@ -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}")

6
commandline/canbus.c
View File

@ -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);

2
commandline/canbus.h
View File

@ -22,7 +22,7 @@
#include <stdint.h>
#ifndef T_POLLING_TMOUT
#define T_POLLING_TMOUT (0.5)
#define T_POLLING_TMOUT (0.1)
#endif
typedef struct{

33
commandline/canopen.c
View File

@ -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){

4
commandline/canopen.h
View File

@ -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

32
commandline/cfg/2dirturret/Readme Normal file
View File

@ -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.

46
commandline/cfg/2dirturret/Rgotoangle Executable file
View File

@ -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

35
commandline/cfg/2dirturret/Start_settings.cfg Normal file
View File

@ -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

43
commandline/cfg/2dirturret/Tgotopos Executable file
View File

@ -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

5
commandline/cfg/2dirturret/Tinit Executable file
View File

@ -0,0 +1,5 @@
#!/bin/sh
CANID=11
steppermove -vvv -I${CANID} -cE3 -s250 -m400 -w

1
commandline/cfg/3axiscorr/Readme Normal file
View File

@ -0,0 +1 @@
scripts for 3-motor local corrector

19
commandline/cfg/3axiscorr/loccorr_init Executable file
View File

@ -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

24
commandline/cfg/3axiscorr/loccorr_telepalka Executable file
View File

@ -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

10
commandline/cfg/Start_settings.cfg
View File

@ -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

20
commandline/cfg/Start_settingsA.cfg Normal file
View File

@ -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

2
commandline/cfg/endswitchesOFF.cfg Normal file
View File

@ -0,0 +1,2 @@
# Disable EXT1, EXT2
0x600F, 1, 0

13
commandline/cfg/endswitches_NC_Pullup.cfg Normal file
View File

@ -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

4
commandline/dicentries.in
View File

@ -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

48
commandline/main.c
View File

@ -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

2
commandline/pusirobot.h
View File

@ -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)