first implementation of the UpdateGlobalThrust function

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()
+        Angles = {
-        AngularRates = gimbalSensor.getAngularRates()
+            xAngle = 0, -- pitch angle
-        LinearAcceleration = gimbalSensor.getLinearAcceleration()
+            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
+        elseif f == "y" then
-        if f == "y" then
             if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end
-        end
+        elseif f == "z" then
-        if 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
+    for f, v in pairs(SensorData.Gimbal.AngularRates) do
-        desiredAngluarThrustVectors[f] = CustomSigmoid(v)
+        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()