From 17029a0a1496e5809d1f885e3f9c4377b2bd5601 Mon Sep 17 00:00:00 2001 From: Sebastiaan de Schaetzen Date: Sun, 21 Jul 2024 13:40:45 +0200 Subject: [PATCH] Some camera controls --- controller-client/main.lua | 44 +++++++++++++++++++ controller-host/main.lua | 8 +++- spider-host/battery.go | 4 +- spider-host/main.go | 50 +++++++++++++++++++--- spider-host/rpi.go | 12 ++++++ spider-host/servo.go | 87 ++++++++++++++++++++++++++++++++++++++ 6 files changed, 196 insertions(+), 9 deletions(-) create mode 100644 spider-host/rpi.go create mode 100644 spider-host/servo.go diff --git a/controller-client/main.lua b/controller-client/main.lua index 81dc88c..4e1f5e8 100644 --- a/controller-client/main.lua +++ b/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 diff --git a/controller-host/main.lua b/controller-host/main.lua index 11f9fb0..93d11bc 100644 --- a/controller-host/main.lua +++ b/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 diff --git a/spider-host/battery.go b/spider-host/battery.go index 2f8c777..e3ce3e6 100644 --- a/spider-host/battery.go +++ b/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() diff --git a/spider-host/main.go b/spider-host/main.go index 70e03ef..1cd6ae9 100644 --- a/spider-host/main.go +++ b/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) } } } diff --git a/spider-host/rpi.go b/spider-host/rpi.go new file mode 100644 index 0000000..50ae6bf --- /dev/null +++ b/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 +} diff --git a/spider-host/servo.go b/spider-host/servo.go new file mode 100644 index 0000000..1d51f78 --- /dev/null +++ b/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, + } +}