adjust PID kp values

docs.md

@@ -194,6 +194,12 @@ Every cycle:
 - Velocity Sensor `getAxis()`, `getVelocity`
 - Poll input signal strengths for global downwards thrust, and fore and aft thrust
 
+### Math
+
+#### Calculate Counteractive Thrust
+
+TODO: fill in
+
 ### Data Structures
 
 #### Config
@@ -241,6 +247,8 @@ Config = {
 }
 ```
 
+#### SensorData
+
 The member variables of the sensor table is as follows:
 
 ```lua
@@ -284,11 +292,22 @@ SensorData = {
 }
 ```
 
+#### Thrusters
+
 The member variables of the Thruster table are as follows (using some example thrusters):
 
 ```lua
 Thrusters = {
     gyroscopic_propeller_bearing_0 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "1", -- if you're running a standard x configuration, what prop is this equivalent to?
         type = "rotator",
         name = "gyroscopic_propeller_bearing_0",
         thruster = nil,
@@ -308,6 +327,15 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_1 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "2", -- if you're running a standard x configuration, what prop is this equivalent to?
         type = "rotator",
         name = "gyroscopic_propeller_bearing_1",
         thruster = nil,
@@ -327,6 +355,15 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_2 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "4", -- if you're running a standard x configuration, what prop is this equivalent to?
         type = "rotator",
         name = "gyroscopic_propeller_bearing_2",
         thruster = nil,
@@ -346,6 +383,15 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_3 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "3", -- if you're running a standard x configuration, what prop is this equivalent to?
         type = "rotator",
         name = "gyroscopic_propeller_bearing_3",
         thruster = nil,

main.lua

@@ -76,7 +76,7 @@ function serializeTable(val, name, skipnewlines, depth)
 
     local tmp = string.rep(" ", depth)
     if name then 
-    	if not string.match(name, '^[a-zA-z_][a-zA-Z0-9_]*$') then
+    	if not string.match(name, '^[a-zA-Z_][a-zA-Z0-9_]*$') then
     		name = string.gsub(name, "'", "\\'")
     		name = "['".. name .. "']"
     	end
@@ -153,6 +153,7 @@ end
 Config.Autopilot = {
     AutopilotEngaged = false,
     AutoForeAft = false,
+    AutopilotDesiredAltitude = nil,
     AutopilotDesiredSpeed = nil,
     AutopilotDesiredHeading = nil
 }
@@ -240,61 +241,60 @@ function PropellerInit()
     local propellers = {}
 
     for _, v in ipairs(TransmissionTypes) do
-        table.insert(transmissions, peripheral.find(v))
+        local vlist = { peripheral.find(v) }
+        if vlist ~= nil and vlist[1] ~= nil then
+            table.insert(transmissions, vlist)
+        end
     end
 
     for _, v in ipairs(PropellerTypes) do
-        table.insert(propellers, peripheral.find(v))
+        local vlist = peripheral.find(v)
+        if vlist ~= nil and vlist[1] ~= nil then
+            table.insert(propellers, vlist)
+        end
     end
 
     for pi, pv in ipairs(propellers) do 
-        for ti, tv in ipairs(transmissions) do
-            if pv.getSubnetworkAnchorId() == tv.getSelfId() then
-                if Thrusters[peripheral.getName(pv)] == nil then
-                    Thrusters[peripheral.getName(pv)] = {
-                        type = "rotator",
-                        thruster = pv,
-                        transmission = tv,
-                        --[[
-                            NOTE:
-                            affectVectors will have values depending on how the thruster's 
-                            thrust vector interacts with the vessel's attitude
-                                angular:
-                            possible directions for angular.yaw: "port"|"star"
-                            possible directions for angular.roll: "port"|"star"
-                            possible directions for angular.pitch: "up"|"down"
-                                lateral:
-                            possible directions for lateral.x: "port"|"star"
-                            possible directions for lateral.y: "up"|"down"
-                            possible directions for lateral.z: "fore"|"aft"
-                        ]]
-                        affectVectors = {
-                            angular = {
-                                yaw = nil,
-                                pitch = nil,
-                                roll = nil
+        if Thrusters[peripheral.getName(pv)] == nil then
+            for _, tv in pairs(transmissions) do
+                if pv.getSourceId() == tv.getSelfId() then
+                    if Thrusters[peripheral.getName(pv)] == nil then
+                        Thrusters[peripheral.getName(pv)] = {
+                            type = "rotator",
+                            thruster = pv,
+                            transmission = peripheral.wrap(tv),
+                            --[[
+                                NOTE:
+                                affectVectors will have values depending on how the thruster's 
+                                thrust vector interacts with the vessel's attitude
+                                    angular:
+                                possible directions for angular.yaw: "port"|"star"
+                                possible directions for angular.roll: "port"|"star"
+                                possible directions for angular.pitch: "up"|"down"
+                                    lateral:
+                                possible directions for lateral.x: "port"|"star"
+                                possible directions for lateral.y: "up"|"down"
+                                possible directions for lateral.z: "fore"|"aft"
+                            ]]
+                            affectVectors = {
+                                angular = {
+                                    yaw = nil,
+                                    pitch = nil,
+                                    roll = nil
+                                },
+                                lateral = {
+                                    x = nil,
+                                    y = nil,
+                                    z = nil
+                                }
                             },
-                            lateral = {
-                                x = nil,
-                                y = nil,
-                                z = nil
-                            }
-                        },
-                        power = (tv.getSignal())/15
-                    }
-                end
-                if Thrusters[peripheral.getName(pv)] ~= nil then
-                    if Thrusters[peripheral.getName(pv)].type == nil then 
-                        Thrusters[peripheral.getName(pv)].type = "rotator" 
+                            power = (tv.getSignal())/15
+                        }
                     end
-                    Thrusters[peripheral.getName(pv)].thruster = pv
-                    Thrusters[peripheral.getName(pv)].transmission = tv
-                    if Thrusters[peripheral.getName(pv)].affectVectors == nil then 
-                        Thrusters[peripheral.getName(pv)].affectVectors = {} 
-                    end
-                    Thrusters[peripheral.getName(pv)].power = (tv.getSignal())/15
                 end
             end
+        elseif Config.Debug then
+            print("DEBUG: thruster of name "..peripheral.getName(pv).." already indexed, skipping (called by PropellerInit)")
         end
     end
 end
@@ -303,7 +303,10 @@ function ThrusterInit()
     local thrusters = {}
 
     for _, v in ipairs(ThrusterTypes) do
-        table.insert(thrusters, peripheral.find(v))
+        local vlist = peripheral.find(v)
+        if vlist ~= nil and vlist[1] ~= nil then
+            table.insert(thrusters, vlist)
+        end
     end
 
     for ti, tv in ipairs(thrusters) do
@@ -339,22 +342,90 @@ function ThrusterInit()
                 },
                 power = tv.getPower()
             }
