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

BIG FORMATING COMMIT

Browse Source
This commit is contained in:
Djeeberjr
2023-07-30 15:55:22 +02:00
parent 088fce0aaa
commit 91972107eb
103 changed files with 1610 additions and 1585 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,8 +7,8 @@ import lib.Pos3;
using tink.CoreApi;
class INS {
private static var heading: Null<Pos3> = null;
private static var alingment: Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
private static var heading:Null<Pos3> = null;
private static var alingment:Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
@:allow(kernel.turtle.Turtle)
private static function moveForward() {
@@ -40,7 +40,8 @@ class INS {
@:allow(kernel.turtle.Turtle)
private static function turnLeft() {
if (heading == null) return;
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) {
@@ -54,7 +55,8 @@ class INS {
@:allow(kernel.turtle.Turtle)
private static function turnRight() {
if (heading == null) return;
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) {
@@ -66,7 +68,7 @@ class INS {
}
}
private static function move(dir: Null<Pos3>) {
private static function move(dir:Null<Pos3>) {
Log.debug('INS move: $dir');
var pos = GPS.getPosition();
var newPos = pos + dir;
@@ -77,17 +79,16 @@ class INS {
return heading;
}
public static function align(): Promise<Noise> {
public static function align():Promise<Noise> {
Log.info("Aligning INS");
return new Promise<Noise>((resolve,reject)->{
if (Turtle.instance.getFuelLevel() < 2){
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.locate().handle((pos1)->{
GPS.locate().handle((pos1) -> {
Log.debug('pos1: $pos1');
if (pos1 == null) {
Log.warn("GPS not available for 1st position");
@@ -103,7 +104,7 @@ class INS {
return;
}
GPS.locate().handle((pos2)->{
GPS.locate().handle((pos2) -> {
Log.debug('pos2: $pos2');
if (pos2 == null) {
Log.warn("GPS not available for 2nd position");
@@ -111,7 +112,7 @@ class INS {
return;
}
var cHeading = calcHeading(pos1,pos2,moved);
var cHeading = calcHeading(pos1, pos2, moved);
if (cHeading == null) {
Log.error("Can't calculate heading");
reject(new Error("Can't calculate heading"));
@@ -124,7 +125,6 @@ class INS {
resolve(Noise);
});
});
return null;
});
@@ -138,7 +138,7 @@ class INS {
return 1;
} else {
Turtle.instance.turnLeft(); // TODO: Check if successfull
if (Turtle.instance.forward().isSuccess()){
if (Turtle.instance.forward().isSuccess()) {
return 2;
} else if (Turtle.instance.back().isSuccess()) {
return 3;
@@ -149,7 +149,7 @@ class INS {
}
}
private static function calcHeading(pos1: Pos3,pos2:Pos3,moved:Int): Null<Pos3> {
private static function calcHeading(pos1:Pos3, pos2:Pos3, moved:Int):Null<Pos3> {
if (moved == 0) {
return pos1 - pos2;
} else if (moved == 1) {
@@ -200,5 +200,4 @@ class INS {
private static function rotatePos3ToLeft(pos3:Pos3):Pos3 {
return rotatePos3ToRight(rotatePos3ToRight(rotatePos3ToRight(pos3)));
}
}