diff --git a/dist/roverd b/dist/roverd index a30fe3ce..ddd4a458 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index ae8b549b..6633c19c 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/camera_servo.go b/pi/roverd/camera_servo.go index 0571403f..69b3aac3 100644 --- a/pi/roverd/camera_servo.go +++ b/pi/roverd/camera_servo.go @@ -18,11 +18,16 @@ type CameraServo struct { pin rpio.Pin mu sync.Mutex currentAngle float64 + desiredAngle float64 lastMove time.Time + moving bool + stopCh chan struct{} closed bool } -const maxServoDegPerSec = 10.0 +const maxServoDegPerSec = 60.0 +const servoStepInterval = 20 * time.Millisecond +const servoAngleEpsilon = 0.01 func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) { if !cfg.Enabled { @@ -41,6 +46,7 @@ func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, er cfg: cfg, logger: logger, pin: pin, + stopCh: make(chan struct{}), } if err := servo.setAngleLocked(cfg.HomeAngle); err != nil { rpio.Close() @@ -58,6 +64,10 @@ func (s *CameraServo) Close() { } s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle)) rpio.Close() + if s.stopCh != nil { + close(s.stopCh) + s.stopCh = nil + } s.closed = true } @@ -72,9 +82,13 @@ func (s *CameraServo) setAngleLocked(angle float64) error { return fmt.Errorf("servo closed") } clamped := clampFloat(angle, s.cfg.MinAngle, s.cfg.MaxAngle) + s.desiredAngle = clamped limited := s.rateLimitAngleLocked(clamped) s.applyPulseLocked(s.angleToPulse(limited)) s.currentAngle = limited + if math.Abs(limited-s.desiredAngle) > servoAngleEpsilon { + s.startMoveLoopLocked() + } return nil } @@ -102,9 +116,13 @@ func (s *CameraServo) SetPulseWidth(micros int) error { } clampedPulse := clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs) targetAngle := s.pulseToAngle(clampedPulse) + s.desiredAngle = targetAngle limited := s.rateLimitAngleLocked(targetAngle) s.applyPulseLocked(s.angleToPulse(limited)) s.currentAngle = limited + if math.Abs(limited-s.desiredAngle) > servoAngleEpsilon { + s.startMoveLoopLocked() + } return nil } @@ -119,6 +137,39 @@ func (s *CameraServo) applyPulseLocked(micros int) { s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen)) } +func (s *CameraServo) startMoveLoopLocked() { + if s.moving || s.stopCh == nil { + return + } + s.moving = true + go func() { + ticker := time.NewTicker(servoStepInterval) + defer ticker.Stop() + for { + select { + case <-ticker.C: + s.mu.Lock() + if s.closed { + s.moving = false + s.mu.Unlock() + return + } + if math.Abs(s.currentAngle-s.desiredAngle) <= servoAngleEpsilon { + s.moving = false + s.mu.Unlock() + return + } + limited := s.rateLimitAngleLocked(s.desiredAngle) + s.applyPulseLocked(s.angleToPulse(limited)) + s.currentAngle = limited + s.mu.Unlock() + case <-s.stopCh: + return + } + } + }() +} + func (s *CameraServo) rateLimitAngleLocked(target float64) float64 { now := time.Now() if s.lastMove.IsZero() {