niklas/cc-haxe
1
0
Fork 0
Code Issues 13 Pull Requests Projects Releases Wiki Activity

idfk

Browse Source
This commit is contained in:
Niklas Kapelle 2024-10-18 16:48:38 +02:00
parent 9d6e8a366b
commit 1d60e13792
Signed by: niklas
GPG Key ID: 4EB651B36D841D16
1 changed files with 36 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/kernel/gps/INS.hx
View File

@ -1,7 +1,6 @@
package kernel.gps; package kernel.gps;
import lib.SinglePromise; import lib.SinglePromise;
import kernel.log.Log;
import kernel.turtle.Turtle; import kernel.turtle.Turtle;
import lib.WorldPos; import lib.WorldPos;
@ -71,6 +70,9 @@ class INS {
} }
private static function move(dir:Null<WorldPos>) { private static function move(dir:Null<WorldPos>) {
if (alignPromise.isRunning()) {
return;
}
var pos = GPS.getPosition(); var pos = GPS.getPosition();
if (pos == null || dir == null) if (pos == null || dir == null)
return; return;
@ -86,40 +88,34 @@ class INS {
return alignPromise.request(); return alignPromise.request();
} }
public static function startAlign():Promise<Noise> { public static function startAlign():Void {
return new Promise<Noise>((resolve, reject) -> {
if (Turtle.getFuelLevel() < 2) { if (Turtle.getFuelLevel() < 2) {
Log.warn("Not enough fuel to align"); alignPromise.reject(new Error("Not enough fuel to align"));
reject(new Error("Not enough fuel to align")); return;
return null;
} }
GPS.locate().handle((result) -> { GPS.locate().handle((result) -> {
switch result { switch result {
case Failure(err): case Failure(err):
Log.warn("GPS not available for 1st position"); alignPromise.reject(new Error("Failed to locate 1st positon: " + err));
reject(new Error("GPS not available"));
return; return;
case Success(pos1): case Success(pos1):
var moved = tryMoving(); var moved = tryMoving();
if (moved == -1) { if (moved == -1) {
Log.warn("Can't move"); alignPromise.reject(new Error("Can't move"));
reject(new Error("Can't move"));
return; return;
} }
GPS.locate().handle((result) -> { GPS.locate().handle((result2) -> {
switch result { switch result2 {
case Failure(err): case Failure(err):
Log.warn("GPS not available for 2nd position"); alignPromise.reject(new Error("GPS not available for 2nd position: " + err));
reject(new Error("GPS not available for 2nd position"));
return; return;
case Success(pos2): case Success(pos2):
var cHeading = calcHeading(pos1, pos2, moved); var cHeading = calcHeading(pos1, pos2, moved);
if (cHeading == null) { if (cHeading == null) {
Log.error("Can't calculate heading"); alignPromise.reject(new Error("Can't calculate heading"));
reject(new Error("Can't calculate heading"));
return; return;
} }
@ -127,13 +123,11 @@ class INS {
moveBack(moved); moveBack(moved);
GPS.setINSPosition(pos1); GPS.setINSPosition(pos1);
resolve(Noise); alignPromise.resolve(Noise);
} }
}); });
} }
}); });
return null;
});
} }
// -1 = not moved, 0 = back, 1 = forward, 2 = left, 3 = right // -1 = not moved, 0 = back, 1 = forward, 2 = left, 3 = right
@ -172,20 +166,14 @@ class INS {
private static function moveBack(moved:Int) { private static function moveBack(moved:Int) {
if (moved == 0) { if (moved == 0) {
Turtle.forward(); Turtle.forward();
// cc.Turtle.forward();
} else if (moved == 1) { } else if (moved == 1) {
Turtle.back(); Turtle.back();
// cc.Turtle.back();
} else if (moved == 2) { } else if (moved == 2) {
Turtle.back(); Turtle.back();
// cc.Turtle.back();
Turtle.turnRight(); Turtle.turnRight();
// cc.Turtle.turnRight();
} else if (moved == 3) { } else if (moved == 3) {
Turtle.forward(); Turtle.forward();
// cc.Turtle.forward();
Turtle.turnRight(); Turtle.turnRight();
// cc.Turtle.turnRight();
} }
} }