+        elseif Config.Debug then
+            print("DEBUG: thruster of name "..peripheral.getName(tv).." already indexed, skipping (called by ThrusterInit)")
         end
-        if Thrusters[peripheral.getName(tv)] ~= nil then
-            if Thrusters[peripheral.getName(tv)].type == nil then 
-                Thrusters[peripheral.getName(tv)].type = "thruster" 
+    end
+end
+
+-- this is for populating nil values from thruster config files
+local function populateThrusterValues()
+    for _, thruster in pairs(Thrusters) do
+        if thruster.name == nil and thruster.thruster == nil then -- as long as we have one of thruster or name, we can fetch other data
+            print("ERROR: Thruster ".._.." has no name or thruster object. Removing entry in thruster list")
+            Thrusters[_] = nil
+            return
+        end
+        if thruster.name == nil and thruster.thruster ~= nil then
+            thruster.name = peripheral.getName(thruster.thruster)
+        end
+        
+        if thruster.thruster == nil and thruster.name ~= nil then
+            thruster.thruster = peripheral.wrap(thruster.name)
+        end
+        if thruster.type == nil then
+            if Config.Debug then print("DEBUG: Thruster "..thruster.name.." does not have a type.") end
+            for _, tt in ipairs(ThrusterTypes) do
+                if peripheral.getType(thruster.thruster) == tt then
+                    if Config.Debug then print("DEBUG: Detected thruster "..thruster.name.." is of type 'thruster'.") end
+                    thruster.type = "thruster"
+                    break
+                end
             end
