first implementation of the UpdateGlobalThrust function

docs.md

@@ -119,7 +119,7 @@ Relevant peripherals:
 > note: returns 0 if the magnitude is below 0.05 m/s
 
 - `getAxis()`: returns the body-frame axis the sensor measures along
-  - returns string "x", "y", and "z"
+  - returns string "x", "y", or "z"
 
 ### Analog Transmission
 
@@ -193,3 +193,93 @@ Every cycle:
 - Altitude Sensor `getHeight()`, `getAirPressure()`, `getVerticalSpeed()`
 - Velocity Sensor `getAxis()`, `getVelocity`
 - Poll input signal strengths for global downwards thrust, and fore and aft thrust
+
+### Data Structures
+
+#### Config
+
+The member variables of the Config table is as follows:
+
+```lua
+Config = {
+  ConfigPath = "/path/to/config.txt",
+  thrusterConfigPath = "path/to/thrusters.txt",
+  Debug = false, -- controls various debug print statements
+  Monitors = {
+    InstrumentPanelMonitor = {
+      name = "peripheralName",
+      peripheral = {} -- stores the table returned by running peripheral.wrap(name)
+    },
+    AutopilotControlMonitor = {
+      name = "peripheralName",
+      peripheral = {}
+    }
+  },
+  Autopilot = {
+    AutopilotEngaged = false, -- global shutoff for all autopilot functions
+    AutoForeAft = false, -- automatic forward/rearward throttle
+    AutopilotDesiredSpeed = nil, -- keep a certain velocity
+    AutopilotDesiredHeading = nil -- maintain a certain heading
+  },
+  SensorCorrection = {
+    Heading = 0 -- this is directly added to the heading value, in case the vessel was built with the heading not being direct north
+  },
+  Throttles = {
+    fore = {
+      name = "peripheralName",
+      side = "redstoneRelaySide" -- it is assumed that a user will use a redstone relay for their throttles
+    },
+    aft = {
+      name = "peripheralName",
+      side = "redstoneRelaySide"
+    },
+    down = {
+      name = "peripheralName",
+      side = "redstoneRelaySide"
+    }
+  }
+}
+```
+
+The member variables of the sensor table is as follows:
+
+```lua
+SensorData = {
+  Velocity = {
+    Raw = {
+      x = nil,
+      y = nil,
+      z = nil
+    }
+  },
+  Altitude = {
+    Altitude = nil,
+    AirPressure = nil,
+    VerticalSpeed = nil
+  },
+  NavTable = {
+    Heading = nil,
+    HasTarget = nil,
+    -- values past this are only populated if HasTarget == true
+    TargetBearing = nil,
+    TargetClosureRate = nil,
+    TargetVerticalOffset = nil,
+    TargetRelativeAngle = nil
+  },
+  Gimbal = {
+    Angles = {
+      xAngle = nil,
+      zAngle = nil
+    },
+    AngularRates = {
+      wx = nil,
+      wy = nil,
+      wz = nil
+    },
+    LinearAcceleration = {
+      ax = nil,
+      ay = nil,
+      az = nil
+  }
+}
+```

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()