legop3/MultiRoombaRover
1
0
Fork 0
mirror of https://github.com/legop3/MultiRoombaRover.git synced 2026-09-15 17:12:59 -04:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

pi side servo stuff

Browse Source
This commit is contained in:
legop3
2025-11-17 18:18:46 -05:00
parent ab15ce7e17
commit 9b552fb857
10 changed files with 309 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dist/servoverifier
Vendored
BIN
View File
Binary file not shown.
pi/roverd/camera_servo.go
+143
View File
@@ -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
+32
View File
@@ -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
+10 -1
View File
@@ -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
+7
View File
@@ -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
+72 -7
View File
@@ -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")
pi/roverd/roverd.sample.yaml
+12
View File
@@ -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
+12
View File
@@ -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
+21 -1
View File
@@ -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
-2
View File
@@ -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]