implemented INS

src/kernel/gps/INS.hx

@@ -0,0 +1,211 @@
+package kernel.gps;
+
+import kernel.log.Log;
+import kernel.turtle.Turtle;
+import lib.Pos3;
+
+using tink.CoreApi;
+
+class INS {
+	public static var instance:INS;
+
+	private var heading: Null<Pos3> = null;
+	private var alingment: Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
+
+	@:allow(kernel.Init)
+	private function new() {
+
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveForward() {
+		if (heading == null) {
+			this.alingment = 0;
+			return;
+		}
+		move(heading);
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveBackward() {
+		if (heading == null) {
+			this.alingment = 0;
+			return;
+		}
+		move(heading.negate());
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveUp() {
+		move({x: 0, y: 1, z: 0});
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveDown() {
+		move({x: 0, y: -1, z: 0});
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function turnLeft() {
+		if (heading == null) return;
+		if (heading.x == 0 && heading.z == -1) {
+			heading = {x: -1, y: 0, z: 0};
+		} else if (heading.x == -1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: 1};
+		} else if (heading.x == 0 && heading.z == 1) {
+			heading = {x: 1, y: 0, z: 0};
+		} else if (heading.x == 1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: -1};
+		}
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function turnRight() {
+		if (heading == null) return;
+		if (heading.x == 0 && heading.z == -1) {
+			heading = {x: 1, y: 0, z: 0};
+		} else if (heading.x == -1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: -1};
+		} else if (heading.x == 0 && heading.z == 1) {
+			heading = {x: -1, y: 0, z: 0};
+		} else if (heading.x == 1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: 1};
+		}
+	}
+
+	private function move(dir: Null<Pos3>) {
+		Log.debug('INS move: $dir');
+		var pos = GPS.instance.getPosition();
+		var newPos = pos + dir;
+		GPS.instance.setINSPosition(newPos);
+	}
+
+	public function getHeading():Null<Pos3> {
+		return heading;
+	}
+
+	public function align(): Promise<Noise> {
+		Log.info("Aligning INS");
+		return new Promise<Noise>((resolve,reject)->{
+
+			if (Turtle.instance.getFuelLevel() < 2){
+				Log.warn("Not enough fuel to align");
+				reject(new Error("Not enough fuel to align"));
+				return null;
+			}
+
+			GPS.instance.locate().handle((pos1)->{
+				Log.debug('pos1: $pos1');
+				if (pos1 == null) {
+					Log.warn("GPS not available for 1st position");
+					reject(new Error("GPS not available"));
+					return;
+				}
+
+				var moved = tryMoving();
+
+				if (moved == -1) {
+					Log.warn("Can't move");
+					reject(new Error("Can't move"));
+					return;
+				}
+
+				GPS.instance.locate().handle((pos2)->{
+					Log.debug('pos2: $pos2');
+					if (pos2 == null) {
+						Log.warn("GPS not available for 2nd position");
+						reject(new Error("GPS not available for 2nd position"));
+						return;
+					}
+
+					var heading = calcHeading(pos1,pos2,moved);
+					if (heading == null) {
+						Log.error("Can't calculate heading");
+						reject(new Error("Can't calculate heading"));
+						return;
+					}
+
+					this.heading = heading;
+					moveBack(moved);
+					GPS.instance.setINSPosition(pos1);
+
+					resolve(Noise);
+				});
+
+			});
+			return null;
+		});
+	}
+
+	// -1 = not moved, 0 = back, 1 = forward, 2 = left, 3 = right
+	private function tryMoving():Int {
+		if (Turtle.instance.back().isSuccess()) {
+			return 0;
+		} else if (Turtle.instance.forward().isSuccess()) {
+			return 1;
+		} else {
+			Turtle.instance.turnLeft(); // TODO: Check if successfull
+			if (Turtle.instance.forward().isSuccess()){
+				return 2;
+			} else if (Turtle.instance.back().isSuccess()) {
+				return 3;
+			} else {
+				// Can't move
+				return -1;
+			}
+		}
+	}
+
+	private function calcHeading(pos1: Pos3,pos2:Pos3,moved:Int): Null<Pos3> {
+		if (moved == 0) {
+			return pos1 - pos2;
+		} else if (moved == 1) {
+			return pos2 - pos1;
+		} else if (moved == 2) {
+			return rotatePos3ToRight(pos2 - pos1);
+		} else if (moved == 3) {
+			return rotatePos3ToLeft(pos2 - pos1);
+		} else {
+			return null;
+		}
+	}
+
+	private function moveBack(moved:Int) {
+		if (moved == 0) {
+			Turtle.instance.forward();
+			// cc.Turtle.forward();
+		} else if (moved == 1) {
+			Turtle.instance.back();
+			// cc.Turtle.back();
+		} else if (moved == 2) {
+			Turtle.instance.back();
+			// cc.Turtle.back();
+			Turtle.instance.turnRight();
+			// cc.Turtle.turnRight();
+		} else if (moved == 3) {
+			Turtle.instance.forward();
+			// cc.Turtle.forward();
+			Turtle.instance.turnRight();
+			// cc.Turtle.turnRight();
+		}
+	}
+
+	private function rotatePos3ToRight(pos:Pos3):Pos3 {
+		if (pos.x == 0 && pos.z == -1) {
+			return {x: 1, y: 0, z: 0};
+		} else if (pos.x == -1 && pos.z == 0) {
+			return {x: 0, y: 0, z: -1};
+		} else if (pos.x == 0 && pos.z == 1) {
+			return {x: -1, y: 0, z: 0};
+		} else if (pos.x == 1 && pos.z == 0) {
+			return {x: 0, y: 0, z: 1};
+		} else {
+			return pos;
+		}
+	}
+
+	private function rotatePos3ToLeft(pos3:Pos3):Pos3 {
+		return rotatePos3ToRight(rotatePos3ToRight(rotatePos3ToRight(pos3)));
+	}
+
+}