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
@@ -283,3 +291,124 @@ 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,
+        transmission = nil,
+        affectVectors = {
+            angular = {
+              yaw = nil,
+              pitch = "up",
+              roll = "star",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
+        },
+        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,
+        transmission = nil,
+        affectVectors = {
+            angular = {
+              yaw = nil,
+              pitch = "up",
+              roll = "port",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
+        },
+        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,
+        transmission = nil,
+        affectVectors = {
+            angular = {
+              yaw = nil,
+              pitch = "down",
+              roll = "port",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
+        },
+        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,
+        transmission = nil,
+        affectVectors = {
+            angular = {
+              yaw = nil,
+              pitch = "down",
+              roll = "star",
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
+        },
+        power = nil
+    }
+}
+```

main.lua

@@ -1,37 +1,4 @@
-Thrusters = {
-    thruster0 = {
-        type = "rotator",
-        name = nil,
-        thruster = nil,
-        transmission = nil,
-        -- NOTE:
-        -- affectVectors will have values depending on how the thruster's 
-        -- thrust vector interacts with the vessel's attitude
-        -- possible directions for yaw: "port"|"star"
-        -- possible directions for roll: "port"|"star"
-        -- possible directions for pitch: "up"|"down"
-        -- possible directions for lateral: "port"|"star"|"fore"|"aft"|"up"|"down"
-        affectVectors = {
-            yaw = nil,
-            pitch = nil,
-            roll = nil,
-            lateral = nil
-        },
-        power = 0
-    },
-    thruster1 = {
-        type = "thruster",
-        name = nil,
-        thruster = nil,
-        affectVectors = {
-            yaw = nil,
-            pitch = nil,
-            roll = nil,
-            lateral = nil
-        },
-        power = 0
-    }
-} -- Table of thruster tables
+Thrusters = {} -- Table of thruster tables; see docs.md####Thrusters for the details of the thruster table structure
 
 ThrustDirections = {
     Angular = {
@@ -109,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
@@ -186,6 +153,7 @@ end
 Config.Autopilot = {
     AutopilotEngaged = false,
     AutoForeAft = false,
+    AutopilotDesiredAltitude = nil,
     AutopilotDesiredSpeed = nil,
     AutopilotDesiredHeading = nil
 }
@@ -273,49 +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
-                        -- possible directions for yaw: "port"|"star"
-                        -- possible directions for roll: "port"|"star"
-                        -- possible directions for pitch: "up"|"down"
-                        -- possible directions for lateral: "port"|"star"|"fore"|"aft"|"up"|"down"
-                        affectVectors = {
-                            yaw = nil,
-                            pitch = nil,
-                            roll = nil,
-                            lateral = 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" 
+        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
+                                }
+                            },
+                            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
@@ -324,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
@@ -333,37 +315,117 @@ function ThrusterInit()
                 thruster = tv,
                 transmission = nil,
                 type = "thruster",
-                -- NOTE:
-                -- affectVectors will have values depending on how the thruster's 
-                -- thrust vector interacts with the vessel's attitude
-                -- possible directions for yaw: "port"|"star"
-                -- possible directions for roll: "port"|"star"
-                -- possible directions for pitch: "up"|"down"
-                -- possible directions for lateral: "port"|"star"|"fore"|"aft"|"up"|"down"
+                --[[
+                    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 = {
-                    yaw = nil,
-                    pitch = nil,
-                    roll = nil,
-                    lateral = nil
+                    angular = {
+                        yaw = nil,
+                        pitch = nil,
+                        roll = nil
+                    },
+                    lateral = {
+                        x = nil,
+                        y = nil,
+                        z = nil
+                    }
                 },
                 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
@@ -422,10 +484,16 @@ local function partiallyUpdateThrusters(thrusterList)
                     transmission = nil,
                     type = "thruster",
                     affectVectors = {
-                        yaw = nil,
-                        roll = nil,
-                        pitch = nil,
-                        lateral = nil
+                        angular = {
+                            yaw = nil,
+                            roll = nil,
+                            pitch = nil
+                        },
+                        lateral = {
+                            x = nil,
+                            y = nil,
+                            z = nil
+                        }
                     },
                     power = tv.getPower()
                 }
@@ -439,10 +507,16 @@ local function partiallyUpdateThrusters(thrusterList)
                     transmission = nil,
                     type = "rotator",
                     affectVectors = {
-                        yaw = nil,
-                        roll = nil,
-                        pitch = nil,
-                        lateral = nil
+                        angular = {
+                            yaw = nil,
+                            roll = nil,
+                            pitch = nil
+                        },
+                        lateral = {
+                            x = nil,
+                            y = nil,
+                            z = nil
+                        }
                     },
                     power = tv.getPower()
                 }
@@ -450,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
@@ -462,14 +536,6 @@ local function partiallyUpdateThrusters(thrusterList)
     end
 end
 
-function UpdateStabilization()
-    -- This function needs full implementation according to requirements in docs.md
-    -- It should adjust thrust based on sensor data to achieve target angles/velocities
-    
-    -- For now, implement placeholder functionality that calls the existing thrust methods
-    print("UpdateStabilization called")
-end
-
 function PollThrottle()
     for _, v in pairs(Config.Throttles) do
         if v.name and v.side then
@@ -479,10 +545,10 @@ function PollThrottle()
 end
 
 function PollSensors()
-    PollVelocity()
-    PollAltitude()
     PollNavTable()
+    PollAltitude()
     PollGimbal()
+    PollVelocity()
 end
 
 function PollVelocity()
@@ -491,8 +557,7 @@ function PollVelocity()
     -- Velocity Sensors
     VelocityVectors = {}
 
-    local next = next
-    if next(velSensors) == nil then -- verify table is not empty
+    if velSensors[1] ~= nil then -- verify array is not empty
 
         SensorData.Velocity = {}
 
@@ -500,15 +565,32 @@ 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.") 
+        end
     end
     
 end
@@ -594,20 +676,34 @@ function PollGimbal()
             xAngle = 0, -- pitch angle
             zAngle = 0  -- roll angle
         }
-        for i, v in ipairs(gimbalSensor.getAngles) do
-            if i == 0 then
+        for i, v in ipairs(gimbalSensor.getAngles()) do
+            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
             wz = 0  -- roll rate
         }
-        for i, v in ipairs(gimbalSensor.getAngularRates) do
+        for i, v in ipairs(gimbalSensor.getAngularRates()) do
             if i == 0 then
                 AngularRates.wx = v
             elseif i == 1 then
@@ -616,12 +712,27 @@ 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
             az = 0  -- fore-aft axis
         }
-        for i, v in ipairs(gimbalSensor.getLinearAcceleration) do
+        for i, v in ipairs(gimbalSensor.getLinearAcceleration()) do
             if i == 0 then
                 LinearAcceleration.ax = v
             elseif i == 1 then
@@ -631,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
@@ -657,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
@@ -670,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
@@ -680,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
@@ -704,64 +843,170 @@ 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
-    local desiredLateralThrustVectors = {}
+    local dt = GetDeltaTime() / 1000
 
-    for f, v in pairs(SensorData.Velocity.Raw) do
-        desiredLateralThrustVectors[f] = CustomSigmoid(v)
-    end
-
-    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
-        end
-    end
-
-    for _, d in pairs(thrustDirections) do
-        for _, t in pairs(Thrusters) do
-            if t.affectVectors.lateral == 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
-
-    -- angular thrust
-    local desiredAngularThrustVectors = {}
-
-    for f, v in pairs(SensorData.Gimbal.AngularRates) do
-        desiredAngularThrustVectors[f] = CustomSigmoid(v)
+    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
     
-    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
+    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
 
-    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)
+    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 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
+
+    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)
+
+    for _, thruster in pairs(Thrusters) do
+        local desiredThrust = 0
+
+        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
@@ -814,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
@@ -834,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)
@@ -915,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))
@@ -929,10 +1183,15 @@ 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()
+        UpdateGlobalThrust()
+
         -- Update monitor displays
         if Config.Monitors.InstrumentPanelMonitor then
             displayInstrumentPanel(Config.Monitors.InstrumentPanelMonitor, SensorData)
@@ -942,21 +1201,10 @@ function Main()
             displayAutopilotControls(Config.Monitors.AutopilotControlMonitor, SensorData)
         end
 
-        local unindexedThrusters = checkIfThrusterIsIndexed()
-        if unindexedThrusters ~= nil then
-            partiallyUpdateThrusters(unindexedThrusters)
-        end
-
-        PollSensors()
-        UpdateGlobalThrust()
-        UpdateStabilization()
-
         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
@@ -968,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
     }