From c0023195c524cee48be1e0f0a18a1d8d46e030e0 Mon Sep 17 00:00:00 2001
From: Sebastiaan de Schaetzen <sebastiaan.de.schaetzen@gmail.com>
Date: Tue, 23 Jul 2024 15:37:55 +0200
Subject: [PATCH] Slight improvements

---
 controller-client/main.lua | 48 ++++++++++++++++++++++++++------------
 spider-host/main.go        | 11 +++++----
 spider-host/servo.go       |  1 +
 3 files changed, 41 insertions(+), 19 deletions(-)

diff --git a/controller-client/main.lua b/controller-client/main.lua
index 4e1f5e8..484c82f 100644
--- a/controller-client/main.lua
+++ b/controller-client/main.lua
@@ -15,6 +15,12 @@ BotState = {
 	servoBattery = nil,
 	servoBatteryCorrected = nil,
 	camfeed = nil,
+
+	viewX = 0,
+	viewY = 0,
+	viewXSent = 0,
+	viewYSent = 0,
+	viewLastUpdate = 0,
 }
 
 Ping = {
@@ -24,10 +30,8 @@ Ping = {
 }
 
 ControllerState = {
-	viewX = 0,
-	viewY = 0,
-	viewChanged = false,
-	viewLastUpdated = 0
+	rightX = 0,
+	rightY = 0
 }
 
 function love.update2()
@@ -41,26 +45,40 @@ function love.update2()
 		love.mqtt.send("command/ping", Ping.payload)
 		print("Sending ping")
 	end
-	if ControllerState.viewChanged and (now - ControllerState.viewLastUpdated) >= 0.05 then
+
+	BotState.viewX = BotState.viewX + ControllerState.rightX * 0.02
+	BotState.viewY = BotState.viewY + ControllerState.rightY * 0.02
+
+	local viewDX, viewDY = math.abs(BotState.viewX - BotState.viewXSent), math.abs(BotState.viewY - BotState.viewYSent)
+	if viewDX > 0.01 or viewDY > 0.01 and (now - BotState.viewLastUpdated) >= 0.05 then
 		love.mqtt.send("command/set_camera_xy", toJSON({
-			x = -ControllerState.viewX * 0.3 + 0.5,
-			y = -ControllerState.viewY * 0.3 + 0.5
+			x = -BotState.viewX * 0.3 + 0.5,
+			y = -BotState.viewY * 0.3 + 0.5
 		}))
-		ControllerState.viewChanged = false
-		ControllerState.viewLastUpdated = now
+		BotState.viewXSent = BotState.viewX
+		BotState.viewYSent = BotState.viewY
+		BotState.viewLastUpdated = now
 	end
 end
 
 function love.joystickaxis2(joystick, axis, value)
-	if axis == 3 and value ~= ControllerState.viewX then
-		ControllerState.viewX = value
-		ControllerState.viewChanged = true
-	elseif axis == 4 and value ~= ControllerState.viewY then
-		ControllerState.viewY = value
-		ControllerState.viewChanged = true
+	if axis == 3 then
+		ControllerState.rightX = value
+	elseif axis == 4 then
+		ControllerState.rightY = value
 	end
 end
 
+-- function love.joystickaxis2(joystick, axis, value)
+-- 	if axis == 3 and value ~= ControllerState.viewX then
+-- 		ControllerState.viewX = value
+-- 		ControllerState.viewChanged = true
+-- 	elseif axis == 4 and value ~= ControllerState.viewY then
+-- 		ControllerState.viewY = value
+-- 		ControllerState.viewChanged = true
+-- 	end
+-- end
+
 function formatSafe(format, value, ...)
 	if value == nil then
 		return "unknown"
diff --git a/spider-host/main.go b/spider-host/main.go
index 1cd6ae9..dbb414e 100644
--- a/spider-host/main.go
+++ b/spider-host/main.go
@@ -75,11 +75,18 @@ func subscribe(client mqtt.Client, topic string, handler mqtt.MessageHandler) {
 	}
 }
 
+func onConnect(client mqtt.Client) {
+	log.Print("Subscribing to command topics")
+	subscribe(client, "spider/command/ping", onPing)
+	subscribe(client, "spider/command/set_camera_xy", onSetCameraXY)
+}
+
 func main() {
 	opts := mqtt.NewClientOptions()
 	opts.AddBroker(broker)
 	opts.SetClientID("spider-host-client")
 	opts.SetResumeSubs(true)
+	opts.SetOnConnectHandler(onConnect)
 	client := mqtt.NewClient(opts)
 
 	token := client.Connect()
@@ -88,10 +95,6 @@ func main() {
 		log.Fatalf("Error connecting to MQTT broker: %v\n", token.Error())
 	}
 
-	log.Print("Subscribing to command topics")
-	subscribe(client, "spider/command/ping", onPing)
-	subscribe(client, "spider/command/set_camera_xy", onSetCameraXY)
-
 	InitBattery()
 	InitServo()
 
diff --git a/spider-host/servo.go b/spider-host/servo.go
index 1d51f78..e8b7db1 100644
--- a/spider-host/servo.go
+++ b/spider-host/servo.go
@@ -52,6 +52,7 @@ func InitServo() {
 	}
 	// Halt the controllers to stop any current movement
 	ServosOff()
+	log.Print("Started servos")
 }
 
 func ServosOff() {