Gearchange configuring

component gearchange "A component to choose spindle gears according to spindle speed";
pin in float speed-command;
pin in bit spindle-on;
pin in bit manual-low;
pin in bit manual-high;
pin in bit brake-enable;
pin out float motor-speed;
pin out bit low-gear;
pin out bit high-gear;
pin out bit brake;
param rw float low-ratio=3;
param rw float high-ratio=1;
param rw float max-low = 500;

author "andy pugh";
license "GPL";
function _;

;;

FUNCTION(_){
    static int old_M3;

    if (spindle_on) {
        if (!old_M3){ // spindle off to on transition
            if (manual_high) {
                high_gear = 1;
                low_gear = 0;
                brake = 0;
            }
            else if (manual_low){
                high_gear = 0;
                low_gear = 1;
                brake = 0;
            } else if (speed_command <= max_low){
                high_gear = 0;
                low_gear = 1;
                brake = 0;
            } else {
                high_gear = 1;
                low_gear = 0;
                brake = 0;
            }
        }
    } else { //spindle off
        high_gear = 0;
        low_gear = 0;
        brake = brake_enable;
    }
        
    old_M3 = spindle_on;

    if (high_gear){
        motor_speed = speed_command * high_ratio;
    } else if (low_gear){
        motor_speed = speed_command * low_ratio;
    }
}

sudo halcompile --install gearchange.comp

loadusr -W hy_vfd -d /dev/ttyUSB0
loadrt gearchange
addf gearchange.0 servo-thread
setp gearchange.0.max-low 500
setp gearchange.0.high-ratio 1
setp gearchange.0.low-ratio 3
setp gearchange.0.brake-enable 0

setp   pid.s.Pgain     [SPINDLE_9]P
setp   pid.s.Igain     [SPINDLE_9]I
setp   pid.s.Dgain     [SPINDLE_9]D
setp   pid.s.bias      [SPINDLE_9]BIAS
setp   pid.s.FF0       [SPINDLE_9]FF0
setp   pid.s.FF1       [SPINDLE_9]FF1
setp   pid.s.FF2       [SPINDLE_9]FF2
setp   pid.s.deadband  [SPINDLE_9]DEADBAND
setp   pid.s.maxoutput [SPINDLE_9]MAX_OUTPUT
setp   pid.s.error-previous-target true
setp   pid.s.maxerror .0005

net spindle-index-enable  <=> pid.s.index-enable
net spindle-enable        =>  pid.s.enable
net spindle-vel-cmd-rpm     => pid.s.command
net spindle-vel-fb-rpm      => pid.s.feedback
net spindle-output        <=  pid.s.output

# ---Encoder feedback signals/setup---

net spindle-revs             <=   hm2_5i24.0.resolver.04.position
net spindle-vel-fb-rps       <=   hm2_5i24.0.resolver.04.velocity
net spindle-index-enable     <=>  hm2_5i24.0.resolver.04.index-enable

# ---setup spindle control signals---

net spindle-vel-cmd-rps        <=  motion.spindle-speed-out-rps
net spindle-vel-cmd-rps-abs    <=  motion.spindle-speed-out-rps-abs
net spindle-vel-cmd-rpm        <=  motion.spindle-speed-out
net spindle-vel-cmd-rpm-abs    <=  motion.spindle-speed-out-abs
net spindle-enable             <=  motion.spindle-on
net spindle-cw                 <=  motion.spindle-forward
net spindle-ccw                <=  motion.spindle-reverse
net spindle-brake              <=  motion.spindle-brake
net spindle-revs               =>  motion.spindle-revs
net spindle-at-speed           =>  motion.spindle-at-speed
net spindle-vel-fb-rps         =>  motion.spindle-speed-in
net spindle-index-enable      <=>  motion.spindle-index-enable

# ---Connect the VFD---

net spindle-at-speed hy_vfd.spindle-at-speed

net machine.on.delayed hy_vfd.enable

net spindle-enable hy_vfd.spindle-on gearchange.0.spindle-on
net spindle-cw hy_vfd.spindle-forward
net spindle-ccw hy_vfd.spindle-reverse
net spindle-vel-cmd-rpm-abs gearchange.0.speed-command
net manual-low gearchange.0.manual-low <=
net manual-high gearchange.0.manual-high <=
net motor-speed gearchange.0.motor-speed => hy_vfd.speed-command

net low-gear gearchange.0.low-gear => hm2_7i84.008a.output-11
net high-gear gearchange.0.high-gear => hm2_7i84.008a.output-10
net brake gearchange.0.brake => hm2_7i84.008a.output-12

...
net spindle-enable        =>  pid.s.enable
...
net spindle-enable             <=  motion.spindle-on
...
net spindle-enable hy_vfd.spindle-on gearchange.0.spindle-on