pi side servo stuff

pi/roverd/camera_servo.go

@@ -0,0 +1,143 @@
+//go:build !dummy
+
+package roverd
+
+import (
+	"fmt"
+	"log"
+	"math"
+	"sync"
+
+	rpio "github.com/stianeikeland/go-rpio/v4"
+)
+
+type CameraServo struct {
+	cfg          CameraServoConfig
+	logger       *log.Logger
+	pin          rpio.Pin
+	mu           sync.Mutex
+	currentAngle float64
+	closed       bool
+}
+
+func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
+	if !cfg.Enabled {
+		return nil, fmt.Errorf("camera servo disabled")
+	}
+	if err := rpio.Open(); err != nil {
+		return nil, fmt.Errorf("open gpio: %w", err)
+	}
+
+	pin := rpio.Pin(cfg.Pin)
+	pin.Mode(rpio.Pwm)
+	targetClock := cfg.FreqHz * cfg.CycleLen
+	pin.Freq(targetClock)
+
+	servo := &CameraServo{
+		cfg:    cfg,
+		logger: logger,
+		pin:    pin,
+	}
+	if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
+		rpio.Close()
+		return nil, err
+	}
+	logger.Printf("camera servo initialized on GPIO %d (%.1f..%.1f deg)", cfg.Pin, cfg.MinAngle, cfg.MaxAngle)
+	return servo, nil
+}
+
+func (s *CameraServo) Close() {
+	s.mu.Lock()
+	defer s.mu.Unlock()
+	if s.closed {
+		return
+	}
+	s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle))
+	rpio.Close()
+	s.closed = true
+}
+
+func (s *CameraServo) SetAngle(angle float64) error {
+	s.mu.Lock()
+	defer s.mu.Unlock()
+	return s.setAngleLocked(angle)
+}
+
+func (s *CameraServo) setAngleLocked(angle float64) error {
+	if s.closed {
+		return fmt.Errorf("servo closed")
+	}
+	clamped := clampFloat(angle, s.cfg.MinAngle, s.cfg.MaxAngle)
+	s.applyPulseLocked(s.angleToPulse(clamped))
+	s.currentAngle = clamped
+	return nil
+}
+
+func (s *CameraServo) Nudge(delta float64) error {
+	s.mu.Lock()
+	defer s.mu.Unlock()
+	if delta == 0 {
+		delta = s.cfg.NudgeDegrees
+	}
+	target := s.currentAngle + delta
+	return s.setAngleLocked(target)
+}
+
+func (s *CameraServo) SetPulseWidth(micros int) error {
+	s.mu.Lock()
+	defer s.mu.Unlock()
+	if s.closed {
+		return fmt.Errorf("servo closed")
+	}
+	if !s.cfg.AllowRawPulse {
+		return fmt.Errorf("raw pulse commands disabled")
+	}
+	if micros <= 0 {
+		return fmt.Errorf("pulse width must be > 0")
+	}
+	s.applyPulseLocked(micros)
+	s.currentAngle = s.pulseToAngle(micros)
+	return nil
+}
+
+func (s *CameraServo) CurrentAngle() float64 {
+	s.mu.Lock()
+	defer s.mu.Unlock()
+	return s.currentAngle
+}
+
+func (s *CameraServo) applyPulseLocked(micros int) {
+	micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
+	s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen))
+}
+
+func (s *CameraServo) angleToPulse(angle float64) int {
+	totalRange := s.cfg.MaxAngle - s.cfg.MinAngle
+	if totalRange == 0 {
+		return s.cfg.MinPulseUs
+	}
+	norm := (angle - s.cfg.MinAngle) / totalRange
+	norm = math.Max(0, math.Min(1, norm))
+	pulseRange := s.cfg.MaxPulseUs - s.cfg.MinPulseUs
+	return s.cfg.MinPulseUs + int(math.Round(norm*float64(pulseRange)))
+}
+
+func (s *CameraServo) pulseToAngle(pulse int) float64 {
+	pulseRange := s.cfg.MaxPulseUs - s.cfg.MinPulseUs
+	if pulseRange == 0 {
+		return s.cfg.MinAngle
+	}
+	norm := float64(pulse-s.cfg.MinPulseUs) / float64(pulseRange)
+	norm = math.Max(0, math.Min(1, norm))
+	return s.cfg.MinAngle + norm*(s.cfg.MaxAngle-s.cfg.MinAngle)
+}
+
+func clampInt(value, min, max int) int {
+	if value < min {
+		return min
+	}
+	if value > max {
+		return max
+	}
+	return value
+}

pi/roverd/camera_servo_dummy.go

