Finished manual gearbox

koptilnya/engine/differential.txt

@@ -1,18 +1,104 @@
-Differential = class("Differential")
+-- @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 {}
+    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.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._gearbox = options.Gearbox
-  self._leftWheel = options.LeftWheel
-  self._rightWheel = options.RightWheel
+    self.avgRPM = 0
 
-  if self._gearbox then
-    self._gearbox:linkDiff(self)
-  end
-end
+    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