Speed Controller

local sc = nil
local sm = nil

local function enumerate()
    local pNames = peripheral.getNames()
    for index, pName in ipairs(pNames) do
        local type = peripheral.getType(pName)
        if not sc and type == "Create_RotationSpeedController" then
            sc = peripheral.wrap(pName)
        elseif not sm and type == "Create_Stressometer" then
            sm = peripheral.wrap(pName)
        end
        if sm and sc then
            return true
        end
    end
    return false
end

if not enumerate() then
    error("Failed to locate speed controller and stressometer")
    return
end

local cap = nil
local stress = nil
local nextCalibration = nil
local reverse = sc.getTargetSpeed() < 0

local function recalibrate()
    local measurements = {}

    measurements[0] = {
        speed = sc.getTargetSpeed() ,
        stress = sm.getStress()
    }
    sc.setTargetSpeed(sc.getTargetSpeed()/2)
    sleep(1)
    measurements[1] = {
        speed = sc.getTargetSpeed(),
        stress = sm.getStress()
    }
    local m = (measurements[0]["stress"] - measurements[1]["stress"])/(measurements[0]["speed"]-measurements[1]["speed"])
    local c = measurements[0]["stress"] - m*measurements[0]["speed"]
    -- y = mx + c
    -- x = (y-c)/m
    local x = (sm.getStressCapacity() - c)/m
    local target = 0
    if reverse then
        target = math.max(math.ceil(x), -256)
    else
        target = math.min(math.floor(x),256)
    end
    print("New target speed: " .. target)
    sc.setTargetSpeed(target)
end


while true do
    if sm.getStress() ~= stress or sm.getStressCapacity() ~= cap then
        -- Something has been added to the network that uses or provides stress
        print("Something changed. Due a recalibration")
        nextCalibration = os.clock() + 30
        stress = sm.getStress()
        cap = sm.getStressCapacity()
    end

    if sm.getStress() > sm.getStressCapacity() or nextCalibration and nextCalibration <= os.clock() then
        -- Calibrate the stress of the system
        nextCalibration = nil
        print("Recalibrating...")
        recalibrate()
        sleep(1)

        stress = sm.getStress()
        cap = sm.getStressCapacity()
    else
        sleep(1)
    end
end