I added the ratios here and started with a very rudimentary error check on the inputs. Also the enable_in/out signal added.
Code:
component gearbox3 "LinuxCNC HAL component for Tecno's gearbox";
author "Rod Webster Hakan Bastedt";
pin in bit input_a "input a";
pin in bit input_b "input b";
pin in bit input_c "input c";
pin in bit input_d "input d";
pin in bit input_e "input e";
pin in bit higear "true if in high gear";
pin in bit enable_in "Enable signal. Input";
pin out bit enable_out "Enable signal. Output";
pin in float speed "Spindle speed in RPM as in Sxxx";
pin out bit gearshift "Need to shift gear, true if high";
pin out s32 wantedgear "Shift to this gear if told so";
pin out float speedratio "max RPM for machine divided by maximum RPM for gear";
param rw float maxrpm "maximum RPM for machine";
function _;
license "GPL";
;;
#include <rtapi_math.h>
FUNCTION(_) {
// Gears are number from 1 (lowest) to 7
static double speeds[7][2] = {
{ 160.0, 80.0},
{ 270.0, 160.0},
{ 450.0, 270.0},
{ 750.0, 450.0},
{1250.0, 750.0},
{1500.0, 1250.0},
{2500.0, 1500.0},
};
static double ratios[7] = {
0.064, 0.108, 0.18, 0.3, 0.5, 0.6, 1.0};
// Check which gear to use with this speed Sxxx
int geartouse = -1;
int i;
for (i=0; i<7; i++) {
if (speed <= speeds[i][0] && speed > speeds[i][1])
geartouse = i+1;
}
// Is at least one of the inputs active? Calculate gear from inputs.
int inputs_sum = 1*input_a + 2*input_b + 3*input_c + 4*input_d + 5*input_e;
int gearengaged;
if (higear)
gearengaged = inputs_sum + 2; // inputs_sum 4,5 => gear 6,7
else
gearengaged = inputs_sum; // inputs_sum 1,2,3,4,5 => gear 1,2,3,4,5
if (inputs_sum > 0 && geartouse == gearengaged) { // Everything ok
gearshift = 0;
wantedgear = gearengaged;
speedratio = ratios[gearengaged-1];
enable_out = enable_in;
} else { // Should change gear
gearshift = 1;
wantedgear = geartouse;
speedratio = ratios[geartouse-1];
enable_out = 0;
}
}
