Some camera controls

controller-client/main.lua

@@ -23,6 +23,13 @@ Ping = {
 	payload = nil,
 }
 
+ControllerState = {
+	viewX = 0,
+	viewY = 0,
+	viewChanged = false,
+	viewLastUpdated = 0
+}
+
 function love.update2()
 	local now = love.timer.getTime()
 	if now - Ping.timeSent > 5 then
@@ -34,6 +41,24 @@ function love.update2()
 		love.mqtt.send("command/ping", Ping.payload)
 		print("Sending ping")
 	end
+	if ControllerState.viewChanged and (now - ControllerState.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
+		}))
+		ControllerState.viewChanged = false
+		ControllerState.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
+	end
 end
 
 function formatSafe(format, value, ...)
@@ -81,3 +106,22 @@ function love.gamepadpressed2(joystick, button)
 		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-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(...)
@@ -86,6 +86,12 @@ function love.gamepadpressed(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,41 @@ 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 main() {
 	opts := mqtt.NewClientOptions()
 	opts.AddBroker(broker)
 	opts.SetClientID("spider-host-client")
+	opts.SetResumeSubs(true)
 	client := mqtt.NewClient(opts)
 
 	token := client.Connect()
@@ -57,22 +89,30 @@ func main() {
 	}
 
 	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())
-	}
+	subscribe(client, "spider/command/ping", onPing)
+	subscribe(client, "spider/command/set_camera_xy", onSetCameraXY)
 
 	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,87 @@
+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()
+}
+
+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,
+	}
+}