From b9452cd598f4e7302e956bb30ede96c07384577e Mon Sep 17 00:00:00 2001 From: Djeeberjr Date: Thu, 13 Apr 2023 22:26:26 +0200 Subject: [PATCH] implemented INS --- src/kernel/Init.hx | 2 + src/kernel/gps/GPS.hx | 30 ++++- src/kernel/gps/INS.hx | 211 ++++++++++++++++++++++++++++++++++++ src/kernel/turtle/Turtle.hx | 25 ++++- 4 files changed, 259 insertions(+), 9 deletions(-) create mode 100644 src/kernel/gps/INS.hx diff --git a/src/kernel/Init.hx b/src/kernel/Init.hx index 6850181..6ab1f1c 100644 --- a/src/kernel/Init.hx +++ b/src/kernel/Init.hx @@ -1,5 +1,6 @@ package kernel; +import kernel.gps.INS; import kernel.fs.FS; import kernel.gps.GPS; import kernel.log.Log; @@ -34,6 +35,7 @@ class Init { Net.instance = new Net(); GPS.instance = new GPS(); + INS.instance = new INS(); // Register default terminate handler KernelEvents.instance.onTerminate.handle(_->{ diff --git a/src/kernel/gps/GPS.hx b/src/kernel/gps/GPS.hx index 5697d30..eb76421 100644 --- a/src/kernel/gps/GPS.hx +++ b/src/kernel/gps/GPS.hx @@ -23,6 +23,8 @@ class GPS { private var cachedPosition:Pos3; private var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = []; + private var futureResolve: (pos:Null) -> Void = null; + @:allow(kernel.Init) private function new() { this.loadCachedPosition(); @@ -39,6 +41,12 @@ class GPS { } } + @:allow(kernel.gps.INS) + private function setINSPosition(pos:Pos3) { + cachedPosition = pos; + posAccuracy = 1; + } + public function getPosition():Null { return cachedPosition; } @@ -52,8 +60,18 @@ class GPS { posAccuracy = 0; } - public function locate() { - sendPositionRequest(); + public function locate():Future> { + // TODO: implenet a timeout + // TODO: dont send a request twice if the last one is still pending or we moved + return new Future>((resolve)->{ + this.futureResolve = resolve; + sendPositionRequest(); + return null; + }); + } + + private function resolveFuture(pos:Null) { + this.futureResolve(pos); } private function persistCachedPositon() { @@ -124,13 +142,19 @@ class GPS { if (lastPositionResponse.length < 4) return; // We need at least 3 responses to calculate the position - var calculatedPosition = calculatePosition().round(); + var calculatedPosition = calculatePosition(); + + if (calculatedPosition != null){ + calculatedPosition = calculatedPosition.round(); + } lastPositionResponse = []; // Reset the response array if (calculatedPosition == null) return; cachedPosition = calculatedPosition; posAccuracy = 3; + + resolveFuture(calculatedPosition); default: } } diff --git a/src/kernel/gps/INS.hx b/src/kernel/gps/INS.hx new file mode 100644 index 0000000..d843a09 --- /dev/null +++ b/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 = 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) { + Log.debug('INS move: $dir'); + var pos = GPS.instance.getPosition(); + var newPos = pos + dir; + GPS.instance.setINSPosition(newPos); + } + + public function getHeading():Null { + return heading; + } + + public function align(): Promise { + Log.info("Aligning INS"); + return new Promise((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 { + 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))); + } + +} diff --git a/src/kernel/turtle/Turtle.hx b/src/kernel/turtle/Turtle.hx index 94b1015..56f3be1 100644 --- a/src/kernel/turtle/Turtle.hx +++ b/src/kernel/turtle/Turtle.hx @@ -2,6 +2,7 @@ package kernel.turtle; import kernel.log.Log; import kernel.turtle.Types; +import kernel.gps.INS; using tink.CoreApi; @@ -34,32 +35,44 @@ class Turtle { public function forward():Outcome { var r = cc.Turtle.forward(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.moveForward(); + return r2; } public function back():Outcome { var r = cc.Turtle.back(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.moveBackward(); + return r2; } public function up():Outcome { var r = cc.Turtle.up(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.moveUp(); + return r2; } public function down():Outcome { var r = cc.Turtle.down(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.moveDown(); + return r2; } public function turnLeft():Outcome { var r = cc.Turtle.turnLeft(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.turnRight(); + return r2; } public function turnRight():Outcome { var r = cc.Turtle.turnRight(); - return conterToOutcome(r); + var r2 = conterToOutcome(r); + if (r2.isSuccess()) INS.instance.turnRight(); + return r2; } public function dig(dir:InteractDirections, ?toolSide:ToolSide):Outcome {