From 133186baf4abd839c95f339fa3682bcf705247c8 Mon Sep 17 00:00:00 2001
From: Manglemix <shabouza030@gmail.com>
Date: Wed, 1 Nov 2023 19:05:54 -0600
Subject: [PATCH] tweaks for tests

---
 lunabase/lunabot.gd    | 3 ++-
 lunabase/main.tscn     | 7 ++++++-
 lunasim/blind/blind.py | 8 ++++----
 3 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/lunabase/lunabot.gd b/lunabase/lunabot.gd
index 7aff8fa9..99a3e9c9 100644
--- a/lunabase/lunabot.gd
+++ b/lunabase/lunabot.gd
@@ -2,7 +2,7 @@ extends Node
 
 
 const RECV_FROM := 43721
-const USE_ARCHIMEDES := false
+const USE_ARCHIMEDES := true
 
 enum Channels { IMPORTANT, CAMERA, ODOMETRY, CONTROLS, MAX }
 enum ImportantMessage { ENABLE_CAMERA, DISABLE_CAMERA, PING }
@@ -270,6 +270,7 @@ func send_arm_controls(arm_vel: float, drum_vel:=0.0) -> void:
 		drum_vel = -1
 		push_warning("Drum velocity lesser than -1!")
 	
+	print_debug(str(arm_vel) + " " + str(drum_vel))
 	lunabot_mutex.lock()
 	controls_data.encode_s8(2, roundi(arm_vel * 127))
 	if USE_ARCHIMEDES:
diff --git a/lunabase/main.tscn b/lunabase/main.tscn
index fdf49715..da25c068 100644
--- a/lunabase/main.tscn
+++ b/lunabase/main.tscn
@@ -17,8 +17,13 @@ func _ready() -> void:
 
 func _input(event: InputEvent) -> void:
 	if event is InputEventJoypadMotion:
-		Lunabot.send_arm_controls(
+#		print(Input.get_action_strength(\"DriveForward\"))
+		Lunabot.send_steering(
 			Input.get_action_strength(\"DriveForward\") - Input.get_action_strength(\"DriveBackward\"),
+			Input.get_action_strength(\"SteerRight\") - Input.get_action_strength(\"SteerLeft\")
+		)
+		Lunabot.send_arm_controls(
+			Input.get_action_strength(\"MoveArmUp\") - Input.get_action_strength(\"MoveArmDown\"),
 			Input.get_action_strength(\"SpinDrumClockwise\") - Input.get_action_strength(\"SpinDrumCounterClockwise\")
 		)
 "
diff --git a/lunasim/blind/blind.py b/lunasim/blind/blind.py
index 426de421..55fbea20 100644
--- a/lunasim/blind/blind.py
+++ b/lunasim/blind/blind.py
@@ -1,5 +1,5 @@
 from multiprocessing import Pool
-from typing import Callable, Iterable, Tuple, Union
+from typing import Iterable, Union
 from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints
 from filterpy.common import Q_discrete_white_noise
 import numpy as np
@@ -313,7 +313,7 @@ def to_test_plan(self) -> np.ndarray:
     travel_plan = TravelPlan(
         [np.array([1., 1.]), np.array([5.6, 1.])],
         [np.array([5.6, 3.]), 30, np.array([5.6, 1.]), 30],
-        360
+        1800
     )
     plan = travel_plan.to_test_plan()
     # plot_val(plan, 1)
@@ -323,8 +323,8 @@ def to_test_plan(self) -> np.ndarray:
         results = p.map(
             run_args,
             [
-                (plan, gen_ukf, 0.001, 0.001, "0.001 z_std")
-            ] * 40
+                (plan, gen_ukf, 0.0001, 0.0001, "0.001 z_std")
+            ] * 20
         )
     sim_time = time.time() - start_time
     results_sorted = {}