Code:
component GB_2speed HS;
pin in bit spindle-enable-in;
pin out bit spindle-enable-out;
pin in float spindle-speed-in;
pin out float motor-speed-out;
pin out bit high-speed-spindle-enable-out;
pin in bit gear-high;
pin in bit motor-high;
pin in bit gear-A;
pin in bit gear-B;
pin in bit gear-C;
pin in bit gear-D;
pin in bit gear-E;
pin out unsigned gear-required;
pin out bit message-##[14];
license "gpl";
function _;
;;
FUNCTION(_){
int actual, message;
int i;
// These need to be edited to suit the gearbox
int speeds[15] = {0, 80, 125, 160, 215, 270, 360, 450, 595, 750, 1000, 1191, 1500, 2500, 0x1P30};
int reqd_gear[15] = {0x0, 0x01, 0x21, 0x02, 0x22, 0x04, 0x24, 0x08, 0x28, 0x10, 0x48, 0x68, 0x50, 0x70, 0x0};
if (spindle_speed_in == 0){
spindle-enable-out = 0;
high-speed-spindle-enable-out = 0;
return;
}
if (spindle_speed_in > 12000){
spindle-enable-out = 0;
high-speed-spindle-enable-out = 1;
return;
}
actual = (gear_high * 0x40 + motor_high * 0x20 + gear_A * 0x10 + gear_B * 0x08 + gear_C * 0x04 + gear_D * 0x02 + gear_E * 0x01);
for (i = 0; spindle_speed_in > speeds[i] && i < 14; i++);
gear_required = reqd_gear[i];
if (gear_required == actual && gear_required != 0x0){ // We are in the right gear
spindle_enable_out = spindle_enable_in;
motor_speed_out = speeds[13] * (spindle_speed_in / speeds[i]);
message = 0;
} else {
int j;
motor_speed_out = 0;
spindle_enable_out = 0;
for (j = 0; j < 13; j++) { message(j) = 0; }
message(i) = 1;
}
}
