adjust PID kp values

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,222 @@ Every cycle:
 - Altitude Sensor `getHeight()`, `getAirPressure()`, `getVerticalSpeed()`
 - 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
+
+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"
+    }
+  }
+}
+```
+
+#### SensorData
+
+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
+  }
+}
+```
+
+#### 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,31 @@
-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
+Thrusters = {} -- Table of thruster tables; see docs.md####Thrusters for the details of the thruster table structure
+
+ThrustDirections = {
+    Angular = {
+        Negative = {
+            x = "pitchdown",
+            y = "yawport",
+            z = "rollport"
         },
-        power = 0
+        Positive = {
+            x = "pitchup",
+            y = "yawstar",
+            z = "rollstar"
+        }
     },
-    thruster1 = {
-        type = "thruster",
-        name = nil,
-        thruster = nil,
-        affectVectors = {
-            yaw = nil,
-            pitch = nil,
-            roll = nil,
-            lateral = nil
+    Lateral = {
+        Negative = {
+            x = "star",
+            y = "down",
+            z = "aft"
         },
-        power = 0
+        Positive = {
+            x = "port",
+            y = "up",
+            z = "fore"
+        }
     }
-} -- Table of thruster tables
+}
 
 SensorData = {}
 
@@ -82,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
@@ -159,6 +153,7 @@ end
 Config.Autopilot = {
     AutopilotEngaged = false,
     AutoForeAft = false,
+    AutopilotDesiredAltitude = nil,
     AutopilotDesiredSpeed = nil,
     AutopilotDesiredHeading = nil
 }
@@ -246,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
@@ -297,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
@@ -306,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
@@ -395,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()
                 }
@@ -412,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()
                 }
@@ -423,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
@@ -435,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
@@ -452,10 +545,10 @@ function PollThrottle()
 end
 
 function PollSensors()
-    PollVelocity()
-    PollAltitude()
     PollNavTable()
+    PollAltitude()
     PollGimbal()
+    PollVelocity()
 end
 
 function PollVelocity()
@@ -464,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 = {}
 
@@ -473,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
@@ -563,35 +672,134 @@ 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 == 1 then
+                Angles.xAngle = v
+            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
+            if i == 0 then
+                AngularRates.wx = v
+            elseif i == 1 then
+                AngularRates.wy = v
+            elseif i == 2 then
+                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
+            if i == 0 then
+                LinearAcceleration.ax = v
+            elseif i == 1 then
+                LinearAcceleration.ay = v
+            elseif i == 2 then
+                LinearAcceleration.az = v
+            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
 
+
+-- 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
 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
@@ -601,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
@@ -611,58 +819,197 @@ 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
 end
 
+function GetThrusterPower(thruster)
+    if thruster.type == "rotator" then
+        return thruster.power
+    end
+    if thruster.type == "thruster" then
+        return thruster.thruster.getPower()
+    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() 
-    -- lateral thrust
-    local desiredLateralThrustVectors = {}
+    -- 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
 
-    for f, v in pairs(VelocityVectors) do
-        desiredLateralThrustVectors[f] = CustomSigmoid(v)
+    local dt = GetDeltaTime() / 1000
+
+    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
 
-    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
-        end
-        if 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
-            if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end
-        end
-    end
+    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
 
-    for _, d in pairs(thrustDirections) do
-        for _, t in pairs(Thrusters) do
-            if t.affectVectors.lateral == d then
-                SetThrusterPower(t, d)
+    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
-
-    -- angular thrust
-    local desiredAngluarThrustVectors = {}
-
-    for f, v in pairs(AngularRates) do
-        desiredAngluarThrustVectors[f] = CustomSigmoid(v)
-    end
-    
-    -- Need to complete this function implementation based on docs.md requirements
 end
 
 function Init()
@@ -712,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
@@ -732,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)
@@ -813,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))
@@ -827,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)
@@ -840,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
@@ -866,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
     }