From 45cd517a2ab4693c792c0bfbed0d0031fa9d8380 Mon Sep 17 00:00:00 2001
From: annabunches <annabunches@gmail.com>
Date: Tue, 20 Jul 2021 02:38:29 -0400
Subject: [PATCH] Get full automation to orbit functioning.

---
 boot/rocket.ks    |  3 +--
 execnode.ks       | 38 +++++++++++++++++++++++++++-----------
 launch.ks         | 14 ++++++--------
 lib/navigation.ks | 21 +++++++++++----------
 lib/throttle.ks   |  1 +
 ui/rocket.ks      |  9 +++++++++
 6 files changed, 55 insertions(+), 31 deletions(-)

diff --git a/boot/rocket.ks b/boot/rocket.ks
index c80efca..8773903 100644
--- a/boot/rocket.ks
+++ b/boot/rocket.ks
@@ -5,10 +5,9 @@
 deletepath("/boot/rocket").
 
 // Install software.
-compile "0:/ui/rocket" to "1:/init".
 compile "0:/lib/navigation" to "1:/lib/navigation".
 compile "0:/lib/throttle" to "1:/lib/throttle".
-copypath("0:/lib/math", "1:/lib/math"). // larger when compiled
+compile "0:/ui/rocket" to "1:/init".
 compile "0:/launch" to "1:/launch".
 copypath("0:/execnode", "1:/execnode"). // larger when compiled
 
diff --git a/execnode.ks b/execnode.ks
index 93b3a86..eabcc9f 100644
--- a/execnode.ks
+++ b/execnode.ks
@@ -1,19 +1,35 @@
-runpathonce("lib/navigation").
-runpathonce("lib/throttle").
+runoncepath("lib/navigation").
+runoncepath("lib/throttle").
 
 SAS off.
-local n is SHIP:NEXTNODE.
-local t is BurnTime(n:MAG)/2.
+local t is BurnTime(NEXTNODE:DELTAV:MAG)/2.
 
-lock STEERING to n:DELTAV.
-wait until VectorAngle(SHIP:VELOCITY, STEERINGMANAGER:TARGET) <= 0.1.
+print "Adjusting heading".
+lock STEERING to NEXTNODE:DELTAV.
+wait until VectorAngle(SHIP:FACING:FOREVECTOR, STEERINGMANAGER:TARGET:FOREVECTOR) <= 0.1.
 
 print "Warping to node".
-KUNIVERSE:WARP:WarpTo(n:TIME - t - 5).
-wait until n:ETA <= t.
+KUNIVERSE:TIMEWARP:WarpTo(NEXTNODE:TIME - t - 5).
+wait until NEXTNODE:ETA <= t.
+
+// todo: pid loop here or nah? overshoot would be tricky to deal with...
+print "Executing burn".
+local throt is 1.0.
+lock THROTTLE to throt.
+local dvLast is NEXTNODE:DELTAV:MAG.
+
+// Execute the burn, throttling down by half every time we're
+// consuming more than 25% of our dV in one update.
+until NEXTNODE:DELTAV:MAG <= 0.25 {
+  local dvRem is NEXTNODE:DELTAV:MAG.
+  if dvRem < dvLast * 0.75 {
+    set throt to throt / 2.
+  }
+  set dvLast to dvRem.
+  wait 0.001.
+}
 
-lock THROTTLE to 1.0.
-wait until n:DELTAV:MAG <= 0.1. // todo: pid loop here or no?
 unlock THROTTLE.
 unlock STEERING.
-print "Node Executed. Have a nice day."
+SAS on.
+print "Node Executed. Have a nice day".
diff --git a/launch.ks b/launch.ks
index 0835f23..f99492f 100644
--- a/launch.ks
+++ b/launch.ks
@@ -15,7 +15,7 @@ RCS off.
 SAS off.
 
 // Countdowns are cute.