-            Thrusters[peripheral.getName(tv)].thruster = tv
-            Thrusters[peripheral.getName(tv)].transmission = nil
-            if Thrusters[peripheral.getName(tv)].affectVectors == nil then 
-                Thrusters[peripheral.getName(tv)].affectVectors = {} 
+
+            for _, pt in ipairs(PropellerTypes) do
+                if peripheral.getType(thruster.thruster) == pt then
+                    if Config.Debug then print("DEBUG: Detected thruster "..thruster.name.." is of type 'rotator'.") end
+                    thruster.type = "rotator"
+                    break
+                end
+            end
+        end
+        if thruster.type == "rotator" and thruster.transmission == nil then
+            if Config.Debug then print("DEBUG: Thruster "..thruster.name.." does not have an associated transmission.") end
+            local subnetworkId = peripheral.wrap(thruster.name).getSourceId()
+            for _, transmissionType in ipairs(TransmissionTypes) do
+                local transmissions = { peripheral.find(transmissionType) }
+                if transmissions ~= nil and transmissions[1] ~= nil then
+                    if Config.Debug then print("DEBUG: transmission of type "..transmissionType.." found.") end
+                    for _, transmission in ipairs(transmissions) do
+                        local thisTransmissionId = transmission.getSelfId()
+                        if Config.Debug then print("DEBUG: Comparing subnetwork IDs values: "..thisTransmissionId.." == "..subnetworkId) end
+                        if transmission.getSelfId() == subnetworkId then
+                            if Config.Debug then print("DEBUG: Detected that thruster "..thruster.name.." is connected to transmission "..peripheral.getName(transmission)) end
+                            thruster.transmission = peripheral.getName(transmission)
+                            break
+                        elseif Config.Debug then
+                            print("DEBUG: Comparison inequal")
+                        end
+                    end
+                elseif Config.Debug then
+                    print("DEBUG: No transmissions of type "..transmissionType.." (print from populateThrusterValues)")
+                end
+            end
+        end
+        if thruster.power == nil then
+            if Config.Debug then print("DEBUG: Thruster "..thruster.name..": thruster power is not populated") end
+            if thruster.type == "thruster" then
+                thruster.power = thruster.thruster.getPower()
+            elseif thruster.type == "rotator" then
+                if thruster.transmission ~= nil then
+                    local transmissionType = peripheral.getType(peripheral.wrap(thruster.transmission))
+                    if transmissionType == "analog_transmission" then
+                        thruster.power = (peripheral.wrap(thruster.transmission).getSignal())/15
+                    elseif transmissionType == "Create_RotationSpeedController" then
+                        thruster.power = (peripheral.wrap(thruster.transmission).getTargetSpeed())/256
+                    end
+                else
+                    print("ERROR: Thruster "..peripheral.getName(thruster.thruster).." has no transmission.")
+                end
             end
-            Thrusters[peripheral.getName(tv)].power = tv.getPower()
         end
     end
 end
 
 function Update()
+    populateThrusterValues()
     PropellerInit()
     ThrusterInit()
 end
@@ -453,8 +524,8 @@ local function partiallyUpdateThrusters(thrusterList)
                 for _, tt in ipairs(TransmissionTypes) do 
                     local transmissions = peripheral.find(tt)
                     for _, t in ipairs(transmissions) do
-                        if tv.getSubnetworkAnchorId() == t.getSelfId() then
-                            thisThruster.transmission = t
+                        if tv.getSourceId() == t.getSelfId() then
+                            thisThruster.transmission = peripheral.getName(t)
                         end
                     end
                 end
@@ -486,7 +557,7 @@ function PollVelocity()
     -- Velocity Sensors
     VelocityVectors = {}
 
-    if velSensors[0] == nil then -- verify array is not empty
+    if velSensors[1] ~= nil then -- verify array is not empty
 
         SensorData.Velocity = {}
 
