From 7c37496b5e68c8861b9728dd265616d99c49d345 Mon Sep 17 00:00:00 2001
From: SamH1608 <72268807+SamH1608@users.noreply.github.com>
Date: Mon, 21 Jun 2021 11:08:18 +0200
Subject: [PATCH] Closed loop controller base

---
 .../java/src/land/ClosedLoopController.java   | 58 +++++++++++++++++++
 1 file changed, 58 insertions(+)
 create mode 100644 app/src/main/java/src/land/ClosedLoopController.java

diff --git a/app/src/main/java/src/land/ClosedLoopController.java b/app/src/main/java/src/land/ClosedLoopController.java
new file mode 100644
index 0000000..c6a806e
--- /dev/null
+++ b/app/src/main/java/src/land/ClosedLoopController.java
@@ -0,0 +1,58 @@
+package src.land;
+
+import java.util.ArrayList;
+
+import src.conf.Logger;
+import src.peng.NewtonGravityFunction;
+import src.peng.ODEFunctionInterface;
+import src.peng.State;
+import src.peng.Vector3d;
+import src.solv.ODESolver;
+import src.solv.Verlet;
+
+public class ClosedLoopController extends LandingController
+{
+	public ClosedLoopController() 
+	{
+		super();
+		
+	}
+	
+	public ArrayList<LanderObject> plotTrajectory(Vector3d landerLocation, 
+			 Vector3d landerVelocity,
+			 double landerMass,
+			 Vector3d planetLocation,
+			 Vector3d planetVelocity,
+			 double planetMass,
+			 double planetRadius)
+	{
+	double[] masses = {landerMass, planetMass};
+	
+	ArrayList<Vector3d> positions = new ArrayList<Vector3d>();
+	positions.add(removeZDimension(normalise(landerLocation, planetLocation))); 		
+	positions.add(new Vector3d(0,0,0));											// Planet always starts at 0,0,0
+	
+	ArrayList<Vector3d> velocities = new ArrayList<Vector3d>();
+	velocities.add(removeZDimension(normalise(landerVelocity, planetVelocity))); 		
+	velocities.add(new Vector3d(0,0,0));											// Planet always starts at 0,0,0
+	
+	State currentState = new State(velocities, positions);
+	ODESolver solver = new Verlet();
+	ODEFunctionInterface f = new NewtonGravityFunction(masses);
+	
+	ArrayList<LanderObject> trajectory = new ArrayList<LanderObject>();
+	
+	Logger.logCSV("closedloop_controller", "Time,Pos X, Pos Y, Pos Z, Vel X, Vel Y, Vel Z");
+	double time = 0;
+	double stepSize = 0.1;
+	while(!testHeight(currentState.position.get(0), currentState.position.get(1), planetRadius))
+	{
+		Logger.logCSV("closedloop_controller", time + "," + currentState.position.get(0).toCSV() + currentState.velocity.get(0).toCSV());
+		currentState = solver.step(f, time, currentState, stepSize);
+		trajectory.add(new LanderObject(currentState.position.get(0), 0));
+		time = time + stepSize;
+	}
+	
+	return trajectory;
+	}
+}