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 better reaction to -q keyword

Browse Source
This commit is contained in:
Edward Emelianov 2020-10-19 15:20:06 +03:00
parent fe0fe62cad
commit 6ef30d6d2a
10 changed files with 139 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
commandline/canbus.c
View File

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

22
commandline/cfg/Oscill Executable file
View File

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

8
commandline/cfg/SetCurnt0.8.cfg Normal file
View File

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

8
commandline/cfg/SetCurnt1.0.cfg Normal file
View File

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

8
commandline/cfg/SetCurnt1.5.cfg Normal file
View File

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

8
commandline/cfg/SetCurnt2.0.cfg Normal file
View File

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

37
commandline/cfg/TurretMove Executable file
View File

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

10
commandline/cmdlnopts.c
View File

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

2
commandline/cmdlnopts.h
View File

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

45
commandline/main.c
View File

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