RPM = 0 --current RPM
MAX_FREQ = 4000 --400.0 Hz
MAX_RPM = 24000 --24000 RPM

SPINDLE_START = 0x0016
SPINDLE_STOP = 0x0000

function SetSpeed(speed)
    local freq_set = (speed/MAX_RPM) * MAX_FREQ
    WriteModbusRegister("FreqSet", math.floor(freq_set))
end

function SpindleCtrl(ctrl)
    WriteModbusRegister("CtrlWord", ctrl)
end

function handle()
    local rpm_set = GetCurSpindleRPM()
	
	local _, is_online = ReadModbusRegister("StatusWord")
	
	if (not is_online) then
	    RPM = 0
		return
	end

	if (rpm_set ~= 0 and RPM == 0) then
	    SetSpeed(rpm_set)
	    SpindleCtrl(SPINDLE_START)
	elseif (rpm_set == 0 and RPM ~= 0) then
	    SetSpeed(rpm_set)
	    SpindleCtrl(SPINDLE_STOP)
	else
		SetSpeed(rpm_set)
	end
	
	RPM = rpm_set
end