Slight improvements

controller-client/draw.lua

@@ -1,8 +1,41 @@
+local showUI = true
 
 local function getTextY(line)
 	return 15 + 25 * line
 end
 
+local function addLineToTextBox(box, text, color)
+	color = color or {1, 1, 1}
+	box.lines = box.lines or {}
+	box.lines[#box.lines + 1] = {text=text, color=color}
+
+	local font = love.graphics.getFont()
+	local width = font:getWidth(text)
+	local boxWidth = box.width or 0
+	if width > boxWidth then
+		box.width = width
+	end
+end
+
+local function drawTextBox(box, x, y, margin, padding)
+	x = x or 0
+	y = y or 0
+	margin = margin or 10
+	padding = padding or 3
+
+	local font = love.graphics.getFont()
+	local lineHeight = font:getHeight() + padding
+
+	love.graphics.setColor(0, 0, 0, 0.5)
+	love.graphics.rectangle("fill", x, y, box.width + margin * 2, #box.lines * lineHeight - padding + margin * 2)
+	
+	love.graphics.setColor(1, 1, 1)
+	for index, line in ipairs(box.lines) do
+		love.graphics.setColor(line.color)
+		love.graphics.print(line.text, x + margin, y + margin + (index - 1) * lineHeight)
+	end
+end
+
 function love.draw2()
 	local width, height = love.graphics.getDimensions()
 
@@ -31,6 +64,9 @@ function love.draw2()
 		love.graphics.setShader(nil)
 	end
 
+	local textbox = {}
+
+	if UIState.showUI then
 		-- Draw time
 		local time
 		if BotState.lastMessage == 0 then
@@ -38,25 +74,26 @@ function love.draw2()
 		else
 			time = math.floor(love.timer.getTime() - BotState.lastMessage) .. "s ago"
 		end
-	love.graphics.print("Last message received: " .. time, 5, 5)
+		addLineToTextBox(textbox, "Last message received: " .. time)
 
 		-- Draw cpu battery
+		local color = {1, 1, 1}
 		if BotState.cpuBatteryCorrected == nil or BotState.cpuBatteryCorrected <= 3 then
-		love.graphics.setColor(1, 0, 0)
+			color = {1, 0, 0}
-	else
-		love.graphics.setColor(1, 1, 1)
 		end
-	love.graphics.print("CPU Batt: " .. formatSafe("%.02f (%.02f) V", BotState.cpuBattery, BotState.cpuBatteryCorrected), 5, getTextY(1))
+		addLineToTextBox(textbox, "CPU Batt: ".. formatSafe("%.02f (%.02f) V", BotState.cpuBattery, BotState.cpuBatteryCorrected), color)
 
 		-- Draw servo battery
+		local color = {1, 1, 1}
 		if BotState.servoBatteryCorrected == nil or BotState.servoBatteryCorrected <= 3 then
-		love.graphics.setColor(1, 0, 0)
+			color = {1, 0, 0}
-	else
-		love.graphics.setColor(1, 1, 1)
 		end
-	love.graphics.print("Servo Batt: " .. formatSafe("%.02f (%.02f) V", BotState.servoBattery, BotState.servoBatteryCorrected), 5, getTextY(2))
+		addLineToTextBox(textbox, "Servo Batt: ".. formatSafe("%.02f (%.02f) V", BotState.servoBattery, BotState.servoBatteryCorrected), color)
 
 		-- Draw latency
 		love.graphics.setColor(1, 1, 1)
-	love.graphics.print("Latency: " .. Ping.latency, 5, getTextY(3))
+		addLineToTextBox(textbox, "Latency: ".. Ping.latency)
+
+		drawTextBox(textbox)
+	end
 end

controller-client/main.lua

@@ -4,6 +4,10 @@ require("draw")
 
 CamShader = nil
 
+UIState = {
+	showUI = true,
+}
+
 BotState = {
 	lastMessage = 0,
 	cpuBattery = nil,
@@ -11,6 +15,12 @@ BotState = {
 	servoBattery = nil,
 	servoBatteryCorrected = nil,
 	camfeed = nil,
+
+	viewX = 0,
+	viewY = 0,
+	viewXSent = 0,
+	viewYSent = 0,
+	viewLastUpdate = 0,
 }
 
 Ping = {
@@ -19,6 +29,11 @@ Ping = {
 	payload = nil,
 }
 
+ControllerState = {
+	rightX = 0,
+	rightY = 0
+}
+
 function love.update2()
 	local now = love.timer.getTime()
 	if now - Ping.timeSent > 5 then
@@ -30,7 +45,39 @@ function love.update2()
 		love.mqtt.send("command/ping", Ping.payload)
 		print("Sending ping")
 	end
+
+	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 = -BotState.viewX * 0.3 + 0.5,
+			y = -BotState.viewY * 0.3 + 0.5
+		}))
+		BotState.viewXSent = BotState.viewX
+		BotState.viewYSent = BotState.viewY
+		BotState.viewLastUpdated = now
 	end
+end
+
+function love.joystickaxis2(joystick, axis, value)
+	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
@@ -71,6 +118,28 @@ function love.mqtt.message(topic, payload)
 	end
 end
 
-function love.gamepadpressed(joystick, button)
+function love.gamepadpressed2(joystick, button)
 	print("Pressed gamepad button " .. button .. " on joystick " .. joystick:getName())
+	if button == "back" then
+		UIState.showUI = not UIState.showUI
+	end
+end
+
+function toJSON(arg)
+	local t = type(arg)
+	if t == "number" then
+		return tostring(arg)
+	elseif t == "nil" then
+		return "null"
+	elseif t == "string" then
+		return '"' .. arg .. '"'
+	elseif t == "boolean" then
+		return tostring(arg)
+	elseif t == "table" then
+		local fields = {}
+		for key, value in pairs(arg) do
+			fields[#fields+1] = string.format('"%s":%s', key, toJSON(value))
+		end
+		return '{' .. table.concat(fields, ',') .. '}'
+	end
 end

controller-client/socket-test.lua

@@ -1,19 +0,0 @@
-local socket = require("socket")
-
-local conn = socket.connect("localhost", 1234)
-print("connected")
-
-conn:settimeout(3)
-
-local data, err, part = conn:receive(3)
-print("Data:", data)
-print("Err:", err)
-print("Part:", part)
-
-
-local data, err, part2 = conn:receive(3, part)
-print("Data:", data)
-print("Err:", err)
-print("Part:", part2)
-
-conn:close()

controller-host/main.lua

@@ -9,7 +9,7 @@ local oldPrint = print
 function print(...)
 	local string = ""
 	for _, v in ipairs({...}) do
-		string = string .. v
+		string = string .. tostring(v)
 	end
 	love.mqtt.send("controller/stdout", string)
 	oldPrint(...)
@@ -45,8 +45,8 @@ function love.draw(...)
 		local centerY = love.graphics.getHeight() / 2
 
 		-- Calculate textX and textY
-		--local textX = math.floor(centerX - (font:getWidth(text) / 2))
+		local textX = math.floor(centerX - (font:getWidth(text) / 2))
-		--local textY = math.floor(centerY - (font:getHeight(text) / 2))
+		local textY = math.floor(centerY - (font:getHeight(text) / 2))
 		local textX, textY = 10, 10
 		
 		local realText
@@ -77,6 +77,21 @@ function love.update(...)
 	end
 end
 
+function love.gamepadpressed(joystick, button)
+	if button == "guide" then
+		love.event.quit()
+	end
+	if love.gamepadpressed2 then
+		safeCall(love.gamepadpressed2, joystick, button)
+	end
+end
+
+function love.joystickaxis(joystick, axis, value)
+	if love.joystickaxis2 then
+		safeCall(love.joystickaxis2, joystick, axis, value)
+	end
+end
+
 function love.mqtt.onError(message)
 	print("MQTT error: " .. message)
 end

spider-host/battery.go

@@ -2,18 +2,16 @@ package main
 
 import (
 	"gobot.io/x/gobot/drivers/i2c"
-	"gobot.io/x/gobot/platforms/raspi"
 	"log"
 )
 
-var rpi *raspi.Adaptor
 var ads *ADS7830
 
 //var mpu *i2c.MPU6050Driver
 //mpu = i2c.NewMPU6050Driver(rpi, i2c.WithBus(0), i2c.WithAddress(0x40))
 
 func InitBattery() {
-	rpi = raspi.NewAdaptor()
+	rpi := GetAdaptor()
 	rpi.Connect()
 	ads = NewADS7830(rpi, i2c.WithBus(1), i2c.WithAddress(0x48))
 	ads.Start()

spider-host/main.go

@@ -1,6 +1,7 @@
 package main
 
 import (
+	"encoding/json"
 	"fmt"
 	mqtt "github.com/eclipse/paho.mqtt.golang"
 	"log"
@@ -44,10 +45,48 @@ func onPing(client mqtt.Client, msg mqtt.Message) {
 	publishTelemetry(client, "pong", msg.Payload())
 }
 
+func onSetCameraXY(client mqtt.Client, msg mqtt.Message) {
+	log.Print("Got move camera")
+	payload := make(map[string]float64)
+	err := json.Unmarshal(msg.Payload(), &payload)
+	if err != nil {
+		log.Printf("Error unmarshalling set_camera_xy payload: %v\n", err)
+		return
+	}
+	x, ok := payload["x"]
+	if !ok {
+		log.Printf("Missing x in set_camera_xy")
+		return
+	}
+	y, ok := payload["y"]
+	if !ok {
+		log.Printf("Missing y in set_camera_xy")
+		return
+	}
+	SetServoAngle(ServoHeadHorizontal, x)
+	SetServoAngle(ServoHeadVertical, y)
+}
+
+func subscribe(client mqtt.Client, topic string, handler mqtt.MessageHandler) {
+	token := client.Subscribe(topic, 0, handler)
+	token.Wait()
+	if token.Error() != nil {
+		log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
+	}
+}
+
+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()
@@ -56,23 +95,27 @@ func main() {
 		log.Fatalf("Error connecting to MQTT broker: %v\n", token.Error())
 	}
 
-	log.Print("Subscribing to command topics")
-	token = client.Subscribe("spider/command/ping", 0, onPing)
-	token.Wait()
-	if token.Error() != nil {
-		log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
-	}
-
 	InitBattery()
+	InitServo()
+
+	ServosOff()
 
 	slowTelemetry := time.NewTicker(3 * time.Second)
 	defer slowTelemetry.Stop()
 
+	//moveServo := time.NewTicker(100 * time.Millisecond)
+	//defer moveServo.Stop()
+
 	publishSlowTelemetry(client)
 	for {
 		select {
 		case <-slowTelemetry.C:
 			publishSlowTelemetry(client)
+			//case <-moveServo.C:
+			//	seconds := time.Now().UnixMilli()
+			//	angle := (math.Cos(float64(seconds)/2_500) + 1) * 0.5
+			//	log.Printf("Target angle: %.1f\n", angle)
+			//	SetServoAngle(ServoHeadHorizontal, angle)
 		}
 	}
 }

spider-host/rpi.go

@@ -0,0 +1,12 @@
+package main
+
+import "gobot.io/x/gobot/platforms/raspi"
+
+var rpi *raspi.Adaptor
+
+func GetAdaptor() *raspi.Adaptor {
+	if rpi == nil {
+		rpi = raspi.NewAdaptor()
+	}
+	return rpi
+}

spider-host/servo.go

@@ -0,0 +1,88 @@
+package main
+
+import (
+	"gobot.io/x/gobot/drivers/i2c"
+	"log"
+	"math"
+)
+
+type Servo int
+
+const (
+	ServoHeadHorizontal Servo = iota
+	ServoHeadVertical
+	ServoCount
+)
+
+type ServoChannel struct {
+	controller *i2c.PCA9685Driver
+	channel    int
+}
+
+var servoController1 *i2c.PCA9685Driver
+var servoController2 *i2c.PCA9685Driver
+
+// var servos = new([ServoCount]*gpio.ServoDriver)
+var servos = new([ServoCount]*ServoChannel)
+
+func InitServo() {
+	adaptor := GetAdaptor()
+	servoController1 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x40))
+	servoController2 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x41))
+
+	createServo(ServoHeadHorizontal, servoController2, 1)
+	createServo(ServoHeadVertical, servoController2, 0)
+
+	err := servoController1.Start()
+	if err != nil {
+		log.Print("Could not start servo controller 1: ", err)
+	}
+	err = servoController2.Start()
+	if err != nil {
+		log.Print("Could not start servo controller 1: ", err)
+	}
+
+	err = servoController1.SetPWMFreq(50)
+	if err != nil {
+		log.Print("Could not set servo controller 1 frequency: ", err)
+	}
+	err = servoController2.SetPWMFreq(50)
+	if err != nil {
+		log.Print("Could not set servo controller 2 frequency: ", err)
+	}
+	// Halt the controllers to stop any current movement
+	ServosOff()
+	log.Print("Started servos")
+}
+
+func ServosOff() {
+	err := servoController1.Halt()
+	if err != nil {
+		log.Print("Could not stop servo controller 1: ", err)
+	}
+	err = servoController2.Halt()
+	if err != nil {
+		log.Print("Could not stop servo controller 2: ", err)
+	}
+}
+
+// SetServoAngle Sets the angle to a value of 0 - 1
+func SetServoAngle(servo Servo, angle float64) {
+	pulseDuration := angle + 1
+	pulseDurationRatio := pulseDuration / 20
+	pulseDurationTicks := pulseDurationRatio * 4095
+	servoDriver := servos[servo]
+	err := servoDriver.controller.SetPWM(servoDriver.channel, 0, uint16(math.Floor(pulseDurationTicks)))
+	if err != nil {
+		log.Printf("Could not set servo %d to angle %0.1f: %v\n", servo, angle, err)
+	}
+}
+
+func createServo(servo Servo, pwmDriver *i2c.PCA9685Driver, channel int) {
+	//servoDriver := gpio.NewServoDriver(pwmDriver, channel)
+	//servos[servo] = servoDriver
+	servos[servo] = &ServoChannel{
+		controller: pwmDriver,
+		channel:    channel,
+	}
+}