Implementing better steering

koptilnya/better_steering/config/sample.txt

@@ -0,0 +1,42 @@
+-- @name Better steering sample config
+-- @author Koptilnya1337
+-- @server
+-- @include /koptilnya/better_steering/steering.txt
+require('/koptilnya/better_steering/steering.txt')
+
+Steering:new({
+    Sensitivity = 0.04,
+    Slaves = {{
+        Lock = 30,
+        Camber = -2,
+        Caster = 5,
+        Ackermann = 15.43,
+        Toe = 0,
+        Offset = 180
+    }, {
+        Lock = 30,
+        Camber = -2,
+        Caster = 5,
+        Ackermann = 15.43,
+        Toe = 0,
+        Offset = 180,
+        IsRight = true
+    }, {
+        Lock = 1,
+        Camber = -2,
+        Caster = 0,
+        Ackermann = 0,
+        Toe = 0,
+        Offset = 180,
+        Direction = -1
+    }, {
+        Lock = 1,
+        Camber = -2,
+        Caster = 0,
+        Ackermann = 0,
+        Toe = 0,
+        Offset = 180,
+        IsRight = true,
+        Direction = -1
+    }}
+})

koptilnya/better_steering/slave.txt

@@ -0,0 +1,114 @@
+-- @include /koptilnya/libs/wire_component.txt
+-- @include /koptilnya/libs/constants.txt
+require('/koptilnya/libs/wire_component.txt')
+require('/koptilnya/libs/constants.txt')
+
+Slave = class('Slave', WireComponent)
+
+function Slave:initialize(options)
+    options = options or {}
+
+    self.lock = options.Lock or 0
+    self.camber = options.Camber or 0
+    self.caster = options.Caster or 0
+    self.ackermann = options.Ackermann or 0
+    self.toe = options.Toe or 0
+    self.offset = options.Offset or 0
+    self.isRight = options.IsRight or false
+    self.direction = options.Direction or 1
+
+    self.order = options.Order or 0
+
+    self.entity = nil
+    self.wheel = nil
+    self.base = options.Base or nil
+
+    self.steer = 0
+    self.steeringAngle = 0
+    self.lateralVel = 0
+    self.lateralForce = 0
+    self.selfAligningTorque = 0
+    self.correctingAngle = 0
+end
+
+function Slave:getInputs()
+    local inputs = {}
+    inputs['SL_' .. self.order] = 'entity'
+    inputs['W_' .. self.order] = 'entity'
+
+    return inputs
+end
+
+function Slave:getOutputs()
+    local outputs = {}
+
+    outputs['SL_' .. self.order .. '_LateralForce'] = 'number'
+    outputs['SL_' .. self.order .. '_SelfAligningTq'] = 'number'
+    outputs['SL_' .. self.order .. '_CorrectingAngle'] = 'number'
+    outputs['W_' .. self.order .. '_LateralVel'] = 'number'
+
+    return outputs
+end
+
+function Slave:updateOutputs()
+    wire.ports['SL_' .. self.order .. '_LateralForce'] = self.lateralForce
+    wire.ports['SL_' .. self.order .. '_SelfAligningTq'] = self.selfAligningTorque
+    wire.ports['SL_' .. self.order .. '_CorrectingAngle'] = self.correctingAngle
+    wire.ports['W_' .. self.order .. '_LateralVel'] = self.lateralVel
+end
+
+function Slave:update()
+    self.entity = wire.ports['SL_' .. self.order]
+    self.wheel = wire.ports['W_' .. self.order]
+
+    if isValid(self.entity) and isValid(self.base) and isValid(self.wheel) then
+        local wheelMass = self.wheel:getMass()
+        local wheelInertia = self.wheel:getInertia():getLength()
+        local wheelRadius = self.wheel:getModelRadius() / UNITS_PER_METER
+        local wheelFriction = self.wheel:getFriction()
+        local lateralVel = self.wheel:worldToLocal(self.wheel:getVelocity() + self.wheel:getPos())[2]
+
+        self.lateralVel = lateralVel
+
+        if math.abs(self.steer) > 0 then
+            self.correctingAngle = 0
+        else
+            self.lateralForce = wheelFriction * wheelMass * lateralVel / TICK_INTERVAL /
+                                    UNITS_PER_METER
+            self.selfAligningTorque = self.lateralForce * wheelRadius * 2
+            self.correctingAngle = self.selfAligningTorque / wheelInertia * TICK_INTERVAL
+        end
+
+        if self.isRight then
+            self.correctingAngle = self.correctingAngle * -1
+        end
+
+        self.correctingAngle = 0
+
+        if not self.entity:isFrozen() and not self.entity:isPlayerHolding() then
+            self.entity:setFrozen(1)
+        end
+
+        local inc = self.steer * self.lock * self.direction
+        self.steeringAngle = math.clamp(self.steeringAngle + inc + self.correctingAngle, -self.lock, self.lock)
+
+        local camber = self.isRight and -self.camber or self.camber
+        local steerRatio = self.lock ~= 0 and self.steeringAngle / self.lock or 0
+        local caster = steerRatio * self.caster
+        local toe = self.isRight and self.toe or -self.toe
+        local ackermann = 0
+
+        if steerRatio > 0 then
+            ackermann = self.isRight and -steerRatio * self.ackermann or steerRatio * self.ackermann
+        elseif steerRatio < 0 then
+            ackermann = self.isRight and steerRatio * self.ackermann or -steerRatio * self.ackermann
+        end
+
+        -- Dividing ackermann by 2 because ackermann angle is difference between wheels angle
+        -- So if we don't divide it by 2, the result angle will be 2 times more than expected
+        local angle = Angle(0, self.offset + self.steeringAngle + toe + ackermann / 2, camber - caster)
+        local localizedAngle = self.base:localToWorldAngles(angle)
+
+        self.entity:setAngles(localizedAngle)
+    end
+end

