[RSDK-2226] audit GPIO servo code (viamrobotics#2283)

Important changes:
- If the frequency is not specified in the config, we've always used the current frequency of the pin. However, if the pin's frequency is currently 0, we now default to a sane value (300 Hz) instead!
- If the frequency is so fast that we cannot move all the way to the max position (because it would require holding the signal high for longer than the period of the PWM), lower the frequency so that we could spend the maximum time high and the minimum time low. 

Unimportant changes:
- Don't capitalize the S in microseconds
- Rename `min` and `max` to `minDeg` and `maxDeg`, to contrast with `minUs` and `maxUs`
- Add more comments!
- Comments: sentences start with capital letters and end with periods. PWM is capitalized.

Tried with 2 different servo motors (a TowerPro  SG-5010 and a Bilda 2000-0025-0503) at my desk. They both work fine when plugged into a Jetson Orin AGX, and also both work fine when plugged into a raspberry pi. I tried plugging them into a PCA9685, but suspect I need to give it an extra power source to drive the motors: I could see the PWM signal change on the oscilloscope, but couldn't get either motor to move.

components/servo/gpio/servo.go

@@ -19,28 +19,33 @@ import (
 const (
 	defaultMinDeg float64 = 0.0
 	defaultMaxDeg float64 = 180.0
-	minWidthUs    uint    = 500  // absolute minimum pwm width
-	maxWidthUs    uint    = 2500 // absolute maximum pwm width
+	minWidthUs    uint    = 500  // absolute minimum PWM width
+	maxWidthUs    uint    = 2500 // absolute maximum PWM width
+	defaultFreq   uint    = 300
 )
 
+// We want to distinguish values that are 0 because the user set them to 0 from ones that are 0
+// because that's the default when the user didn't set them. Consequently, all numerical fields in
+// this struct are pointers. They'll be nil if they were unset, and point to a value (possibly 0!)
+// if they were set.
 type servoConfig struct {
-	Pin   string `json:"pin"`   // Pin a GPIO pin with pwm capabilities
-	Board string `json:"board"` // Board a board that exposes GPIO pins
-	// MinDeg minimum angle the servo can reach, note this doesn't affect PWM calculation
+	Pin   string `json:"pin"`   // Pin is a GPIO pin with PWM capabilities.
+	Board string `json:"board"` // Board is a board that exposes GPIO pins.
+	// MinDeg is the minimum angle the servo can reach. Note this doesn't affect PWM calculation.
 	MinDeg *float64 `json:"min_angle_deg,omitempty"`
-	// MaxDeg maximum angle the servo can reach, note this doesn't affect PWM calculation
+	// MaxDeg is the maximum angle the servo can reach. Note this doesn't affect PWM calculation.
 	MaxDeg *float64 `json:"max_angle_deg,omitempty"`
-	// StartPos starting position of the servo in degree
+	// StartPos is the starting position of the servo in degrees.
 	StartPos *float64 `json:"starting_position_deg,omitempty"`
-	// Frequency when set the servo driver will attempt to change the GPIO pin's Frequency
+	// Frequency at which to drive the PWM
 	Frequency *uint `json:"frequency_hz,omitempty"`
-	// Resolution resolution of the PWM driver (eg number of ticks for a full period) if left or 0
-	// the driver will attempt to estimate the resolution
+	// Resolution of the PWM driver (eg number of ticks for a full period). If omitted or 0, the
+	// driver will attempt to estimate the resolution.
 	Resolution *uint `json:"pwm_resolution,omitempty"`
-	// MinWidthUS override the safe minimum width in us this affect PWM calculation
-	MinWidthUS *uint `json:"min_width_us,omitempty"`
-	// MaxWidthUS Override the safe maximum width in us this affect PWM calculation
-	MaxWidthUS *uint `json:"max_width_us,omitempty"`
+	// MinWidthUs overrides the safe minimum PWM width in microseconds.
+	MinWidthUs *uint `json:"min_width_us,omitempty"`
+	// MaxWidthUs overrides the safe maximum PWM width in microseconds.
+	MaxWidthUs *uint `json:"max_width_us,omitempty"`
 }
 
 // Validate ensures all parts of the config are valid.
@@ -53,6 +58,7 @@ func (config *servoConfig) Validate(path string) ([]string, error) {
 	if config.Pin == "" {
 		return nil, viamutils.NewConfigValidationFieldRequiredError(path, "pin")
 	}
+
 	if config.StartPos != nil {
 		minDeg := defaultMinDeg
 		maxDeg := defaultMaxDeg
@@ -64,16 +70,17 @@ func (config *servoConfig) Validate(path string) ([]string, error) {
 		}
 		if *config.StartPos < minDeg || *config.StartPos > maxDeg {
 			return nil, viamutils.NewConfigValidationError(path,
-				errors.Errorf("starting_position_degs should be between %.1f and %.1f", minDeg, maxDeg))
+				errors.Errorf("starting_position_deg should be between minimum (%.1f) and maximum (%.1f) positions", minDeg, maxDeg))
 		}
 	}
+
 	if config.MinDeg != nil && *config.MinDeg < 0 {
 		return nil, viamutils.NewConfigValidationError(path, errors.New("min_angle_deg cannot be lower than 0"))
 	}
-	if config.MinWidthUS != nil && *config.MinWidthUS < minWidthUs {
+	if config.MinWidthUs != nil && *config.MinWidthUs < minWidthUs {
 		return nil, viamutils.NewConfigValidationError(path, errors.Errorf("min_width_us cannot be lower than %d", minWidthUs))
 	}
-	if config.MaxWidthUS != nil && *config.MaxWidthUS > maxWidthUs {
+	if config.MaxWidthUs != nil && *config.MaxWidthUs > maxWidthUs {
 		return nil, viamutils.NewConfigValidationError(path, errors.Errorf("max_width_us cannot be higher than %d", maxWidthUs))
 	}
 	return deps, nil
@@ -93,8 +100,8 @@ type servoGPIO struct {
 	resource.AlwaysRebuild
 	resource.TriviallyCloseable
 	pin       board.GPIOPin
-	min       float64
-	max       float64
+	minDeg    float64
+	maxDeg    float64
 	logger    golog.Logger
 	opMgr     operation.SingleOperationManager
 	frequency uint
@@ -121,27 +128,11 @@ func newGPIOServo(ctx context.Context, deps resource.Dependencies, conf resource
 		return nil, errors.Wrap(err, "couldn't get servo pin")
 	}
 
-	frequency, err := pin.PWMFreq(ctx, nil)
-	if err != nil {
-		return nil, errors.Wrap(err, "couldn't get servo pin pwm frequency")
-	}
-	if newConf.Frequency != nil {
-		if *newConf.Frequency > 450 || *newConf.Frequency == 0 {
-			return nil, errors.Errorf("PWM frequencies should not be above 450Hz or 0, have %d", frequency)
-		}
-
-		err = pin.SetPWMFreq(ctx, *newConf.Frequency, nil)
-		if err != nil {
-			return nil, errors.Wrap(err, "error setting servo pin frequency")
-		}
-		frequency = *newConf.Frequency
-	}
-
 	minDeg := defaultMinDeg
-	maxDeg := defaultMaxDeg
 	if newConf.MinDeg != nil {
 		minDeg = *newConf.MinDeg
 	}
+	maxDeg := defaultMaxDeg
 	if newConf.MaxDeg != nil {
 		maxDeg = *newConf.MaxDeg
 	}
@@ -150,18 +141,50 @@ func newGPIOServo(ctx context.Context, deps resource.Dependencies, conf resource
 		startPos = *newConf.StartPos
 	}
 	minUs := minWidthUs
+	if newConf.MinWidthUs != nil {
+		minUs = *newConf.MinWidthUs
+	}
 	maxUs := maxWidthUs
-	if newConf.MinWidthUS != nil {
-		minUs = *newConf.MinWidthUS
+	if newConf.MaxWidthUs != nil {
+		maxUs = *newConf.MaxWidthUs
+	}
+
+	// If the frequency isn't specified in the config, we'll use whatever it's currently set to
+	// instead. If it's currently set to 0, we'll default to using 300 Hz.
+	frequency, err := pin.PWMFreq(ctx, nil)
+	if err != nil {
+		return nil, errors.Wrap(err, "couldn't get servo pin pwm frequency")
+	}
+	if frequency == 0 {
+		frequency = defaultFreq
+	}
+	if newConf.Frequency != nil {
+		if *newConf.Frequency > 450 || *newConf.Frequency < 50 {
+			return nil, errors.Errorf(
+				"PWM frequencies should not be above 450Hz or below 50, have %d", newConf.Frequency)
+		}
+
+		frequency = *newConf.Frequency
 	}
-	if newConf.MaxWidthUS != nil {
-		maxUs = *newConf.MaxWidthUS
+
+	// We need the pin to be high for up to maxUs microseconds, plus the motor's deadband width
+	// time spent low before going high again. The deadband width is usually at least 1
+	// microsecond, but rarely over 10. Call it 50 microseconds just to be safe.
+	const maxDeadbandWidthUs = 50
+	if maxFrequency := 1e6 / (maxUs + maxDeadbandWidthUs); frequency > maxFrequency {
+		logger.Warnf("servo frequency (%f.1) is above maximum (%f.1), setting to max instead",
+			frequency, maxFrequency)
+		frequency = maxFrequency
+	}
+
+	if err := pin.SetPWMFreq(ctx, frequency, nil); err != nil {
+		return nil, errors.Wrap(err, "error setting servo pin frequency")
 	}
 
 	servo := &servoGPIO{
 		Named:     conf.ResourceName().AsNamed(),
-		min:       minDeg,
-		max:       maxDeg,
+		minDeg:    minDeg,
+		maxDeg:    maxDeg,
 		frequency: frequency,
 		pin:       pin,
 		logger:    logger,
@@ -170,45 +193,45 @@ func newGPIOServo(ctx context.Context, deps resource.Dependencies, conf resource
 		currPct:   0,
 	}
 
+	// Try to detect the PWM resolution.
 	if err := servo.Move(ctx, uint32(startPos), nil); err != nil {
 		return nil, errors.Wrap(err, "couldn't move servo to start position")
 	}
-	if servo.pwmRes == 0 {
-		if err := servo.findPWMResolution(ctx); err != nil {
-			return nil, errors.Wrap(err, "failed to guess the pwm resolution")
-		}
-		if err := servo.Move(ctx, uint32(startPos), nil); err != nil {
-			return nil, errors.Wrap(err, "couldn't move servo to start position")
-		}
+	if err := servo.findPWMResolution(ctx); err != nil {
+		return nil, errors.Wrap(err, "failed to guess the pwm resolution")
 	}
+	if err := servo.Move(ctx, uint32(startPos), nil); err != nil {
+		return nil, errors.Wrap(err, "couldn't move servo back to start position")
+	}
+
 	return servo, nil
 }
 
-// Given minUs, maxUs, deg and frequency attempt to calculate the corresponding duty cycle pct.
+// Given minUs, maxUs, deg, and frequency attempt to calculate the corresponding duty cycle pct.
 func mapDegToDutyCylePct(minUs, maxUs uint, minDeg, maxDeg, deg float64, frequency uint) float64 {
-	period := 1.0 / float64(frequency) // dutyCycle in s
-	degRange := maxDeg - minDeg        // servo moves from minDeg to maxDeg
-	uSRange := float64(maxUs - minUs)  // pulse width between minUs to maxUs
+	period := 1.0 / float64(frequency)
+	degRange := maxDeg - minDeg
+	uSRange := float64(maxUs - minUs)
 
-	scale := uSRange / degRange
+	usPerDeg := uSRange / degRange
 
-	pwmWidthUs := float64(minUs) + (deg-minDeg)*scale
+	pwmWidthUs := float64(minUs) + (deg-minDeg)*usPerDeg
 	return (pwmWidthUs / (1000 * 1000)) / period
 }
 
-// Given minUs, maxUs, deg and frequency returns the corresponding duty cycle pct.
+// Given minUs, maxUs, duty cycle pct, and frequency returns the position in degrees.
 func mapDutyCylePctToDeg(minUs, maxUs uint, minDeg, maxDeg, pct float64, frequency uint) float64 {
-	period := 1.0 / float64(frequency) // dutyCycle in s
+	period := 1.0 / float64(frequency)
 	pwmWidthUs := pct * period * 1000 * 1000
-	degRange := maxDeg - minDeg       // servo moves from minDeg to maxDeg
-	uSRange := float64(maxUs - minUs) // pulse width between minUs to maxUs
+	degRange := maxDeg - minDeg
+	uSRange := float64(maxUs - minUs)
 
 	pwmWidthUs = math.Max(float64(minUs), pwmWidthUs)
 	pwmWidthUs = math.Min(float64(maxUs), pwmWidthUs)
 
-	scale := degRange / uSRange
+	degsPerUs := degRange / uSRange
 
-	return math.Round(minDeg + (pwmWidthUs-float64(minUs))*scale)
+	return math.Round(minDeg + (pwmWidthUs-float64(minUs))*degsPerUs)
 }
 
 // Attempt to find the PWM resolution assuming a hardware PWM
@@ -225,8 +248,10 @@ func (s *servoGPIO) findPWMResolution(ctx context.Context) error {
 	currPct := s.currPct
 	realPct, err := s.pin.PWM(ctx, nil)
 	if err != nil {
-		return errors.Wrap(err, "couldn't find PWM resolution")
+		return errors.Wrap(err, "cannot find PWM resolution")
 	}
+
+	// The direction will be towards whichever extreme duration (minUs or maxUs) is farther away.
 	dir := 1.0
 	lDist := s.currPct*periodUs - float64(s.minUs)
 	rDist := float64(s.maxUs) - s.currPct*periodUs
@@ -248,10 +273,11 @@ func (s *servoGPIO) findPWMResolution(ctx context.Context) error {
 			return errors.Errorf("giving up searching for the resolution tried to match %.7f but got %.7f", realPct, r2)
 		}
 	}
+
 	resolution := []int{16, 15, 14, 12, 8}
 	for _, r := range resolution {
 		val := (1 << r) - 1
-		pct := currPct + dir*1/float64(val)
+		pct := currPct + dir/float64(val)
 		err := s.pin.SetPWM(ctx, pct, nil)
 		if err != nil {
 			return errors.Wrap(err, "couldn't search for PWM resolution")
@@ -262,7 +288,7 @@ func (s *servoGPIO) findPWMResolution(ctx context.Context) error {
 		realPct, err := s.pin.PWM(ctx, nil)
 		s.logger.Debugf("starting step %d currPct %.7f target Pct %.14f realPct %.14f", val, currPct, pct, realPct)
 		if err != nil {
-			return errors.Wrap(err, "couldn't find PWM find servo PWM resolution")
+			return errors.Wrap(err, "couldn't find servo PWM resolution")
 		}
 		if realPct != currPct {
 			if realPct == pct {
@@ -285,13 +311,13 @@ func (s *servoGPIO) Move(ctx context.Context, ang uint32, extra map[string]inter
 	ctx, done := s.opMgr.New(ctx)
 	defer done()
 	angle := float64(ang)
-	if angle < s.min {
-		angle = s.min
+	if angle < s.minDeg {
+		angle = s.minDeg
 	}
-	if angle > s.max {
-		angle = s.max
+	if angle > s.maxDeg {
+		angle = s.maxDeg
 	}
-	pct := mapDegToDutyCylePct(s.minUs, s.maxUs, s.min, s.max, angle, s.frequency)
+	pct := mapDegToDutyCylePct(s.minUs, s.maxUs, s.minDeg, s.maxDeg, angle, s.frequency)
 	if s.pwmRes != 0 {
 		realTick := math.Round(pct * float64(s.pwmRes))
 		pct = realTick / float64(s.pwmRes)
@@ -309,13 +335,15 @@ func (s *servoGPIO) Position(ctx context.Context, extra map[string]interface{})
 	if err != nil {
 		return 0, errors.Wrap(err, "couldn't get servo pin duty cycle")
 	}
-	return uint32(mapDutyCylePctToDeg(s.minUs, s.maxUs, s.min, s.max, pct, s.frequency)), nil
+	return uint32(mapDutyCylePctToDeg(s.minUs, s.maxUs, s.minDeg, s.maxDeg, pct, s.frequency)), nil
 }
 
 // Stop stops the servo. It is assumed the servo stops immediately.
 func (s *servoGPIO) Stop(ctx context.Context, extra map[string]interface{}) error {
 	ctx, done := s.opMgr.New(ctx)
 	defer done()
+	// Turning the pin all the way off (i.e., setting the duty cycle to 0%) will cut power to the
+	// motor. If you wanted to send it to position 0, you should set it to `minUs` instead.
 	if err := s.pin.SetPWM(ctx, 0.0, nil); err != nil {
 		return errors.Wrap(err, "couldn't stop servo")
 	}

components/servo/gpio/servo_test.go

@@ -26,8 +26,8 @@ func TestValidate(t *testing.T) {
 		MinDeg:     ptr(1.5),
 		MaxDeg:     ptr(90.0),
 		StartPos:   ptr(3.5),
-		MinWidthUS: ptr(uint(501)),
-		MaxWidthUS: ptr(uint(2499)),
+		MinWidthUs: ptr(uint(501)),
+		MaxWidthUs: ptr(uint(2499)),
 	}
 
 	deps, err := cfg.Validate("test")
@@ -43,31 +43,31 @@ func TestValidate(t *testing.T) {
 
 	cfg.MaxDeg = ptr(90.0)
 
-	cfg.MinWidthUS = ptr(uint(450))
+	cfg.MinWidthUs = ptr(uint(450))
 	_, err = cfg.Validate("test")
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(), test.ShouldContainSubstring, "error validating \"test\": min_width_us cannot be lower than 500")
-	cfg.MinWidthUS = ptr(uint(501))
+	cfg.MinWidthUs = ptr(uint(501))
 
-	cfg.MaxWidthUS = ptr(uint(2520))
+	cfg.MaxWidthUs = ptr(uint(2520))
 	_, err = cfg.Validate("test")
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(), test.ShouldContainSubstring, "error validating \"test\": max_width_us cannot be higher than 2500")
-	cfg.MaxWidthUS = ptr(uint(2499))
+	cfg.MaxWidthUs = ptr(uint(2499))
 
 	cfg.StartPos = ptr(91.0)
 	_, err = cfg.Validate("test")
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(),
 		test.ShouldContainSubstring,
-		"error validating \"test\": starting_position_degs should be between 1.5 and 90.0")
+		"error validating \"test\": starting_position_deg should be between minimum (1.5) and maximum (90.0) positions")
 
 	cfg.StartPos = ptr(1.0)
 	_, err = cfg.Validate("test")
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(),
 		test.ShouldContainSubstring,
-		"error validating \"test\": starting_position_degs should be between 1.5 and 90.0")
+		"error validating \"test\": starting_position_deg should be between minimum (1.5) and maximum (90.0) positions")
 
 	cfg.StartPos = ptr(199.0)
 	cfg.MaxDeg = nil
@@ -76,7 +76,7 @@ func TestValidate(t *testing.T) {
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(),
 		test.ShouldContainSubstring,
-		"error validating \"test\": starting_position_degs should be between 0.0 and 180.0")
+		"error validating \"test\": starting_position_deg should be between minimum (0.0) and maximum (180.0) positions")
 
 	cfg.StartPos = ptr(0.0)
 	_, err = cfg.Validate("test")
@@ -122,6 +122,10 @@ func setupDependencies(t *testing.T) resource.Dependencies {
 		innerTick1 = utils.ScaleByPct(scale1, dutyCyclePct)
 		return nil
 	}
+	pin0.SetPWMFreqFunc = func(ctx context.Context, freqHz uint, extra map[string]interface{}) error {
+		return nil
+	}
+
 	pin1 := &inject.GPIOPin{}
 	pin1.PWMFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) {
 		pct := float64(innerTick2) / float64(scale2)
@@ -134,6 +138,10 @@ func setupDependencies(t *testing.T) resource.Dependencies {
 		innerTick2 = utils.ScaleByPct(scale2, dutyCyclePct)
 		return nil
 	}
+	pin1.SetPWMFreqFunc = func(ctx context.Context, freqHz uint, extra map[string]interface{}) error {
+		return nil
+	}
+
 	board1.GPIOPinByNameFunc = func(name string) (board.GPIOPin, error) {
 		switch name {
 		case "0":