From ae0e728d3e365988367b26396c7215031a4e8e07 Mon Sep 17 00:00:00 2001 From: templeofshadow Date: Sat, 27 Jun 2026 17:34:10 -0500 Subject: [PATCH] first implementation of the UpdateGlobalThrust function --- main.lua | 130 +++++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 116 insertions(+), 14 deletions(-) diff --git a/main.lua b/main.lua index 5239681..a7aabc8 100644 --- a/main.lua +++ b/main.lua @@ -33,6 +33,33 @@ Thrusters = { } } -- Table of thruster tables +ThrustDirections = { + Angular = { + Negative = { + x = "pitchdown", + y = "yawport", + z = "rollport" + }, + Positive = { + x = "pitchup", + y = "yawstar", + z = "rollstar" + } + }, + Lateral = { + Negative = { + x = "star", + y = "down", + z = "aft" + }, + Positive = { + x = "port", + y = "up", + z = "fore" + } + } +} + SensorData = {} ThrusterTypes = { @@ -563,9 +590,46 @@ function PollGimbal() SensorData.Gimbal = {} - Angles = gimbalSensor.getAngles() - AngularRates = gimbalSensor.getAngularRates() - LinearAcceleration = gimbalSensor.getLinearAcceleration() + Angles = { + xAngle = 0, -- pitch angle + zAngle = 0 -- roll angle + } + for i, v in ipairs(gimbalSensor.getAngles) do + if i == 0 then + Angles.xAngle = v + elseif i == 1 then + Angles.zAngle = v + end + end + + AngularRates = { + wx = 0, -- pitch rate + wy = 0, -- yaw rate + wz = 0 -- roll rate + } + for i, v in ipairs(gimbalSensor.getAngularRates) do + if i == 0 then + AngularRates.wx = v + elseif i == 1 then + AngularRates.wy = v + elseif i == 2 then + AngularRates.wz = v + end + end + LinearAcceleration = { + ax = 0, -- port-starboard axis + ay = 0, -- up-down axis + az = 0 -- fore-aft axis + } + for i, v in ipairs(gimbalSensor.getLinearAcceleration) do + if i == 0 then + LinearAcceleration.ax = v + elseif i == 1 then + LinearAcceleration.ay = v + elseif i == 2 then + LinearAcceleration.az = v + end + end SensorData.Gimbal.Angles = Angles SensorData.Gimbal.AngularRates = AngularRates @@ -577,13 +641,18 @@ function PollGimbal() end end + +-- the sigmoid is probably the best function to use for determining how much thrust to apply based on the current velocity or angular rate function Sigmoid(x) local e = 2.718281828459045 return 1 / (1 + e^(-x)) end +-- this sigmoid is modified to return a value between -1 and 1, and inverted so that the output is negative when the input is positive, and vice versa +-- the idea is to take the velocity or angular rate, and return a value that can be used to determine how much thrust to apply in the opposite direction to counteract it +-- then, the output can be given to the update thrust function which will propogate the value to the thrusters function CustomSigmoid(x) - return ((2 * Sigmoid(x)) - 1) * -1 + return ((-2 * Sigmoid(x)) + 1) end -- thruster is a thruster type, power is a vector from 0.0 to 1.0 @@ -626,11 +695,24 @@ function SetThrusterPower(thruster, power) end end +function GetThrusterPower(thruster) + if thruster.type == "rotator" then + return thruster.power + end + if thruster.type == "thruster" then + return thruster.thruster.getPower() + end +end + function UpdateGlobalThrust() + -- for each thruster, there is a table of affectVectors that determine in what directions the thruster can apply thrust + -- each thruster has a power value from 0 to 1, which is the amount of thrust the thruster is currently applying + -- depending on the desired thrust vectors, some counteractive desired thrust vectors will be ignored + -- lateral thrust local desiredLateralThrustVectors = {} - for f, v in pairs(VelocityVectors) do + for f, v in pairs(SensorData.Velocity.Raw) do desiredLateralThrustVectors[f] = CustomSigmoid(v) end @@ -638,11 +720,9 @@ function UpdateGlobalThrust() for f, v in pairs(desiredLateralThrustVectors) do if f == "x" then if v > 0 then table.insert(thrustDirections, { port = v }) else table.insert(thrustDirections, { star = math.abs(v) }) end - end - if f == "y" then + elseif f == "y" then if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end - end - if f == "z" then + elseif f == "z" then if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end end end @@ -650,19 +730,41 @@ function UpdateGlobalThrust() for _, d in pairs(thrustDirections) do for _, t in pairs(Thrusters) do if t.affectVectors.lateral == d then - SetThrusterPower(t, d) + local correctedPower = GetThrusterPower(t) + d + if correctedPower > 1 then correctedPower = 1 end + if correctedPower < 0 then correctedPower = 0 end + SetThrusterPower(t, correctedPower) end end end -- angular thrust - local desiredAngluarThrustVectors = {} + local desiredAngularThrustVectors = {} - for f, v in pairs(AngularRates) do - desiredAngluarThrustVectors[f] = CustomSigmoid(v) + for f, v in pairs(SensorData.Gimbal.AngularRates) do + desiredAngularThrustVectors[f] = CustomSigmoid(v) end - -- Need to complete this function implementation based on docs.md requirements + for f, v in pairs(desiredAngularThrustVectors) do + if f == "wx" then + if v > 0 then table.insert(thrustDirections, { pitchdown = v }) else table.insert(thrustDirections, { pitchup = math.abs(v) }) end + elseif f == "wy" then + if v > 0 then table.insert(thrustDirections, { yawport = v }) else table.insert(thrustDirections, { yawstar = math.abs(v) }) end + elseif f == "wz" then + if v > 0 then table.insert(thrustDirections, { rollport = v }) else table.insert(thrustDirections, { rollstar = math.abs(v) }) end + end + end + + for _, d in pairs(thrustDirections) do + for _, t in pairs(Thrusters) do + if t.affectVectors.angular == d then + local correctedPower = GetThrusterPower(t) + d + if correctedPower > 1 then correctedPower = 1 end + if correctedPower < 0 then correctedPower = 0 end + SetThrusterPower(t, correctedPower) + end + end + end end function Init()