Some camera controls

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,
+	}
+}