diff --git a/bbb.go b/bbb.go index 61534d6..793565e 100644 --- a/bbb.go +++ b/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 { diff --git a/controller/pca9685/pca9685.go b/controller/pca9685/pca9685.go index 050be3c..9a9407d 100644 --- a/controller/pca9685/pca9685.go +++ b/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. diff --git a/controller/servoblaster/servoblaster.go b/controller/servoblaster/servoblaster.go index 6784e75..c9c3460 100644 --- a/controller/servoblaster/servoblaster.go +++ b/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 } diff --git a/gpio.go b/gpio.go index eadffa3..fc1c5c3 100644 --- a/gpio.go +++ b/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 } diff --git a/motion/servo/servo.go b/motion/servo/servo.go index 4b7fa05..c0876d2 100644 --- a/motion/servo/servo.go +++ b/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)) } diff --git a/samples/.gitignore b/samples/.gitignore index 3e9e014..b5bdec4 100644 --- a/samples/.gitignore +++ b/samples/.gitignore @@ -15,6 +15,7 @@ mcp4725 pca9685 pwm servo +servobbb servoblaster tmp006 us020 diff --git a/samples/servo.go b/samples/servo.go index 80876a3..27b48a5 100644 --- a/samples/servo.go +++ b/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) diff --git a/samples/servobbb.go b/samples/servobbb.go new file mode 100644 index 0000000..13361a1 --- /dev/null +++ b/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 + } + } +} diff --git a/samples/servoblaster.go b/samples/servoblaster.go index 2d7626b..b752ebb 100644 --- a/samples/servoblaster.go +++ b/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)