@@ -494,15 +565,28 @@ function PollVelocity()
             
             local vsAxis = velsensor.getAxis()
             local vsVelocity = velsensor.getVelocity()
+            
+            -- correct for heading correction value
+            if Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
+                if vsAxis == "x" then vsAxis = "z" end
+                if vsAxis == "z" then vsAxis = "x" end
+            elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
+                vsVelocity = -vsVelocity
+            elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+                if vsAxis == "x" then vsAxis = "z" end
+                if vsAxis == "z" then vsAxis = "x" end
+                vsVelocity = -vsVelocity
+            end
+
             VelocityVectors[vsAxis] = vsVelocity
             
             SensorData.Velocity.Raw = VelocityVectors
-
-            if Config.Debug then
-                print("DEBUG: PollVelocity fetched sensor data: "..tableToString(SensorData.Velocity))
-            end
             
         end
+
+        if Config.Debug then
+            print("DEBUG: PollVelocity fetched sensor data: "..tableToString(SensorData.Velocity))
+        end
     else
         if Config.Debug then
             print("DEBUG: No velocity sensors found.") 
@@ -593,13 +677,27 @@ function PollGimbal()
             zAngle = 0  -- roll angle
         }
         for i, v in ipairs(gimbalSensor.getAngles()) do
-            if i == 0 then
+            if i == 1 then
                 Angles.xAngle = v
-            elseif i == 1 then
+            elseif i == 2 then
                 Angles.zAngle = v
             end
         end
 
