Starting from scratch (again) - Lathe gears

component gearchange """Adjust motor speed command for geared spindles.
Scales the input value to the output depending on selected gear.""";

pin in float speed-in "Speed command input.";
pin out float speed-out "Speed command to DAC/PWM";
pin in bit sel-# [32 : personality] """Gear selection by bit. Higher
numbers take priority if more than one is set""";
pin in bit dir-in "Direction input for absolute speed + dir modes";
pin out bit dir-out "Direction output (will be reversed for negative ratios)";
pin out bit min-lim "Indicates that the minimum limitation is active";
pin out bit max-lim "Indicates that the maximum limitation is active";
pin in signed gear-number =-1 "Numeric (rather than bit) Gear selection";
pin out float speed-filtered "Speed command to display, limited and absolute";
pin out float accel-limit "Acceleration limit for the selected gear";
pin out unsigned gear "The actual detected gear";
param rw float min-#[32: personality] = 0
"Minimum allowed speed in gear range N";
param rw float max-#[32 : personality] = 100000
"Maximum allowed speed in gear range N";
param rw float acc-#[32 : personality] = 0
"Maximum Acceleration in output units/s. 0 = no limit";
param rw float scale-#[32 : personality] = 1.0 """Ratio between speed input and
the required output value""";
modparam dummy num_gears """The number of gear ratios to create in
each instance.
To create mutiple instances use multiple, comma separated values.""";

description """The output will be a  value scaled for the selected gear, and
clamped to the min/max values for that gear.

This version should work for up to 32 gears.
To load two instances, one with 5 gears and one with 6 use:

loadrt gearchange num_gears=5,6

Gears are numbered from zero, ie the count includes neutral, so typically the
modparam should be one more than the actual gear ratio count.

The Component accepts negative speeds, negative outputs and negative scales to
allow for gear ranges that reverse the motion.

The scale pin is the ratio between output value and input speed, so
the same VFD
scale parameter (typically set by the PWM scale parameter) will work for any
gear. It might be sensible to set the PWM scale to 1, in which case the
individual gear scales will be in rpm/volt.
The output value is that motor speed which would be required to give the
requested output speed with a gear ratio of 1 at the set PWM scale. For example
if gear 1 has a max speed of 1000rpm and requires 10V to the VFD to achieve
that, and gear 2 has a max speed of 4000rpm then gearchange.N.ratio2 should be
set to 4. Then a spindle speed of 1000rpm would give 10V output in gear 1 and
2.5V in gear 2.


The gear can be input either as a numeric value (possibly assembled from
\\fBweighted_sum\\fR components to convert the positions of several levers into
a gear) or as an individual select bit for each gear.""";

option count_function yes;
option extra_setup yes;

variable float old_speed = 0;

function _;
license "GPL";
;;
#include <rtapi_math.h>
#define MAX_INSTS 8
static int num_gears[MAX_INSTS];

RTAPI_MP_ARRAY_INT(num_gears, MAX_INSTS, "Gear count for each gearbox");

FUNCTION(_) {
    double speed_target;
    double delta;

    if (gear_number != -1) gear = gear_number;
    else for (gear = personality-1 ; gear > 0 && !sel(gear) ; gear--) {}

    speed_filtered = fabs(speed_in);
    if (speed_filtered > max(gear)) {
        max_lim = 1; min_lim = 0;
        speed_filtered = max(gear);}
    else if (speed_filtered != 0 && speed_filtered < min(gear)) { // zero is 0
        min_lim = 1; max_lim = 0;
        speed_filtered = min(gear);}
    else { min_lim = 0; max_lim = 0; }

    if (scale(gear) == 0) {
        speed_target = 0;}
    else {
        speed_target = speed_filtered / scale(gear) * ((speed_in < 0)? -1 : 1);}

    delta = acc(gear) * fperiod * ((speed_target > old_speed)? 1 : -1);

    if (acc(gear) != 0 && fabs(speed_target - old_speed) > fabs(delta)){
        speed_out = (old_speed += delta);}
    else{
        speed_out = (old_speed = speed_target);}

    dir_out = ((scale(gear) < 0)? !dir_in : dir_in);

    accel_limit = acc(gear);
}

EXTRA_SETUP( ) {
    if (num_gears[extra_arg] < 2) personality = 2;
    else personality = num_gears[extra_arg];
    return 0;
}

int get_count(void){
    int i;
    for (i = 0; num_gears[i] != 0 && i < MAX_INSTS; i++){}
    if (i == 0) return 1;
    return i;
}