106 lines
4.2 KiB
Lua
106 lines
4.2 KiB
Lua
-- @include /koptilnya/libs/wire_component.txt
|
|
-- @include /koptilnya/libs/constants.txt
|
|
-- @include /koptilnya/libs/entity.txt
|
|
require('/koptilnya/libs/wire_component.txt')
|
|
require('/koptilnya/libs/constants.txt')
|
|
require('/koptilnya/libs/entity.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 = options.Slave
|
|
self.wheel = options.Wheel
|
|
self.base = options.Base or nil
|
|
|
|
self.steer = 0
|
|
self.steeringAngle = 0
|
|
self.lateralVel = 0
|
|
self.baseLngVel = 0
|
|
self.lateralForce = 0
|
|
self.selfAligningTorque = 0
|
|
self.correctingAngle = 0
|
|
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'
|
|
outputs['SL_' .. self.order .. '_SteeringAngle'] = 'number'
|
|
outputs['SL_' .. self.order .. '_BaseLngVel'] = '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
|
|
wire.ports['SL_' .. self.order .. '_SteeringAngle'] = self.steeringAngle
|
|
wire.ports['SL_' .. self.order .. '_BaseLngVel'] = self.baseLngVel
|
|
end
|
|
|
|
function Slave:update()
|
|
if isValid(self.entity) and isValid(self.base) and isValid(self.wheel) then
|
|
if not self.entity:isFrozen() and not self.entity:isPlayerHolding() then
|
|
self.entity:setFrozen(1)
|
|
end
|
|
|
|
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 wheelLocalVel = getLocalVelocity(self.wheel)
|
|
local baseLocalVel = getLocalVelocity(self.base)
|
|
local lateralVel = wheelLocalVel[2]
|
|
local baseLngVel = baseLocalVel[1]
|
|
|
|
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
|
|
|
|
self.lateralForce = wheelFriction * wheelMass * lateralVel / UNITS_PER_METER
|
|
self.selfAligningTorque = self.lateralForce * wheelRadius
|
|
|
|
self.correctingAngle = self.selfAligningTorque / wheelInertia * math.sin(math.rad(math.abs(self.caster * 2))) *
|
|
math.sign(baseLngVel) * (self.isRight and -1 or 1)
|
|
|
|
local inc = self.steer * self.lock * self.direction * math.cos(math.rad(math.abs(self.caster * 2)))
|
|
self.steeringAngle = math.clamp(self.steeringAngle + inc + self.correctingAngle, -self.lock, self.lock)
|
|
|
|
-- 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)
|
|
|
|
self.lateralVel = lateralVel
|
|
self.baseLngVel = baseLngVel
|
|
end
|
|
end
|