Finished grip steering

koptilnya/grip_steering/main.txt

@@ -6,11 +6,40 @@ Steering = class('Steering')
 
 require('./slave.txt')
 
+function inrange(val, min, max)
+    return val >= min and val <= max
+end
+
+function isNaN(val)
+    return val ~= val
+end
+
+function vectorIsNaN(vec)
+    return isNaN(vec[1]) or isNaN(vec[2]) or isNaN(vec[3])
+end
+
+function getLocalVelocity(ent)
+    return ent:worldToLocal((ent:getVelocity() + ent:getPos()))
+end
+
 local options = {
-    Camber = 0,
-    Caster = 85,
-    Ackermann = 10,
-    Lock = 50
+    Camber = -2,
+    Caster = 5,
+    Ackermann = 15,
+    Lock = 45,
+
+    ZeroThreshold = 2,
+
+    StartSpeedThreshold = 40,
+    EndSpeedThreshold = 150,
+    SpeedCoeff = 0.2,
+
+    StartCorrectionThreshold = 5,
+    EndCorrectionThreshold = 40,
+    CorrectionOnCoeff = 0.6,
+    CorrectionOffCoeff = 1,
+    CorrectionOnLerp = 0.3,
+    CorrectionOffLerp = 0.6
 }
 
 function Steering:_adjustPorts()
@@ -23,7 +52,8 @@ function Steering:_adjustPorts()
 
     local outputs = {
         SteerAngle = 'number',
-        TargetAngle = 'number'
+        TargetAngle = 'number',
+        CorrectionAngle = 'number'
     }
 
     wire.adjustPorts(inputs, outputs)
@@ -37,6 +67,7 @@ function Steering:_createSlaves()
         local slave = Slave:new({
             Entity = v,
             IsLeft = k % 2 == 1,
+            Offset = 180,
             Base = wire.ports.Base,
             Camber = self.slavesConfig.Camber,
             Caster = self.slavesConfig.Caster,
@@ -49,33 +80,99 @@ end
 
 function Steering:_updateOutputs()
     wire.ports.TargetAngle = self.targetAngle
+    wire.ports.SteerAngle = self.angle
+    wire.ports.CorrectionAngle = self.correction
+end
+
+function Steering:_getCorrectionGradient(speed)
+    return math.clamp(math.remap(speed, self.startCorrectionThreshold, self.endCorrectionThreshold, 0, 1), 0, 1)
+end
+
+function Steering:getBaseSpeed()
+    local MPH = self.base:getVelocity():getLength() * 3600 / 63360
+    local KPH = MPH * 1.609
+
+    return KPH
+end
+
+function Steering:_getSpeedCorrectionGradient(speed)
+    return math.clamp(math.remap(speed, self.startSpeedThreshold, self.endSpeedThreshold, 0, 1), 0, 1)
+end
+
+function Steering:_getCorrectionAngle()
+    local localVel = getLocalVelocity(self.base)
+    local forwardVel = (localVel * Vector(1, 0, 0)):getNormalized()
+    local planarVec = (localVel * Vector(1, 1, 0)):getNormalized()
+
+    forwardVel = vectorIsNaN(forwardVel) and Vector(0, 0, 0) or forwardVel
+    planarVec = vectorIsNaN(planarVec) and Vector(0, 0, 0) or planarVec
+
+    local cross = forwardVel:cross(planarVec)
+
+    return math.deg(math.acos(forwardVel:dot(planarVec))) * math.sign(localVel[2])
 end
 
 function Steering:initialize(options)
+    self:_adjustPorts()
+
     options = options or {}
 
+    self.base = wire.ports.Base
+
     self.slavesConfig = {
         Camber = options.Camber,
         Caster = options.Caster,
         Ackermann = options.Ackermann
     }
 
-    self.lock = 50
+    self.lock = options.Lock
 
     self.targetAngle = 0
     self.angle = 0
 
+    self.zeroThreshold = options.ZeroThreshold
+
+    self.speedCoeff = options.SpeedCoeff
+    self.startSpeedThreshold = options.StartSpeedThreshold
+    self.endSpeedThreshold = options.EndSpeedThreshold
+
+    self.startCorrectionThreshold = options.StartCorrectionThreshold
+    self.endCorrectionThreshold = options.EndCorrectionThreshold
+    self.correctionOnCoeff = options.CorrectionOnCoeff
+    self.correctionOffCoeff = options.CorrectionOffCoeff
+
+    self.correctionOnLerp = options.CorrectionOnLerp
+    self.correctionOffLerp = options.CorrectionOffLerp
+
+    self.correction = 0
+
     self.slaves = {}
 
-    self:_adjustPorts()
     self:_createSlaves()
 end
 
 function Steering:update()
     local horizontal = wire.ports.Axles.Horizontal.Value
+    local horizontalTarget = wire.ports.Axles.Horizontal.Target
+
+    local baseSpeedKMH = self:getBaseSpeed()
+    local angleCorrection = self:_getCorrectionGradient(baseSpeedKMH)
+    local speedCorrection = self.angle * self:_getSpeedCorrectionGradient(baseSpeedKMH) * self.speedCoeff
 
     self.targetAngle = horizontal * self.lock
-    self.angle = -horizontal * self.lock
+
+    local correctionCoeff = horizontalTarget ~= 0 and self.correctionOnCoeff or self.correctionOffCoeff
+    local correctionLerpCoeff = horizontalTarget ~= 0 and self.correctionOnLerp or self.correctionOffLerp
+    local correction = self:_getCorrectionAngle() * angleCorrection * correctionCoeff
+    self.correction = math.lerp(correctionLerpCoeff, self.correction, isNaN(correction) and 0 or correction)
+
+    local possibleAngle = math.clamp(horizontal * self.lock + speedCorrection + self.correction, -self.lock, self.lock)
+
+    if inrange(possibleAngle, -self.zeroThreshold, self.zeroThreshold) then
+        self.angle = 0
+    else
+        self.angle = possibleAngle
+    end
 
     for k, v in pairs(self.slaves) do
         v:rotate(self.angle)

koptilnya/grip_steering/slave.txt

@@ -10,6 +10,7 @@ function Slave:initialize(options)
     self.caster = options.Caster
     self.ackermann = options.Ackermann
     self.isLeft = options.IsLeft
+    self.offset = options.Offset
 end
 
 function Slave:rotate(ang)
@@ -17,9 +18,12 @@ function Slave:rotate(ang)
         self.entity:setFrozen(1)
     end
 
-    local angle = Angle(0, ang, 0)
+    local ackermann = math.sin(math.rad(ang)) ^ 2 * self.ackermann * (self.isLeft and -1 or 1)
+    local yaw = self.offset + ang + ackermann
+    local roll = (self.isLeft and 1 or -1) * self.camber - ang / 270 * self.caster
+
+    local angle = Angle(0, yaw, roll)
     local transformedAngle = self.base:localToWorldAngles(angle)
 
     self.entity:setAngles(transformedAngle)
-    -- rotate slave here
 end

koptilnya/input_system/main.txt

@@ -5,10 +5,10 @@
 -- @include ./sv_input.txt
 local Axles = {
     Horizontal = {
-        Negative = KEY.A,
-        Positive = KEY.D,
-        Gravity = 0.05,
-        Sensitivity = 0.1
+        Negative = KEY.D,
+        Positive = KEY.A,
+        Gravity = 0.1,
+        Sensitivity = 0.2
     }
 }