/* sudo halcompile --install unlocking.comp */
component unlocking "HAL component that arbitrates between various states and sets adaptive feed accordingly";
pin in float idleaxis-##[3] ;
pin in bit isunlockaxis-##[3] ;
pin out float afeed "adaptive feed";
function _ ;
license "GPL";
;;
FUNCTION(_) {
int i;
for (i = 0 ; i < 3; i++){
if(idleaxis(i)){
if(!isunlockaxis(i)) {afeed = 0.0001;}
else {afeed = 1;}
}
}
if(!idleaxis(0) && !idleaxis(1) && !idleaxis(2) ) {afeed = 1.0;}
}
Code:
loadrt unlocking
addf unlocking.0 servo-thread
net adfeed unlocking.0.afeed motion.adaptive-feed
net in:positionX unlocking.0.idleaxis-00 axis.0.joint-vel-cmd
net in:positionY unlocking.0.idleaxis-01 axis.1.joint-vel-cmd
net in:positionZ unlocking.0.idleaxis-02 axis.2.joint-vel-cmd
net isunlockX unlocking.0.isunlockaxis-00
net isunlockY unlocking.0.isunlockaxis-01
net isunlockZ unlocking.0.isunlockaxis-02
loadusr halshow 5axis.halshow
loadusr sim_pin isunlockX isunlockY isunlockZ
