From 9b552fb8571ce3edcc6988ca8001d5453fd624ea Mon Sep 17 00:00:00 2001 From: legop3 Date: Mon, 17 Nov 2025 18:18:46 -0500 Subject: [PATCH] pi side servo stuff --- dist/servoverifier | Bin 1769656 -> 1769656 bytes pi/roverd/camera_servo.go | 143 ++++++++++++++++++++++++++++++++ pi/roverd/camera_servo_dummy.go | 32 +++++++ pi/roverd/cmd/roverd/main.go | 11 ++- pi/roverd/command.go | 7 ++ pi/roverd/config.go | 79 ++++++++++++++++-- pi/roverd/roverd.sample.yaml | 12 +++ pi/roverd/roverd.yaml | 12 +++ pi/roverd/wsclient.go | 22 ++++- pi/systemd/roverd.service | 2 - 10 files changed, 309 insertions(+), 11 deletions(-) create mode 100644 pi/roverd/camera_servo.go create mode 100644 pi/roverd/camera_servo_dummy.go diff --git a/dist/servoverifier b/dist/servoverifier index 3a8a5ca02942a2d4edd4a5a9643f60f7736d8cee..5259d26bf94f47ab964f271a176c2168fb4c8e24 100755 GIT binary patch delta 319 zcmdndn6slXXF~_SW>SfByji}vTU2T-q;aJ|j((y?a87_}>)ZZ33Ka0$gnZ+-(9pZ34V) z0(@-({A~gP+XMvVJv0)N3{8_$%~K7{(+muZ%n~gv(o8Ikjg8VwO;giSQq7W0QY=i( zEi9(HM+nNX7#dlbv}Z;L04X;F@6p1y0mN0F|NL4kR3d8S8JQDuH* zx#i@I{9=|7p#h2Io{_-@;h8?>PJRXDg{FoKEDQ_`B0$Ux#O{8f3}&h;KFmDPc4*mw z7n@RdUS{jd&j@c4XcJ&;6JTl+U~Us&X%k>=6JTo-U~d!PXcORU6X0qS;BFJ(X%paW z6X0tT;BONU*d`z-@1bFmlwz4`WSW+2l9FnWnw*xFXlZC{nwFMgXlR;fYLH@Rl9p@; zl$h=wAt=XUU}R;`o*5ws#6mzU48$TpEDFS8Kr9Z#65BH)Btuq`VeoXI!TQ_Hn53Hi F0RVB`VTk|$ diff --git a/pi/roverd/camera_servo.go b/pi/roverd/camera_servo.go new file mode 100644 index 00000000..d1ad08ac --- /dev/null +++ b/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 +} diff --git a/pi/roverd/camera_servo_dummy.go b/pi/roverd/camera_servo_dummy.go new file mode 100644 index 00000000..5e407ae6 --- /dev/null +++ b/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 +} diff --git a/pi/roverd/cmd/roverd/main.go b/pi/roverd/cmd/roverd/main.go index db257c71..aa4c0b0c 100644 --- a/pi/roverd/cmd/roverd/main.go +++ b/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 { diff --git a/pi/roverd/command.go b/pi/roverd/command.go index 2bb3a494..17b060fe 100644 --- a/pi/roverd/command.go +++ b/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"` diff --git a/pi/roverd/config.go b/pi/roverd/config.go index e32dcfe8..f7eba1e3 100644 --- a/pi/roverd/config.go +++ b/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"` - ServerURL string `yaml:"serverUrl"` - Serial SerialConfig `yaml:"serial"` - BRC BRCConfig `yaml:"brc"` - Battery BatteryConfig `yaml:"battery"` - MaxWheelMMs int `yaml:"maxWheelSpeed"` - Media MediaConfig `yaml:"media"` + Name string `yaml:"name"` + ServerURL string `yaml:"serverUrl"` + Serial SerialConfig `yaml:"serial"` + BRC BRCConfig `yaml:"brc"` + Battery BatteryConfig `yaml:"battery"` + MaxWheelMMs int `yaml:"maxWheelSpeed"` + 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") diff --git a/pi/roverd/roverd.sample.yaml b/pi/roverd/roverd.sample.yaml index d990146f..dfcba7ab 100644 --- a/pi/roverd/roverd.sample.yaml +++ b/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 diff --git a/pi/roverd/roverd.yaml b/pi/roverd/roverd.yaml index ed1deddd..3457688f 100644 --- a/pi/roverd/roverd.yaml +++ b/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 diff --git a/pi/roverd/wsclient.go b/pi/roverd/wsclient.go index 8c4849be..375518b9 100644 --- a/pi/roverd/wsclient.go +++ b/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 { diff --git a/pi/systemd/roverd.service b/pi/systemd/roverd.service index 33bdd301..41250ba2 100644 --- a/pi/systemd/roverd.service +++ b/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]