koptilnya/better_steering/steering.txt

@@ -0,0 +1,79 @@
+-- @include /koptilnya/libs/wire_component.txt
+-- @include ./slave.txt
+-- @include /koptilnya/libs/table.txt
+require('/koptilnya/libs/wire_component.txt')
+require('./slave.txt')
+require('/koptilnya/libs/table.txt')
+
+Steering = class('Steering', WireComponent)
+
+function Steering:initialize(options)
+    options = options or {}
+    self.slaves = {}
+    self.steer = 0
+    self.sensitivity = options.Sensitivity or 1
+
+    for key, slave in ipairs(options.Slaves) do
+        slave.Order = key
+        slave.Base = wire.ports.Base
+        table.insert(self.slaves, Slave:new(slave))
+    end
+
+    self:_adjustPorts()
+
+    hook.add('tick', 'update', function()
+        self:update()
+    end)
+end
+
+function Steering:_adjustPorts()
+    local inputs = {}
+    local outputs = {}
+
+    inputs = table.merge(inputs, self:getInputs())
+    outputs = table.merge(outputs, self:getOutputs())
+
+    for _, slave in ipairs(self.slaves) do
+        inputs = table.merge(inputs, slave:getInputs())
+        outputs = table.merge(outputs, slave:getOutputs())
+    end
+
+    wire.adjustPorts(inputs, outputs)
+end
+
+function Steering:getInputs()
+    return {
+        TargetSteer = 'number',
+        Base = 'entity'
+    }
+end
+
+function Steering:getOutputs()
+    return {
+        Steer = 'number'
+    }
+end
+
+function Steering:updateOutputs()
+    local totalSteeringAngle = table.reduce(self.slaves, function(steer, slave)
+        return steer + slave.direction * slave.steeringAngle / (slave.lock ~= 0 and slave.lock or 1)
+    end, 0)
+
+    local steerableSlaves = table.filter(self.slaves, function(slave)
+        return slave.lock ~= 0
+    end)
+
+    wire.ports.Steer = totalSteeringAngle / #steerableSlaves
+end
+
+function Steering:update()
+    self.steer = wire.ports.TargetSteer * self.sensitivity
+
+    self:updateOutputs()
+
+    for _, slave in ipairs(self.slaves) do
+        slave.steer = self.steer
+        slave:update()
+        slave:updateOutputs()
+    end
+end