- support for servo blaster

- servo requires the underlying PWM to support a SetMicroseconds method

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)

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 {

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
+}

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))
 }

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)
 	}
 

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)

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
+		}
+	}
+}