105 lines
3.3 KiB
Plaintext
105 lines
3.3 KiB
Plaintext
-- @include ../libs/constants.txt
|
|
-- @include ../libs/table.txt
|
|
require('../libs/constants.txt')
|
|
require('../libs/table.txt')
|
|
|
|
Differential = class('Differential')
|
|
|
|
function Differential:initialize(options)
|
|
options = options or {}
|
|
|
|
self.power = options.Power or 1
|
|
self.coast = options.Coast or 1
|
|
self.preload = options.Preload or 10
|
|
self.viscousCoeff = options.ViscousCoeff or 0.9
|
|
self.usePowerBias = options.UsePowerBias or 10
|
|
self.axle = options.Axle or Vector(1, 0, 0)
|
|
self.distributionCoeff = options.DistributionCoeff or 1
|
|
|
|
self.avgRPM = 0
|
|
|
|
self._leftWheel = nil
|
|
self._rightWheel = nil
|
|
|
|
self.lwInertia = Vector(0)
|
|
self.rwInertia = Vector(0)
|
|
|
|
self.lwav = 0
|
|
self.rwav = 0
|
|
|
|
self.lwtq = 0
|
|
self.rwtq = 0
|
|
|
|
self.limitTorque = 250
|
|
end
|
|
|
|
function Differential:update()
|
|
if isValid(self._leftWheel) and isValid(self._rightWheel) then
|
|
self.lwav = math.rad(self._leftWheel:getAngleVelocity()[2])
|
|
self.rwav = math.rad(self._rightWheel:getAngleVelocity()[2]) * -1
|
|
|
|
self.avgRPM = (self.lwav + self.rwav) / 2 * RAD_TO_RPM
|
|
|
|
-- RAV = relative anglar velocity
|
|
local lwrav = self.rwav - self.lwav
|
|
local rwrav = self.lwav - self.rwav
|
|
|
|
local lwtq = lwrav * self.lwInertia
|
|
local rwtq = rwrav * self.rwInertia
|
|
|
|
-- Calculating damping torque here
|
|
local lwDampingTorque = self.viscousCoeff * lwtq
|
|
local rwDampingTorque = self.viscousCoeff * rwtq
|
|
|
|
-- Whether use diff power or diff coast
|
|
local usePower = self._gearbox and self._gearbox.torque > self.usePowerBias or false
|
|
local usedCoeff = usePower and self.power or self.coast
|
|
|
|
local lwPowerCoeff = usedCoeff
|
|
local rwPowerCoeff = usedCoeff
|
|
|
|
-- this could probably be shortened?
|
|
-- if lwtq - rwtq > self.preload then
|
|
-- lwPowerCoeff = usedCoeff
|
|
-- rwPowerCoeff = usedCoeff
|
|
-- end
|
|
|
|
-- if rwtq - lwtq > self.preload then
|
|
-- lwPowerCoeff = usedCoeff
|
|
-- rwPowerCoeff = usedCoeff
|
|
-- end
|
|
|
|
local lwCorrectingTorque = math.clamp(lwDampingTorque * lwPowerCoeff, -self.limitTorque, self.limitTorque)
|
|
local rwCorrectingTorque = math.clamp(rwDampingTorque * rwPowerCoeff, -self.limitTorque, self.limitTorque)
|
|
|
|
local gboxTorque = self._gearbox and self._gearbox.torque or 0 -- 6289.245
|
|
|
|
local lwCorrectedTorque = lwCorrectingTorque * 600 + gboxTorque * self.distributionCoeff * 52.49
|
|
local rwCorrectedTorque = rwCorrectingTorque * 600 + gboxTorque * self.distributionCoeff * 52.49
|
|
|
|
self.lwtq = lwCorrectingTorque
|
|
self.rwtq = rwCorrectingTorque
|
|
|
|
self._leftWheel:applyTorque(lwCorrectedTorque * -self._leftWheel:getRight() * TICK_INTERVAL)
|
|
self._rightWheel:applyTorque(rwCorrectedTorque * self._rightWheel:getRight() * TICK_INTERVAL)
|
|
end
|
|
end
|
|
|
|
function Differential:linkWheels(left, right)
|
|
if isValid(left) and isValid(right) then
|
|
self._leftWheel = left
|
|
self._rightWheel = right
|
|
|
|
self.lwInertia = left:getInertia():getLength()
|
|
self.rwInertia = right:getInertia():getLength()
|
|
end
|
|
end
|
|
|
|
function Differential:linkGearbox(gbox)
|
|
if gbox ~= nil then
|
|
self._gearbox = gbox
|
|
|
|
gbox:linkDiff(self)
|
|
end
|
|
end
|