diff --git a/README.md b/README.md index 625bbf3..14e9408 100644 --- a/README.md +++ b/README.md @@ -35,3 +35,5 @@ Use various sensors on the RaspberryPi with Golang (like a ninja!) * **PCA9685** 16-channel, 12-bit PWM Controller with I2C protocol [Documentation](http://godoc.org/github.com/kid0m4n/go-rpi/controller/pca9685), [Datasheet](http://www.adafruit.com/datasheets/PCA9685.pdf), [Product Page](http://www.adafruit.com/products/815) * **MCP4725** 12-bit DAC [Documentation](http://godoc.org/github.com/kid0m4n/go-rpi/controller/mcp4725), [Datasheet](http://www.adafruit.com/datasheets/mcp4725.pdf), [Product Page](http://www.adafruit.com/products/935) + +* **ServoBlaster** RPi PWM/PCM based PWM controller [Documentation](http://godoc.org/github.com/kid0m4n/go-rpi/controller/servoblaster), [Product Page](https://github.com/richardghirst/PiBits/tree/master/ServoBlaster) diff --git a/controller/pca9685/pca9685.go b/controller/pca9685/pca9685.go index 135c054..5dfeea3 100644 --- a/controller/pca9685/pca9685.go +++ b/controller/pca9685/pca9685.go @@ -149,6 +149,11 @@ func (d *PCA9685) SetPwm(channel, onTime, offTime int) (err error) { return } +func (d *PCA9685) SetMicroseconds(channel, us int) (err error) { + offTime := us * d.Freq * pwmControlPoints / 1000000 + return d.SetPwm(channel, 0, offTime) +} + // Close stops the controller and resets mode and pwm controller registers. func (d *PCA9685) Close() (err error) { if err = d.setup(); err != nil { diff --git a/controller/servoblaster/servoblaster.go b/controller/servoblaster/servoblaster.go new file mode 100644 index 0000000..d827fa5 --- /dev/null +++ b/controller/servoblaster/servoblaster.go @@ -0,0 +1,56 @@ +// Package servoblaster allows interfacing with the software servoblaster driver. +// +// More details on ServoBlaster at: https://github.com/richardghirst/PiBits/tree/master/ServoBlaster +package servoblaster + +import ( + "fmt" + "log" + "os" +) + +// ServoBlaster represents a software RPi PWM/PCM based servo control module. +type ServoBlaster struct { + initialized bool + fd *os.File + + // Debug level. + Debug bool +} + +// New creates a new ServoBlaster instance. +func New() *ServoBlaster { + return &ServoBlaster{} +} + +func (d *ServoBlaster) setup() (err error) { + if d.initialized { + return + } + if d.fd, err = os.OpenFile("/dev/servoblaster", os.O_WRONLY, os.ModeExclusive); err != nil { + return + } + d.initialized = true + return +} + +// SetMicroseconds sends a command to the PWM driver to generate a us wide pulse. +func (d *ServoBlaster) SetMicroseconds(channel, us int) (err error) { + if err = d.setup(); err != nil { + return + } + cmd := fmt.Sprintf("%v=%vus\n", channel, us) + if d.Debug { + log.Printf("servoblaster: sending command %q", cmd) + } + _, err = d.fd.WriteString(cmd) + return +} + +// Close closes the open driver handle. +func (d *ServoBlaster) Close() (err error) { + if d.fd != nil { + err = d.fd.Close() + } + return +} diff --git a/motion/servo/servo.go b/motion/servo/servo.go index fa49e98..ec2f4f3 100644 --- a/motion/servo/servo.go +++ b/motion/servo/servo.go @@ -9,43 +9,35 @@ import ( // A PWM interface implements access to a pwm controller. type PWM interface { - SetPwm(channel int, onTime int, offTime int) error + SetMicroseconds(channel int, us int) error } type Servo struct { PWM PWM - Freq int Channel int - Minms, Maxms float64 + Minus, Maxus int Debug bool } // New creates a new Servo interface. -// pwm: instance of a PWM controller. -// freq: Frequency of pwm signal (typically 50Hz) -// channel: PWM channel of the pwm controller to be used -// minms: Pulse width corresponding to servo position of 0deg -// maxms: Pulse width corresponding to servo position of 180deg -func New(pwm PWM, freq, channel int, minms, maxms float64) *Servo { +func New(pwm PWM, channel int, minus, maxus int) *Servo { return &Servo{ PWM: pwm, - Freq: freq, Channel: channel, - Minms: minms, - Maxms: maxms, + Minus: minus, + Maxus: maxus, } } // SetAngle sets the servo angle. func (s *Servo) SetAngle(angle int) error { - us := util.Map(int64(angle), 0, 180, int64(s.Minms*1000), int64(s.Maxms*1000)) - offTime := int(us) * s.Freq * 4096 / 1000000 + us := util.Map(int64(angle), 0, 180, int64(s.Minus), int64(s.Maxus)) if s.Debug { - log.Printf("servo: given angle %v calculated %v us offTime %v", angle, us, offTime) + log.Printf("servo: given angle %v calculated %v us", angle, us) } - return s.PWM.SetPwm(s.Channel, 0, offTime) + return s.PWM.SetMicroseconds(s.Channel, int(us)) } diff --git a/samples/pca9685.go b/samples/pca9685.go index cff6229..33cb993 100644 --- a/samples/pca9685.go +++ b/samples/pca9685.go @@ -16,11 +16,11 @@ func main() { log.Panic(err) } - pca9685 := pca9685.New(bus, 0x42, 1000) + pca9685 := pca9685.New(bus, 0x41, 1000) pca9685.Debug = true defer pca9685.Close() - if err := pca9685.SetPwm(0, 0, 2000); err != nil { + if err := pca9685.SetPwm(15, 0, 2000); err != nil { log.Panic(err) } diff --git a/samples/servo.go b/samples/servo.go index d479a96..67c95c1 100644 --- a/samples/servo.go +++ b/samples/servo.go @@ -21,7 +21,7 @@ func main() { pwm.Debug = true defer pwm.Close() - servo := servo.New(pwm, 50, 15, 1, 2.5) + servo := servo.New(pwm, 0, 500, 2500) servo.Debug = true c := make(chan os.Signal, 1) diff --git a/samples/servoblaster.go b/samples/servoblaster.go new file mode 100644 index 0000000..b01a995 --- /dev/null +++ b/samples/servoblaster.go @@ -0,0 +1,50 @@ +package main + +import ( + "log" + "os" + "os/signal" + "time" + + "github.com/kid0m4n/go-rpi/controller/servoblaster" + "github.com/kid0m4n/go-rpi/motion/servo" +) + +func main() { + sb := servoblaster.New() + sb.Debug = true + defer sb.Close() + + servo := servo.New(sb, 0, 500, 2500) + servo.Debug = true + + 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 + var err error + switch left { + case true: + err = servo.SetAngle(45) + case false: + err = servo.SetAngle(135) + } + if err != nil { + log.Panic(err) + } + case <-c: + return + } + } +}