tweaks for tests

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:

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\")
 		)
 "

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 = {}