-print "Initiating automated launch sequence".
+print "Initiating automated launch sequence.".
 from { local x is 5. } until x = 0 step { set x to x - 1. } do {
   print "..." + x.
   wait 0.5.
@@ -32,11 +32,11 @@ lock STEERING to heading(90,90,270).
 stage.
 wait until SHIP:ALTITUDE > 200.
 
-print "Vectoring away from launchpad".
+print "Vectoring away from launchpad.".
 lock STEERING to heading(90, INITIAL_PITCH, 270).
 wait until SHIP:ALTITUDE > GRAVITY_TURN_START.
 
-print "Pitching for gravity turn".
+print "Pitching for gravity turn.".
 // Pitch over...
 lock STEERING to heading(90, GRAVITY_PITCH, 270).
 // Wait until we have rotated to (approximately) that pitch...
@@ -62,14 +62,12 @@ lock THROTTLE to 0.0.
 set SHIP:CONTROL:PILOTMAINTHROTTLE to 0.
 wait 0.001. // make sure these control updates get applied
 
-print "Target apoapsis acquired. Creating maneuver node".
+print "Target apoapsis acquired. Creating maneuver node.".
 AddCircularizationNode().
-print "Releasing controls. Circularization maneuver added. Good luck, Kerman!".
+runpath("/execnode").
+print "Orbit acquired. Releasing controls. Good luck, Kerman.".
 unlock THROTTLE.
 unlock STEERING.
 SAS on.
-wait 5.
 
 // todo - automatically execute the node
-
-CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Close Terminal").
diff --git a/lib/navigation.ks b/lib/navigation.ks
index c893aa3..f107695 100644
--- a/lib/navigation.ks
+++ b/lib/navigation.ks
@@ -28,15 +28,16 @@ function AddCircularizationNode {
   }
   local n is Node(t, 0, 0, 0).
   add(n).
-
-  local delta is 1.
-  local diff is n:ORBIT:APOAPSIS - n:ORBIT:PERIAPSIS.
-  until (diff < 1000) {
-    set n:PROGRADE to n:PROGRADE + delta.
-    local newDiff is n:ORBIT:APOAPSIS - n:ORBIT:PERIAPSIS.
-    if newDiff > diff {
-      set delta to (delta * -1) / 10.
-    }
-    set diff to newDiff.
+ 
+  local epsilon is 100.
+  lock diff to n:ORBIT:APOAPSIS - n:ORBIT:PERIAPSIS.
+  local pid is PIDLoop(0.05, 0.006, 0.006).
+  set pid:EPSILON to epsilon.
+  set pid:SETPOINT to n:ORBIT:APOAPSIS.
+  
+  until abs(diff) < epsilon {
+    print diff.
+    set n:PROGRADE to pid:Update(TIME:SECONDS, n:ORBIT:PERIAPSIS).
+    wait 0.001.
   }
 }
diff --git a/lib/throttle.ks b/lib/throttle.ks
index 99aaef9..55e5b3f 100644
--- a/lib/throttle.ks
+++ b/lib/throttle.ks
@@ -23,6 +23,7 @@ function TWR {
 function BurnTime {
   parameter dV.
 
+  local en is list().
   list ENGINES in en.
 
   local f is en[0]:MAXTHRUST * 1000.  // Engine Thrust (kg * m/s²)
diff --git a/ui/rocket.ks b/ui/rocket.ks
index b19fd47..c5fd0e6 100644
--- a/ui/rocket.ks
+++ b/ui/rocket.ks
@@ -58,6 +58,15 @@ set nodeButton:onClick to nodeButtonPressed@.
 local termButton is interface:AddButton("Terminal").
 set termButton:onClick to terminalButtonPressed@.
 
+// debug
+function circButtonPressed {
+  runpath("/lib/navigation").
+  AddCircularizationNode().
+}
+local cButton is interface:AddButton("Circularize").
+set cButton:onClick to circButtonPressed@.
+// end debug
+
 interface:show().
 
 wait until false.