adjust PID kp values

docs.md

@@ -307,6 +307,7 @@ Thrusters = {
         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,
@@ -334,6 +335,7 @@ Thrusters = {
         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,
@@ -361,6 +363,7 @@ Thrusters = {
         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,
@@ -388,6 +391,7 @@ Thrusters = {
         primary_altitude_thruster = true,
         primary_fore_thruster = false,
         primary_aft_thruster = false,
+        x_config_equivalent = "3", -- if you're running a standard x configuration, what prop is this equivalent to?
         type = "rotator",
         name = "gyroscopic_propeller_bearing_3",
         thruster = nil,

main.lua

@@ -410,11 +410,11 @@ local function populateThrusterValues()
                 thruster.power = thruster.thruster.getPower()
             elseif thruster.type == "rotator" then
                 if thruster.transmission ~= nil then
-                    local transmissionType = peripheral.getType(thruster.transmission)
+                    local transmissionType = peripheral.getType(peripheral.wrap(thruster.transmission))
                     if transmissionType == "analog_transmission" then
-                        thruster.power = (thruster.transmission.getSignal())/15
+                        thruster.power = (peripheral.wrap(thruster.transmission).getSignal())/15
                     elseif transmissionType == "Create_RotationSpeedController" then
-                        thruster.power = (thruster.transmission.getTargetSpeed())/256
+                        thruster.power = (peripheral.wrap(thruster.transmission).getTargetSpeed())/256
                     end
                 else
                     print("ERROR: Thruster "..peripheral.getName(thruster.thruster).." has no transmission.")
@@ -567,12 +567,12 @@ function PollVelocity()
             local vsVelocity = velsensor.getVelocity()
             
             -- correct for heading correction value
-            if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+            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
+            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
@@ -685,14 +685,14 @@ function PollGimbal()
         end
 
         -- correct for heading correction value
-        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+        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
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
             local temp = Angles.xAngle
             Angles.xAngle = -Angles.zAngle
             Angles.zAngle = -temp
@@ -714,14 +714,14 @@ function PollGimbal()
         end
 
         -- correct for heading correction value
-        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+        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
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
             local temp = AngularRates.wx
             AngularRates.wx = -AngularRates.wz
             AngularRates.wz = -temp
@@ -743,14 +743,14 @@ function PollGimbal()
         end
 
         -- correct for heading correction value
-        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
+        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
+        elseif Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
             local temp = LinearAcceleration.ax
             LinearAcceleration.ax = -LinearAcceleration.az
             LinearAcceleration.az = -temp
@@ -787,8 +787,10 @@ function SetThrusterPower(thruster, power)
         return
     end
 
-    if Config.Debug then print("DEBUG: thruster "..thruster.name..": power "..power) 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
@@ -817,14 +819,13 @@ 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
 
             peripheral.wrap(thruster.transmission).setSignal(actualPower)
 
-            if Config.Debug then print("DEBUG: actualPower: "..actualPower) end
+            -- if Config.Debug then print("DEBUG: actualPower: "..actualPower) end
         end
         
     elseif thruster.type == "thruster" then
@@ -859,12 +860,36 @@ function CreatePID(kp, ki, kd)
         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
-            self.integral = self.integral + (error * dt)
-            local derivative = (error - self.lastError) / dt
+
+            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 (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
+
+            return clampedOutput
         end
     }
 end
@@ -873,13 +898,13 @@ end
 PIDs = {
 -- angular
     -- attitude PIDs
-    RollAttitudePID = CreatePID(4.5, 0.01, 0.05),
-    PitchAttitudePID = CreatePID(4.5, 0.01, 0.05),
+    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(1.2, 0.01, 0.05),
-    PitchRatePID = CreatePID(1.2, 0.01, 0.05),
+    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
@@ -889,30 +914,43 @@ PIDs = {
     ForeAftPID  = CreatePID(0.1, 0.01, 0.05),
 
     -- rate PIDs
-    VerticalRatePID = CreatePID(0.1, 0.01, 0.05),
+    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
 
-    local dt = GetDeltaTime()
+    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 throttleOutput = PIDs.AltitudePID:update(Config.Autopilot.AutopilotDesiredAltitude or 0, SensorData.Altitude.Altitude or 0, dt)
+    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 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
@@ -921,27 +959,56 @@ function UpdateGlobalThrust()
     for _, thruster in pairs(Thrusters) do
         local desiredThrust = 0
 
-        -- Calculate desired thrust based on affectVectors and PID outputs
-        if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then
-            desiredThrust = desiredThrust + pitchRateOutput
-        end
-        if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
-            desiredThrust = desiredThrust + rollRateOutput
-        end
-        if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
-            desiredThrust = desiredThrust + yawRateOutput
-        end
+        if thruster.x_config_equivalent ~= nil then
+            
+            -- Calculate desired thrust based on affectVectors and PID outputs
+            if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then
+                if thruster.affectVectors.angular.pitch == "up" then
+                    desiredThrust = desiredThrust + pitchRateOutput
+                else
+                    desiredThrust = desiredThrust - pitchRateOutput
+                end
+            end
+            
+            if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
+                if thruster.affectVectors.angular.roll == "port" then
+                    desiredThrust = desiredThrust + rollRateOutput
+                else
+                    desiredThrust = desiredThrust - rollRateOutput
+                end
+                
+            end
+            
+            if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
+                if thruster.affectVectors.angular.yaw == "port" then
+                    desiredThrust = desiredThrust + yawRateOutput
+                else
+                    desiredThrust = desiredThrust - yawRateOutput
+                end
+            end
 
-        if thruster.affectVectors.lateral.y == "up" then
-            desiredThrust = desiredThrust + throttleOutput 
+            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
-
-        -- Normalize the desired thrust to be between 0 and 1
-        local normalizedThrust = math.max(0, math.min(1, (desiredThrust + 1) / 2)) -- Convert from [-1, 1] to [0, 1]
-
-        if Config.Debug then print("DEBUG: "..thruster.name..": desiredThrust "..desiredThrust..", normalizedThrust "..normalizedThrust) end
-
-        SetThrusterPower(thruster, normalizedThrust)
     end
 end
 

thrusters.txt.example

@@ -6,6 +6,7 @@ return {
         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,
@@ -31,6 +32,7 @@ return {
         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,
@@ -56,6 +58,7 @@ return {
         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,
@@ -81,6 +84,7 @@ return {
         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,