Laser power scaling based on speed and direction

component laserreg "";

pin in   float inpower    "Desired laser power at constant speed";
pin in   float reqspd    "Requested speed, connect to motion.requested-vel";
pin in   float curspd    "Current speed, connect to motion.current-vel";
pin in   float velx    "Current speed x component, connect to joint.0.vel-cmd";
pin in   float vely    "Current speed y component, connect to joint.1.vel-cmd";
pin in   float share    "Fraction of power given to pure Y movement, eg. 0.6";
pin out  float outpower "Scaled output laser power";

function _ fp "Update the laser power";

license "GPL";

;;

#include <math.h>

FUNCTION(_) {

    outpower = inpower * (curspd / reqspd);
    
    float angle = atan2(fabs(velx), fabs(vely));  // angle away from y in radians
    float horz = angle / (0.5*M_PI);          // 1 when moving perfectly along x axis
    outpower *= share + (1-share) * horz;

    if ( outpower > inpower )
        outpower = inpower;        
    if ( outpower > 100 )
        outpower = 100;        
    if ( outpower < 0 )
        outpower = 0;

}

loadrt lincurve personality=2
loadrt laserreg

addf lincurve.0 servo-thread
addf laserreg.0    servo-thread


# Laser power mapping (0-100 -> 0-65535)
setp lincurve.0.x-val-00 0
setp lincurve.0.y-val-00 0

setp lincurve.0.x-val-01 100
setp lincurve.0.y-val-01 65535

net laser-requested-speed    motion.requested-vel    => laserreg.0.reqspd
net laser-current-speed        motion.current-vel    => laserreg.0.curspd
net laser-velx            joint.0.vel-cmd        => laserreg.0.velx
net laser-vely            joint.1.vel-cmd        => laserreg.0.vely
setp                laserreg.0.share    0.75

net laser-percent        spindle.0.speed-out    => laserreg.0.inpower
net laser-regulated-percent    laserreg.0.outpower    => lincurve.0.in
net laser-65535                lincurve.0.out          => weeny.spindle.set-speed

#<minpwr>=10
#<maxpwr>=91
#<maxspd>=800
#<minspd>=399

#<size>=6
#<margin>=2
#<stride>=[#<size>+#<margin>]

g21

#<x>=0
#<y>=0
#<pwr>=#<minpwr>
#<spd>=#<maxspd>

o101 while [#<pwr> LT #<maxpwr>]
  #<y> = 0
  #<spd>=#<maxspd>
  o110 while [#<spd> GT #<minspd>]
    g0 x#<x> y#<y>
    m3 s[#<pwr>]            
    g1 y[#<y> + #<size>] f#<spd>
    g1 x[#<x> +#<size>]
    g1 y#<y>
    g1 x#<x>
    m5
    #<y> = [#<y> + #<stride>]
    #<spd> = [#<spd> - 100]
  o110 endwhile
  #<x> = [#<x> + #<stride>]
  #<pwr> = [#<pwr> + 10]
o101 endwhile

m2