@@ -0,0 +1,32 @@
+//go:build dummy
+
+package roverd
+
+import (
+	"fmt"
+	"log"
+)
+
+type CameraServo struct{}
+
+func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
+	return nil, fmt.Errorf("camera servo not supported in dummy build")
+}
+
+func (c *CameraServo) Close() {}
+
+func (c *CameraServo) SetAngle(angle float64) error {
+	return fmt.Errorf("camera servo disabled")
+}
+
+func (c *CameraServo) Nudge(delta float64) error {
+	return fmt.Errorf("camera servo disabled")
+}
+
+func (c *CameraServo) SetPulseWidth(micros int) error {
+	return fmt.Errorf("camera servo disabled")
+}
+
+func (c *CameraServo) CurrentAngle() float64 {
+	return 0
+}

pi/roverd/cmd/roverd/main.go

@@ -60,10 +60,19 @@ func main() {
 		mediaSupervisor.Start(ctx)
 	}
 
+	var cameraServo *roverd.CameraServo
+	if cfg.CameraServo.Enabled {
+		cameraServo, err = roverd.NewCameraServo(cfg.CameraServo, logger)
+		if err != nil {
+			logger.Fatalf("init camera servo: %v", err)
+		}
+		defer cameraServo.Close()
+	}
+
 	autoCharge := roverd.NewAutoChargeController(adapter, eventStream, logger)
 	go autoCharge.Run(ctx, sensorSamples)
 
