115 lines
4.0 KiB
Plaintext
115 lines
4.0 KiB
Plaintext
-- @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
|