unified servo and analog output support for pwms

bbb.go

@@ -17,6 +17,9 @@ import (
 	"strconv"
 	"strings"
 	"time"
+
+	"github.com/golang/glog"
+	"github.com/kidoman/embd/util"
 )
 
 var bbbPins = PinMap{
@@ -244,19 +247,22 @@ const (
 	// BBBPWMDefaultDuty represents the default duty (0ns) for pwm.
 	BBBPWMDefaultDuty = 0
 
-	// BBBPWMDefaultPeriod represents the default period (500000ns) for pwm.
+	// BBBPWMDefaultPeriod represents the default period (500000ns) for pwm. Equals 2000 Hz.
 	BBBPWMDefaultPeriod = 500000
 
-	// BBBPWMMaxPulseWidth represents the max period (1000000000ns) supported by pwm.
+	// BBBPWMMaxPulseWidth represents the max period (1000000000ns) supported by pwm. Equals 1 Hz.
 	BBBPWMMaxPulseWidth = 1000000000
 )
 
 type bbbPWMPin struct {
 	n string
 
-	duty     *os.File
-	period   *os.File
-	polarity *os.File
+	period   int
+	polarity Polarity
+
+	dutyf     *os.File
+	periodf   *os.File
+	polarityf *os.File
 
 	initialized bool
 }
@@ -292,18 +298,22 @@ func (p *bbbPWMPin) init() error {
 	if err := p.ensurePeriodFileExists(basePath, 500*time.Millisecond); err != nil {
 		return err
 	}
-	if p.period, err = p.periodFile(basePath); err != nil {
+	if p.periodf, err = p.periodFile(basePath); err != nil {
 		return err
 	}
-	if p.duty, err = p.dutyFile(basePath); err != nil {
+	if p.dutyf, err = p.dutyFile(basePath); err != nil {
 		return err
 	}
-	if p.polarity, err = p.polarityFile(basePath); err != nil {
+	if p.polarityf, err = p.polarityFile(basePath); err != nil {
 		return err
 	}
 
 	p.initialized = true
 
+	if err := p.reset(); err != nil {
+		return err
+	}
+
 	return nil
 }
 
@@ -372,8 +382,14 @@ func (p *bbbPWMPin) SetPeriod(ns int) error {
 		return fmt.Errorf("embd: pwm period for %v is out of bounds (must be =< %vns)", p.n, BBBPWMMaxPulseWidth)
 	}
 
-	_, err := p.period.WriteString(strconv.Itoa(ns))
-	return err
+	_, err := p.periodf.WriteString(strconv.Itoa(ns))
+	if err != nil {
+		return err
+	}
+
+	p.period = ns
+
+	return nil
 }
 
 func (p *bbbPWMPin) SetDuty(ns int) error {
@@ -382,11 +398,35 @@ func (p *bbbPWMPin) SetDuty(ns int) error {
 	}
 
 	if ns > BBBPWMMaxPulseWidth {
-		return fmt.Errorf("embd: pwm duty for %v is out of bounds (must be =< %vns)", p.n, BBBPWMMaxPulseWidth)
+		return fmt.Errorf("embd: pwm duty %v for pin %v is out of bounds (must be =< %vns)", p.n, BBBPWMMaxPulseWidth)
 	}
 
-	_, err := p.duty.WriteString(strconv.Itoa(ns))
-	return err
+	if ns > p.period {
+		return fmt.Errorf("embd: pwm duty %v for pin %v is greater than the period %v", ns, p.n, p.period)
+	}
+
+	_, err := p.dutyf.WriteString(strconv.Itoa(ns))
+	if err != nil {
+		return err
+	}
+
+	return nil
+}
+
+func (p *bbbPWMPin) SetMicroseconds(us int) error {
+	if p.period != 20000000 {
+		glog.Warningf("embd: pwm pin %v has freq %v hz. recommended 50 hz for servo mode", 1000000000/p.period)
+	}
+	duty := us * 1000 // in nanoseconds
+	if duty > p.period {
+		return fmt.Errorf("embd: calculated pwm duty %vns for pin %v (servo mode) is greater than the period %vns", duty, p.n, p.period)
+	}
+	return p.SetDuty(duty)
+}
+
+func (p *bbbPWMPin) SetAnalog(value byte) error {
+	duty := util.Map(int64(value), 0, 255, 0, int64(p.period))
+	return p.SetDuty(int(duty))
 }
 
 func (p *bbbPWMPin) SetPolarity(pol Polarity) error {
@@ -394,8 +434,14 @@ func (p *bbbPWMPin) SetPolarity(pol Polarity) error {
 		return err
 	}
 
-	_, err := p.polarity.WriteString(strconv.Itoa(int(pol)))
-	return err
+	_, err := p.polarityf.WriteString(strconv.Itoa(int(pol)))
+	if err != nil {
+		return err
+	}
+
+	p.polarity = pol
+
+	return nil
 }
 
 func (p *bbbPWMPin) reset() error {

controller/pca9685/pca9685.go

@@ -20,9 +20,7 @@ const (
 
 	pwm0OnLowReg = 0x6
 
-	minAnalogValue = 0
-	maxAnalogValue = 255
-
+	// inspired by arduino's default freq for analogWrites
 	defaultFreq = 490
 )
 
@@ -140,17 +138,35 @@ func (d *PCA9685) SetPwm(channel, onTime, offTime int) error {
 	return nil
 }
 
-// SetMicroseconds is a convinience method which allows easy servo control.
-func (d *PCA9685) SetMicroseconds(channel, us int) error {
-	offTime := us * d.Freq * pwmControlPoints / 1000000
-	return d.SetPwm(channel, 0, offTime)
+type pwmChannel struct {
+	d *PCA9685
+
+	channel int
+}
+
+func (p *pwmChannel) SetMicroseconds(us int) error {
+	return p.d.setMicroseconds(p.channel, us)
 }
 
 // SetAnalog is a convinience method which allows easy manipulation of the PWM
 // based on a (0-255) range value.
-func (d *PCA9685) SetAnalog(channel int, value byte) error {
-	offTime := util.Map(int64(value), minAnalogValue, maxAnalogValue, 0, pwmControlPoints-1)
-	return d.SetPwm(channel, 0, int(offTime))
+func (p *pwmChannel) SetAnalog(value byte) error {
+	offTime := util.Map(int64(value), 0, 255, 0, pwmControlPoints-1)
+	return p.d.SetPwm(p.channel, 0, int(offTime))
+}
+
+func (d *PCA9685) ServoChannel(channel int) *pwmChannel {
+	return &pwmChannel{d: d, channel: channel}
+}
+
+func (d *PCA9685) AnalogChannel(channel int) *pwmChannel {
+	return &pwmChannel{d: d, channel: channel}
+}
+
+// SetMicroseconds is a convinience method which allows easy servo control.
+func (d *PCA9685) setMicroseconds(channel, us int) error {
+	offTime := us * d.Freq * pwmControlPoints / 1000000
+	return d.SetPwm(channel, 0, offTime)
 }
 
 // Close stops the controller and resets mode and pwm controller registers.

controller/servoblaster/servoblaster.go

@@ -33,8 +33,22 @@ func (d *ServoBlaster) setup() error {
 	return nil
 }
 
+type pwmChannel struct {
+	d *ServoBlaster
+
+	channel int
+}
+
+func (p *pwmChannel) SetMicroseconds(us int) error {
+	return p.d.setMicroseconds(p.channel, us)
+}
+
+func (d *ServoBlaster) Channel(channel int) *pwmChannel {
+	return &pwmChannel{d: d, channel: channel}
+}
+
 // SetMicroseconds sends a command to the PWM driver to generate a us wide pulse.
-func (d *ServoBlaster) SetMicroseconds(channel, us int) error {
+func (d *ServoBlaster) setMicroseconds(channel, us int) error {
 	if err := d.setup(); err != nil {
 		return err
 	}

gpio.go

@@ -86,6 +86,12 @@ type PWMPin interface {
 	// SetPolarity sets the polarity of a pwm pin.
 	SetPolarity(pol Polarity) error
 
+	// SetMicroseconds sends a command to the PWM driver to generate a us wide pulse.
+	SetMicroseconds(us int) error
+
+	// SetAnalog allows easy manipulation of the PWM based on a (0-255) range value.
+	SetAnalog(value byte) error
+
 	// Close releases the resources associated with the pin.
 	Close() error
 }

motion/servo/servo.go

@@ -11,25 +11,28 @@ const (
 	maxus = 2400
 )
 
+const (
+	// DefaultFreq represents the default (preferred) freq of a PWM doing servo duties.
+	DefaultFreq = 50
+)
+
 // A PWM interface implements access to a pwm controller.
 type PWM interface {
-	SetMicroseconds(channel int, us int) error
+	SetMicroseconds(us int) error
 }
 
 type Servo struct {
-	PWM     PWM
-	Channel int
+	PWM PWM
 
 	Minus, Maxus int
 }
 
 // New creates a new Servo interface.
-func New(pwm PWM, channel int) *Servo {
+func New(pwm PWM) *Servo {
 	return &Servo{
-		PWM:     pwm,
-		Channel: channel,
-		Minus:   minus,
-		Maxus:   maxus,
+		PWM:   pwm,
+		Minus: minus,
+		Maxus: maxus,
 	}
 }
 
@@ -39,5 +42,5 @@ func (s *Servo) SetAngle(angle int) error {
 
 	glog.V(1).Infof("servo: given angle %v calculated %v us", angle, us)
 
-	return s.PWM.SetMicroseconds(s.Channel, int(us))
+	return s.PWM.SetMicroseconds(int(us))
 }

samples/.gitignore

@@ -15,6 +15,7 @@ mcp4725
 pca9685
 pwm
 servo
+servobbb
 servoblaster
 tmp006
 us020

samples/servo.go

@@ -20,11 +20,13 @@ func main() {
 
 	bus := embd.NewI2CBus(1)
 
-	pwm := pca9685.New(bus, 0x41)
-	pwm.Freq = 50
-	defer pwm.Close()
+	d := pca9685.New(bus, 0x41)
+	d.Freq = 50
+	defer d.Close()
 
-	servo := servo.New(pwm, 0)
+	pwm := d.ServoChannel(0)
+
+	servo := servo.New(pwm)
 
 	c := make(chan os.Signal, 1)
 	signal.Notify(c, os.Interrupt, os.Kill)

samples/servobbb.go

@@ -0,0 +1,51 @@
+// +build ignore
+
+package main
+
+import (
+	"os"
+	"os/signal"
+	"time"
+
+	"github.com/kidoman/embd"
+	"github.com/kidoman/embd/motion/servo"
+)
+
+func main() {
+	embd.InitGPIO()
+	defer embd.CloseGPIO()
+
+	pwm, err := embd.NewPWMPin("P9_14")
+	if err != nil {
+		panic(err)
+	}
+	defer pwm.Close()
+
+	servo := servo.New(pwm)
+
+	c := make(chan os.Signal, 1)
+	signal.Notify(c, os.Interrupt, os.Kill)
+
+	turnTimer := time.Tick(500 * time.Millisecond)
+	left := true
+
+	servo.SetAngle(90)
+	defer func() {
+		servo.SetAngle(90)
+	}()
+
+	for {
+		select {
+		case <-turnTimer:
+			left = !left
+			switch left {
+			case true:
+				servo.SetAngle(70)
+			case false:
+				servo.SetAngle(110)
+			}
+		case <-c:
+			return
+		}
+	}
+}

samples/servoblaster.go

@@ -15,7 +15,9 @@ func main() {
 	sb := servoblaster.New()
 	defer sb.Close()
 
-	servo := servo.New(sb, 0)
+	pwm := sb.Channel(0)
+
+	servo := servo.New(pwm)
 
 	c := make(chan os.Signal, 1)
 	signal.Notify(c, os.Interrupt, os.Kill)