#!/usr/bin/perl

# set 4th axis spindle to either position-mode, 
#    or to velocity-mode, in degrees/second

# NB LinuxCNC passes P/Q args as floating-point which bash doesn't handle well

# G-code usage: M133 P<mode> Q<dps> ; P0 position, P1 velocity
#  e.g., M133 P1 Q80 ; start velocity-mode at 80 degrees/sec
#        M133 P0 ; set position mode, abruptly
#        sequence M133 P1 Q0; M133 P0; may be preferable

$mode=shift;
$dps=shift;

print "\nM133.pl entered: Mode:$mode Speed:$dps"."deg/sec\n";

if($mode == 0) {
  print "  M133.pl setting POSITION mode:$mode\n";
  # Mode 0 -- POSITION MODE
  #  set velocity-mode axis's velocity parameter to 0
  print "  halcmd setp stepgen.4.velocity-cmd 0\n";
  system "halcmd setp stepgen.4.velocity-cmd 0;";
  #  dissociate A-axis astep/adir from veloc-mode axis
  print "  halcmd unlinkp stepgen.4.step\n";
  system "halcmd unlinkp stepgen.4.step;";
  print "  halcmd unlinkp stepgen.4.dir\n";
  system "halcmd unlinkp stepgen.4.dir;";

  #  associate A-axis astep/adir with position-mode axis
  print "  halcmd net astep stepgen.3.step\n";
  system "halcmd net astep stepgen.3.step;";
  print "  halcmd net adir stepgen.3.dir\n";
  system "halcmd net adir stepgen.3.dir;";
} else {
  print "  M133.pl setting VELOCITY mode:$mode deg/sec:$dps\n";
  # Mode !0 -- VELOCITY MODE
  #  dissociate A-axis astep/adir from position-mode axis
  print "  halcmd unlinkp stepgen.3.step\n";
  system "halcmd unlinkp stepgen.3.step;";
  print "  halcmd unlinkp stepgen.3.dir\n";
  system "halcmd unlinkp stepgen.3.dir;";

  # NB velocity was passed-in
  #  set velocity-mode axis's velocity parameter to velocity argument
  print "halcmd setp stepgen.4.enable TRUE\n";
  system "halcmd setp stepgen.4.enable TRUE;";
  print "  halcmd setp stepgen.4.velocity-cmd $dps\n";
  system "halcmd setp stepgen.4.velocity-cmd $dps;";

  #  associate astep/adir with veloc-mode axis
  print "  halcmd net adir  stepgen.4.dir\n";
  system "halcmd net adir  stepgen.4.dir;";
  print "  halcmd net astep stepgen.4.step\n";
  system "halcmd net astep stepgen.4.step;";
}

print "M133.pl exiting\n";
exit 0;