+        -- correct for heading correction value
+        if Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
+            local temp = Angles.xAngle
+            Angles.xAngle = Angles.zAngle
+            Angles.zAngle = temp
+        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
+            Angles.xAngle = -Angles.xAngle
+            Angles.zAngle = -Angles.zAngle
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+            local temp = Angles.xAngle
+            Angles.xAngle = -Angles.zAngle
+            Angles.zAngle = -temp
+        end
+
         AngularRates = {
             wx = 0, -- pitch rate
             wy = 0, -- yaw rate
@@ -614,6 +712,21 @@ function PollGimbal()
                 AngularRates.wz = v
             end
         end
+
+        -- correct for heading correction value
+        if Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
+            local temp = AngularRates.wx
+            AngularRates.wx = AngularRates.wz
+            AngularRates.wz = temp
+        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
+            AngularRates.wx = -AngularRates.wx
+            AngularRates.wz = -AngularRates.wz
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+            local temp = AngularRates.wx
+            AngularRates.wx = -AngularRates.wz
+            AngularRates.wz = -temp
+        end
+
         LinearAcceleration = {
             ax = 0, -- port-starboard axis
             ay = 0, -- up-down axis
@@ -629,12 +742,26 @@ function PollGimbal()
             end
         end
 
+        -- correct for heading correction value
+        if Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
+            local temp = LinearAcceleration.ax
+            LinearAcceleration.ax = LinearAcceleration.az
+            LinearAcceleration.az = temp
+        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
+            LinearAcceleration.ax = -LinearAcceleration.ax
+            LinearAcceleration.az = -LinearAcceleration.az
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+            local temp = LinearAcceleration.ax
+            LinearAcceleration.ax = -LinearAcceleration.az
+            LinearAcceleration.az = -temp
+        end
+
         SensorData.Gimbal.Angles = Angles
         SensorData.Gimbal.AngularRates = AngularRates
         SensorData.Gimbal.LinearAcceleration = LinearAcceleration
 
         if Config.Debug then
-            print("DEBUG: PollGimbaal fetched sensor data: "..tableToString(SensorData.Gimbal))
+            print("DEBUG: PollGimbal fetched sensor data: "..tableToString(SensorData.Gimbal))
         end
     end
 end
@@ -655,10 +782,24 @@ end
 
 -- thruster is a thruster type, power is a vector from 0.0 to 1.0
 function SetThrusterPower(thruster, power)
+    if thruster == nil then
+        print("ERROR: SetThrusterPower called with nil thruster.")
+        return
+    end
+
+    if power > 1.0 then power = 1.0 elseif power < 0.0 then power = 0.0 end
+
+    --if Config.Debug then print("DEBUG: thruster "..thruster.name..": power "..power) end
+
     if thruster.type == "rotator" then
         thruster.power = power
         local actualPower = 0
-        if peripheral.getType(thruster.transmission) == "Create_RotationSpeedController" then
+        if thruster.transmission == nil then
+            print("ERROR: SetThrusterPower called with nil transmission for rotator thruster.")
+            print("Thruster: "..thruster.name)
+            return
+        end
+        if peripheral.getType(peripheral.wrap(thruster.transmission)) == "Create_RotationSpeedController" then
             if (math.ceil(power*256) - power*256 <= 0.5) then
                 actualPower = math.ceil(power*256)
             end
@@ -668,7 +809,7 @@ function SetThrusterPower(thruster, power)
 
             thruster.transmission.setTargetSpeed(actualPower)
         end
-        if peripheral.getType(thruster.transmission) == "analog_transmission" then
+        if peripheral.getType(peripheral.wrap(thruster.transmission)) == "analog_transmission" then
             if (math.ceil(power*15) - power*15) <= 0.5 then
                 actualPower = math.ceil(power*15)
             end
@@ -678,16 +819,16 @@ function SetThrusterPower(thruster, power)
 
             if actualPower < 1 then
                 actualPower = 15 -- signal 15 will decouple the speed and stop the motion
-            end
-            if actualPower <= 15 then
+            elseif actualPower <= 15 then
                 actualPower = actualPower - 1
             end
 
-            thruster.transmission.setSignal(actualPower)
+            peripheral.wrap(thruster.transmission).setSignal(actualPower)
+
+            -- if Config.Debug then print("DEBUG: actualPower: "..actualPower) end
         end
         
-    end
-    if thruster.type == "thruster" then
+    elseif thruster.type == "thruster" then
         thruster.power = power
         thruster.thruster.setPowerNormalized(power)
     end
@@ -702,80 +843,173 @@ function GetThrusterPower(thruster)
     end
 end
 
+-- PID systems for stabilization and autopilot
+LastTime = os.epoch("utc")
+
+function GetDeltaTime()
+    local currentTime = os.epoch("utc")
+    local dt = (currentTime - LastTime) / 1000 -- convert milliseconds to seconds
+    LastTime = currentTime
+    if dt <= 0 then dt = 0.001 end -- prevent division by zero
+    return dt
+end
+
+function CreatePID(kp, ki, kd)
+    return {
+        kp = kp, 
+        ki = ki, 
+        kd = kd,
+        integral = 0, lastError = 0,
+
+        minOutput = 0.0,
+        maxOutput = 32.0,
+        update = function(self, setpoint, pv, dt)
+            if dt <= 0 then return 0.0 end
+            
+            local error = setpoint - pv
+
+            local P = self.kp * error
+            
+            local D = self.kd * ((error - self.lastError) / dt)
+
+            local potential_i = self.integral + (self.ki * error * dt)
+
+            local raw_output = P + potential_i + D
+
+            if Config.Debug then print("DEBUG: PID raw output: "..raw_output) end
+
+            local clampedOutput = 0
+
+            if raw_output >= self.minOutput and raw_output <= self.maxOutput then
+                self.integral = potential_i
+                clampedOutput = raw_output
+            else
+                clampedOutput = math.max(self.minOutput, math.min(raw_output, self.maxOutput))
+            end
+
+            self.lastError = error
+
+            return clampedOutput
+        end
+    }
+end
+
+-- tune these for the best values for each vessel
+PIDs = {
+-- angular
+    -- attitude PIDs
+    RollAttitudePID = CreatePID(3.0, 0.01, 0.05),
+    PitchAttitudePID = CreatePID(3.0, 0.01, 0.05),
+    YawAttitudePID = CreatePID(0.1, 0.01, 0.05),
+
+    -- rate PIDs
+    RollRatePID = CreatePID(0.8, 0.01, 0.05),
+    PitchRatePID = CreatePID(0.8, 0.01, 0.05),
+    YawRatePID = CreatePID(0.5, 0.01, 0.05),
+
+-- lateral
+    -- value PIDs
+    AltitudePID = CreatePID(0.6, 0.01, 0.05),
+    PortStarPID = CreatePID(0.1, 0.01, 0.05),
+    ForeAftPID  = CreatePID(0.1, 0.01, 0.05),
+
+    -- rate PIDs
+    VerticalRatePID = CreatePID(0.8, 0.01, 0.05),
+    PortStarRatePID = CreatePID(0.1, 0.01, 0.05),
+    ForeAftRatePID  = CreatePID(0.1, 0.01, 0.05)
+}
+
+-- min and max thrust for thrust normalization
+local minThrust = math.huge
+local maxThrust = -math.huge
+
+local function normalize(val, min, max)
+    if min == max then return 0.5 end
+    return (val - min) / (max - min)
+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
-    UpdateGlobalLateralThrust()
+    local dt = GetDeltaTime() / 1000
 
-    -- angular thrust
-    UpdateGlobalAngularThrust()
-end
-
-function UpdateGlobalLateralThrust() 
-    -- lateral thrust
-    -- verify that SensorData.Velocity.Raw is not nil
-    if SensorData.Velocity == nil or SensorData.Velocity.Raw == nil then
-        if Config.Debug then
-            print("DEBUG: SensorData.Velocity.Raw or SensorData.Velocity is nil, skipping UpdateGlobalLateralThrust")
-        end
+    if SensorData.Gimbal == nil or SensorData.Altitude == nil or SensorData.Velocity == nil then
+        print("ERROR: One or more of SensorData is nil. Cannot update thrust.")
         return
     end
+    
+    local AltitudeRateOutput = PIDs.VerticalRatePID:update(Config.Autopilot.AutopilotDesiredAltitude, SensorData.Altitude.Altitude, dt)
+    local throttleOutput     = PIDs.AltitudePID:update(AltitudeRateOutput, SensorData.Altitude.VerticalSpeed, dt)
+    if Config.Debug then print("DEBUG: Throttle Output: "..throttleOutput) end
 
-    -- TODO: Refactor this
+    local pitchOutput        = PIDs.PitchAttitudePID:update(0, SensorData.Gimbal.Angles.xAngle, dt)
+    local pitchRateOutput    = PIDs.PitchRatePID:update(pitchOutput, SensorData.Gimbal.AngularRates.wx or 0, dt)
+    if Config.Debug then print("DEBUG: Pitch Output: "..pitchRateOutput) end
 
-    local desiredLateralThrustVectors = {}
+    local rollOutput         = PIDs.RollAttitudePID:update(0, SensorData.Gimbal.Angles.zAngle, dt)
+    local rollRateOutput     = PIDs.RollRatePID:update(rollOutput, SensorData.Gimbal.AngularRates.wz or 0, dt)
+    if Config.Debug then print("DEBUG: Roll Output: "..rollRateOutput) end
 
-    for f, v in pairs(SensorData.Velocity.Raw) do
-        desiredLateralThrustVectors[f] = CustomSigmoid(v)
-    end
+    local yawOutput = 5
+    if Config.Autopilot.AutopilotDesiredHeading ~= nil then yawOutput = PIDs.YawAttitudePID:update(Config.Autopilot.AutopilotDesiredHeading, SensorData.NavTable.Heading, dt) end
+    local yawRateOutput      = PIDs.YawRatePID:update(Config.Autopilot.AutopilotDesiredYawRate or 0, SensorData.Gimbal.AngularRates.wy or 0, dt)
 
-    --[[
-        desiredLateralThrustVectors is a table that is similar to SensorData.Velocity.Raw, but with values between -1 and 1.
-        essentially, it reverses the velocity vector and then clamps the range between -1 and 1, so that the value can be used for thrust power
-        
-    ]]
+    for _, thruster in pairs(Thrusters) do
+        local desiredThrust = 0
 
-    local thrustDirections = {}
-    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
-        elseif f == "y" then
-            if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end
-        elseif f == "z" then
-            if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end
+        if thruster.x_config_equivalent ~= nil then
+            
+            -- Calculate desired thrust based on affectVectors and PID outputs
+            if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then
+                if thruster.affectVectors.angular.pitch == "up" then
+                    desiredThrust = desiredThrust + pitchRateOutput
+                else
+                    desiredThrust = desiredThrust - pitchRateOutput
+                end
+            end
+            
+            if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
+                if thruster.affectVectors.angular.roll == "port" then
+                    desiredThrust = desiredThrust + rollRateOutput
+                else
+                    desiredThrust = desiredThrust - rollRateOutput
+                end
+                
+            end
+            
+            if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
+                if thruster.affectVectors.angular.yaw == "port" then
+                    desiredThrust = desiredThrust + yawRateOutput
+                else
+                    desiredThrust = desiredThrust - yawRateOutput
+                end
+            end
+
+            if thruster.affectVectors.lateral.y == "up" then
+                desiredThrust = desiredThrust + throttleOutput 
+            end
+
+            --if Config.Debug then print("DEBUG: "..thruster.name..": desiredThrust "..desiredThrust.." (from UpdateGlobalThrust)") end
+
+            SetThrusterPower(thruster, desiredThrust)
+        else
+            if Config.Debug then print("DEBUG: Thruster has x_config_equivalent set") end
+
+            local prop1 = throttleOutput + pitchRateOutput + rollRateOutput -- front-left
+            local prop2 = throttleOutput + pitchRateOutput - rollRateOutput -- front-right
+            local prop3 = throttleOutput - pitchRateOutput - rollRateOutput -- back-right
+            local prop4 = throttleOutput - pitchRateOutput + rollRateOutput -- back-left
+
+            if     thruster.x_config_equivalent == "1" then SetThrusterPower(thruster, prop1)
+            elseif thruster.x_config_equivalent == "2" then SetThrusterPower(thruster, prop2)
+            elseif thruster.x_config_equivalent == "3" then SetThrusterPower(thruster, prop3)
+            elseif thruster.x_config_equivalent == "4" then SetThrusterPower(thruster, prop4)
+            else print("ERROR: Thruster "..thruster.name.." has invalid x_config_equivalent value")
+            end
         end
     end
-
-    --[[
-        thrustDirections is an array of tables, each table has a key that is the direction of the desired thrust, and a value that is the magnitude of the desired thrust
-        like so:
-        thrustDirections = {
-            { port = 0.5 },
-            { down = 0.1 },
-            { aft = 0.6 }
-        }
-        there will only ever be three tables in thrustDirections, as there are only three axes of movement, and each axis can only have one desired thrust direction at a time
-        (angular thrust is the same, but with different keys for the tables, like pitchup/pitchdown, yawport/yawstar, rollport/rollstar)
-        
-    ]]
-
-
-end
-
-function UpdateGlobalAngularThrust()
-    -- angular thrust
-    -- verify that SensorData.Gimbal.AngularRates is not nil
-    if SensorData.Gimbal == nil or SensorData.Gimbal.AngularRates == nil then
-        if Config.Debug then
-            print("DEBUG: SensorData.Gimbal.AngularRates or SensorData.Gimbal is nil, skipping UpdateGlobalAngularThrust")
-        end
-        return
-    end
-
-    -- TODO: Implement this
 end
 
 function Init()
@@ -825,7 +1059,10 @@ function Init()
         end
     end
 
-    -- If we're here, the config loaded successfully and the initialization is done.
+    -- If we're here, the config loaded successfully.
+
+    populateThrusterValues()
+    Update()
 end
 
 -- Display functions for monitors
@@ -845,9 +1082,9 @@ function displayInstrumentPanel(monitor, sensorData)
     if next(sensorData) ~= nil then
         if sensorData then
             m.setCursorPos(1, 3)
-            m.write("Pitch:   " .. string.format("%.2f", SensorData.Gimbal.Angles[0] or 0))
+            m.write("Pitch:   " .. string.format("%.2f", SensorData.Gimbal.Angles.xAngle or 0))
             m.setCursorPos(1, 4)
-            m.write("Roll:    " .. string.format("%.2f", SensorData.Gimbal.Angles[1] or 0))
+            m.write("Roll:    " .. string.format("%.2f", SensorData.Gimbal.Angles.zAngle or 0))
             m.setCursorPos(1, 5)
             m.write("Yaw:     " .. string.format("%.2f", SensorData.NavTable.Heading or 0))
             m.setCursorPos(1, 6)
@@ -926,6 +1163,12 @@ function collectSensorData()
 end
 
 function WriteConfigFiles()
+    -- since the thruster object will change every time, set each thruster's thruster object to nil before writing to the config file, so that the config file only contains the thruster names and not the thruster objects
+    for _, thruster in pairs(Thrusters) do
+        thruster.thruster = nil
+        thruster.transmission = nil
+    end
+
     print("Writing to thruster config file.")
     local thrusterConfigFile = fs.open(Config.thrusterConfigPath, "w+")
     thrusterConfigFile.write(tableToString(Thrusters))
@@ -940,13 +1183,13 @@ end
 function Main()
     Init()
 
+    os.sleep(5) -- pause for any initialization errors to display
+
     local sentinel = true
     print("Mainloop starting. Press 'q' to stop the loop and save configuration changes.")
     while sentinel do
 
         PollSensors()
-        
-        UpdateStabilization()
         UpdateGlobalThrust()
 
         -- Update monitor displays
@@ -958,17 +1201,10 @@ function Main()
             displayAutopilotControls(Config.Monitors.AutopilotControlMonitor, SensorData)
         end
 
-        local unindexedThrusters = checkIfThrusterIsIndexed()
-        if unindexedThrusters ~= nil then
-            partiallyUpdateThrusters(unindexedThrusters)
-        end
-
         local function getTerminateEvent()
             local event, key, is_held = os.pullEventRaw("key")
             if keys.getName(key) == "q" then 
                 print("Quit Input Received") 
-                
-                WriteConfigFiles()
 
                 sentinel = false
             else
@@ -980,6 +1216,17 @@ function Main()
 
         parallel.waitForAny(do_sleep, getTerminateEvent)
     end
+
+    for _, t in pairs(Thrusters) do
+        if t.type == "rotator" then
+            SetThrusterPower(t, 0.0)
+            peripheral.wrap(t.transmission).releaseSignal() -- release control of transmissions on quit
+        else
+            SetThrusterPower(t, 0.0)
+        end
+    end
+    
+    WriteConfigFiles()
 end
 
 -- Run the main function when the script starts

thrusters.txt.example

@@ -1,53 +1,105 @@
 return {
     gyroscopic_propeller_bearing_0 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "1",
         type = "rotator",
         name = "gyroscopic_propeller_bearing_0",
         thruster = nil,
         transmission = nil,
         affectVectors = {
-            yaw = nil,
-            pitch = "up",
-            roll = "star",
-            lateral = nil
+            angular = {
+              yaw = nil,
+              pitch = "up",
+              roll = "star",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_1 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "2",
         type = "rotator",
         name = "gyroscopic_propeller_bearing_1",
         thruster = nil,
         transmission = nil,
         affectVectors = {
-            yaw = nil,
-            pitch = "up",
-            roll = "port",
-            lateral = nil
+            angular = {
+              yaw = nil,
+              pitch = "up",
+              roll = "port",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_2 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "4",
         type = "rotator",
         name = "gyroscopic_propeller_bearing_2",
         thruster = nil,
         transmission = nil,
         affectVectors = {
-            yaw = nil,
-            pitch = "down",
-            roll = "port",
-            lateral = nil
+            angular = {
+              yaw = nil,
+              pitch = "down",
+              roll = "port",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_3 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
+        x_config_equivalent = "3",
         type = "rotator",
         name = "gyroscopic_propeller_bearing_3",
         thruster = nil,
         transmission = nil,
         affectVectors = {
-            yaw = nil,
-            pitch = "down",
-            roll = "star",
-            lateral = nil
+            angular = {
+              yaw = nil,
+              pitch = "down",
+              roll = "star",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     }