-	client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, logger)
+	client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, cameraServo, logger)
 
 	retryDelay := time.Second
 	for ctx.Err() == nil {

pi/roverd/command.go

@@ -22,6 +22,7 @@ type inboundMessage struct {
 	Raw          string               `json:"raw,omitempty"`
 	SensorStream *sensorStreamPayload `json:"sensorStream,omitempty"`
 	Media        *mediaCommand        `json:"media,omitempty"`
+	Servo        *servoPayload        `json:"servo,omitempty"`
 }
 
 type driveDirectPayload struct {
@@ -43,6 +44,12 @@ type mediaCommand struct {
 	Action string `json:"action"`
 }
 
+type servoPayload struct {
+	Angle   *float64 `json:"angle,omitempty"`
+	Nudge   *float64 `json:"nudge,omitempty"`
+	PulseUs *int     `json:"pulseUs,omitempty"`
+}
+
 type ackMessage struct {
 	Type   string `json:"type"`
 	ID     string `json:"id"`

pi/roverd/config.go

@@ -66,14 +66,29 @@ type MediaConfig struct {
 	VideoBitrate   int      `yaml:"videoBitrate" json:"-"`
 }
 
+type CameraServoConfig struct {
+	Enabled       bool    `yaml:"enabled"`
+	Pin           int     `yaml:"pin"`
+	FreqHz        int     `yaml:"freqHz"`
+	CycleLen      int     `yaml:"cycleLen"`
+	MinPulseUs    int     `yaml:"minPulseUs"`
+	MaxPulseUs    int     `yaml:"maxPulseUs"`
+	MinAngle      float64 `yaml:"minAngle"`
+	MaxAngle      float64 `yaml:"maxAngle"`
+	HomeAngle     float64 `yaml:"homeAngle"`
+	NudgeDegrees  float64 `yaml:"nudgeDegrees"`
+	AllowRawPulse bool    `yaml:"allowRawPulse"`
+}
+
 type Config struct {
-	Name        string        `yaml:"name"`
+	Name        string            `yaml:"name"`
-	ServerURL   string        `yaml:"serverUrl"`
+	ServerURL   string            `yaml:"serverUrl"`
-	Serial      SerialConfig  `yaml:"serial"`
+	Serial      SerialConfig      `yaml:"serial"`
-	BRC         BRCConfig     `yaml:"brc"`
+	BRC         BRCConfig         `yaml:"brc"`
-	Battery     BatteryConfig `yaml:"battery"`
+	Battery     BatteryConfig     `yaml:"battery"`
-	MaxWheelMMs int           `yaml:"maxWheelSpeed"`
+	MaxWheelMMs int               `yaml:"maxWheelSpeed"`
-	Media       MediaConfig   `yaml:"media"`
+	Media       MediaConfig       `yaml:"media"`
+	CameraServo CameraServoConfig `yaml:"cameraServo"`
 }
 
 func LoadConfig(path string) (*Config, error) {
@@ -101,6 +116,17 @@ func LoadConfig(path string) (*Config, error) {
 			VideoFPS:       30,
 			VideoBitrate:   3000000,
 		},
+		CameraServo: CameraServoConfig{
+			Pin:          19,
+			FreqHz:       50,
+			CycleLen:     20000,
+			MinPulseUs:   900,
+			MaxPulseUs:   2100,
+			MinAngle:     -15,
+			MaxAngle:     30,
+			HomeAngle:    0,
+			NudgeDegrees: 2,
+		},
 	}
 	if err := yaml.Unmarshal(data, &cfg); err != nil {
 		return nil, err
@@ -151,9 +177,48 @@ func LoadConfig(path string) (*Config, error) {
 		}
 		cfg.Media.PublishURL = derived
 	}
+	if err := validateServoConfig(&cfg.CameraServo); err != nil {
+		return nil, fmt.Errorf("cameraServo: %w", err)
+	}
 	return &cfg, nil
 }
 
+func validateServoConfig(cfg *CameraServoConfig) error {
+	if !cfg.Enabled {
+		return nil
+	}
+	if cfg.Pin <= 0 {
+		return errors.New("pin must be > 0")
+	}
+	if cfg.FreqHz <= 0 {
+		return errors.New("freqHz must be > 0")
+	}
+	if cfg.CycleLen <= 0 {
+		return errors.New("cycleLen must be > 0")
+	}
+	if cfg.MinPulseUs <= 0 || cfg.MaxPulseUs <= 0 || cfg.MinPulseUs >= cfg.MaxPulseUs {
+		return errors.New("minPulseUs/maxPulseUs invalid")
+	}
+	if cfg.MinAngle >= cfg.MaxAngle {
+		return errors.New("minAngle must be less than maxAngle")
+	}
+	cfg.HomeAngle = clampFloat(cfg.HomeAngle, cfg.MinAngle, cfg.MaxAngle)
+	if cfg.NudgeDegrees <= 0 {
+		cfg.NudgeDegrees = 2
+	}
+	return nil
+}
+
+func clampFloat(value, min, max float64) float64 {
+	if value < min {
+		return min
+	}
+	if value > max {
+		return max
+	}
+	return value
+}
+
 func derivePublishURL(serverURL, roverName string, port int) (string, error) {
 	if roverName == "" {
 		return "", errors.New("missing rover name for publishUrl")

pi/roverd/roverd.sample.yaml

@@ -25,3 +25,15 @@ media:
   service: video-publisher.service
   healthUrl: ""
   healthInterval: 30s
+cameraServo:
+  enabled: false
+  pin: 19
+  freqHz: 50
+  cycleLen: 20000
+  minPulseUs: 900
+  maxPulseUs: 2100
+  minAngle: -15
+  maxAngle: 30
+  homeAngle: 0
+  nudgeDegrees: 2
+  allowRawPulse: false

pi/roverd/roverd.yaml

@@ -19,3 +19,15 @@ media:
   service: mediamtx.service
   healthUrl: http://127.0.0.1:9997/v3/paths/list
   healthInterval: 30s
+cameraServo:
+  enabled: false
+  pin: 19
+  freqHz: 50
+  cycleLen: 20000
+  minPulseUs: 900
+  maxPulseUs: 2100
+  minAngle: -15
+  maxAngle: 30
+  homeAngle: 0
+  nudgeDegrees: 2
+  allowRawPulse: false

pi/roverd/wsclient.go

@@ -17,16 +17,18 @@ type WSClient struct {
 	sensorFrames <-chan []byte
 	events       <-chan RoverEvent
 	media        *MediaSupervisor
+	servo        *CameraServo
 	log          *log.Logger
 }
 
-func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events <-chan RoverEvent, media *MediaSupervisor, logger *log.Logger) *WSClient {
+func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events <-chan RoverEvent, media *MediaSupervisor, servo *CameraServo, logger *log.Logger) *WSClient {
 	return &WSClient{
 		cfg:          cfg,
 		adapter:      adapter,
 		sensorFrames: frames,
 		events:       events,
 		media:        media,
+		servo:        servo,
 		log:          logger,
 	}
 }
@@ -141,11 +143,29 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
 			return fmt.Errorf("media supervisor disabled")
 		}
 		return c.media.HandleAction(ctx, msg.Media.Action)
+	case msg.Servo != nil:
+		if c.servo == nil {
+			return fmt.Errorf("camera servo disabled")
+		}
+		return c.handleServoCommand(msg.Servo)
 	default:
 		return fmt.Errorf("unsupported command type: %s", msg.Type)
 	}
 }
 
+func (c *WSClient) handleServoCommand(payload *servoPayload) error {
+	switch {
+	case payload.Angle != nil:
+		return c.servo.SetAngle(*payload.Angle)
+	case payload.Nudge != nil:
+		return c.servo.Nudge(*payload.Nudge)
+	case payload.PulseUs != nil:
+		return c.servo.SetPulseWidth(*payload.PulseUs)
+	default:
+		return fmt.Errorf("servo command requires angle, nudge, or pulseUs")
+	}
+}
+
 func (c *WSClient) forwardSensors(ctx context.Context, conn *websocket.Conn) {
 	for {
 		select {

pi/systemd/roverd.service

@@ -8,8 +8,6 @@ Type=simple
 ExecStart=/usr/local/bin/roverd -config /etc/roverd.yaml
 Restart=on-failure
 RestartSec=5
-User=roverd
-Group=roverd
 AmbientCapabilities=CAP_SYS_TTY_CONFIG CAP_SYS_RAWIO
 
 [Install]