Added PWM0 support to c.h.i.p

Signed-off-by: Erik Agsjö <erik.agsjo@gmail.com>

Updated C.H.I.P README

Signed-off-by: Erik Agsjö <erik.agsjo@gmail.com>

platforms/chip/README.md

@@ -11,6 +11,9 @@ For documentation about the C.H.I.P. platform click [here](http://docs.getchip.c
 go get -d -u gobot.io/x/gobot/... && go install gobot.io/x/gobot/platforms/chip
 ```
 
+Note that PWM might not be available in your kernel and you might need to load the right overlays
+to expose PWM on the PWM0 pin.
+
 ## How to Use
 
 The pin numbering used by your Gobot program should match the way your board is labeled right on the board itself.

platforms/chip/chip_adaptor.go

@@ -20,6 +20,7 @@ type Adaptor struct {
 	digitalPins map[int]sysfs.DigitalPin
 	pinMap      map[string]int
 	i2cBuses    [3]sysfs.I2cDevice
+	pwm         *pwmControl
 }
 
 var fixedPins = map[string]int{
@@ -92,11 +93,17 @@ func (c *Adaptor) SetName(n string) { c.name = n }
 
 // Connect initializes the board
 func (c *Adaptor) Connect() (err error) {
-	return
+	return nil
 }
 
 // Finalize closes connection to board and pins
 func (c *Adaptor) Finalize() (err error) {
+	if c.pwm != nil {
+		if e := c.closePWM(); e != nil {
+			err = multierror.Append(err, e)
+		}
+		c.pwm = nil
+	}
 	for _, pin := range c.digitalPins {
 		if pin != nil {
 			if e := pin.Unexport(); e != nil {
@@ -195,6 +202,43 @@ func (c *Adaptor) GetDefaultBus() int {
 	return 1
 }
 
+// PwmWrite writes a PWM signal to the specified pin
+func (c *Adaptor) PwmWrite(pin string, val byte) (err error) {
+	if pin != "PWM0" {
+		return fmt.Errorf("PWM is only available on pin PWM0")
+	}
+	if c.pwm == nil {
+		err = c.initPWM(pwmFrequency)
+		if err != nil {
+			return
+		}
+	}
+	duty := gobot.ToScale(gobot.FromScale(float64(val), 0, 255), 0, 100)
+	return c.pwm.setDutycycle(duty)
+}
+
+const pwmFrequency = 100
+
+// ServoWrite writes a servo signal to the specified pin
+func (c *Adaptor) ServoWrite(pin string, angle byte) (err error) {
+	if pin != "PWM0" {
+		return fmt.Errorf("Servo is only available on pin PWM0")
+	}
+	if c.pwm == nil {
+		err = c.initPWM(pwmFrequency)
+		if err != nil {
+			return
+		}
+	}
+	// 0.5 ms => -90
+	// 1.5 ms =>   0
+	// 2.0 ms =>  90
+	const minDuty = 100 * 0.0005 * pwmFrequency
+	const maxDuty = 100 * 0.0020 * pwmFrequency
+	duty := gobot.ToScale(gobot.FromScale(float64(angle), 0, 180), minDuty, maxDuty)
+	return c.pwm.setDutycycle(duty)
+}
+
 func getXIOBase() (baseAddr int, err error) {
 	// Default to original base from 4.3 kernel
 	baseAddr = 408

platforms/chip/chip_pwm.go

@@ -0,0 +1,188 @@
+package chip
+
+import (
+	"fmt"
+	"io"
+	"os"
+)
+
+const pwmSysfsPath = "/sys/class/pwm/pwmchip0"
+
+type pwmControl struct {
+	periodFile   *os.File
+	dutyFile     *os.File
+	polarityFile *os.File
+	enableFile   *os.File
+
+	duty        uint32
+	periodNanos uint32
+	enabled     bool
+}
+
+func exportPWM() (err error) {
+	exporter, err := os.OpenFile(pwmSysfsPath+"/export", os.O_WRONLY, 0666)
+	if err != nil {
+		return err
+	}
+	_, err = io.WriteString(exporter, "0")
+	return err
+}
+
+func unexportPWM() (err error) {
+	exporter, err := os.OpenFile(pwmSysfsPath+"/unexport", os.O_WRONLY, 0666)
+	if err != nil {
+		return err
+	}
+	_, err = io.WriteString(exporter, "0")
+	return err
+}
+
+//	return fmt.Errorf("PWM is not available, check device tree setup")
+
+func (c *Adaptor) initPWM(pwmFrequency float64) (err error) {
+	const basePath = pwmSysfsPath + "/pwm0"
+
+	if _, err = os.Stat(basePath); err != nil {
+		if os.IsNotExist(err) {
+			if err = exportPWM(); err != nil {
+				return
+			}
+		} else {
+			return
+		}
+	}
+
+	var enableFile *os.File
+	var periodFile *os.File
+	var dutyFile *os.File
+	var polarityFile *os.File
+
+	defer func() {
+		if enableFile != nil {
+			enableFile.Close()
+		}
+		if periodFile != nil {
+			periodFile.Close()
+		}
+		if dutyFile != nil {
+			dutyFile.Close()
+		}
+		if polarityFile != nil {
+			polarityFile.Close()
+		}
+	}()
+
+	if enableFile, err = os.OpenFile(basePath+"/enable", os.O_WRONLY, 0666); err != nil {
+		return
+	}
+	if periodFile, err = os.OpenFile(basePath+"/period", os.O_WRONLY, 0666); err != nil {
+		return
+	}
+	if dutyFile, err = os.OpenFile(basePath+"/duty_cycle", os.O_WRONLY, 0666); err != nil {
+		return
+	}
+	if polarityFile, err = os.OpenFile(basePath+"/polarity", os.O_WRONLY, 0666); err != nil {
+		return
+	}
+
+	c.pwm = &pwmControl{
+		enableFile:   enableFile,
+		periodFile:   periodFile,
+		dutyFile:     dutyFile,
+		polarityFile: polarityFile,
+	}
+
+	enableFile = nil
+	periodFile = nil
+	dutyFile = nil
+	polarityFile = nil
+
+	// Set up some sane PWM defaults to make servo functions
+	// work out of the box.
+	if err = c.pwm.setPolarityInverted(false); err != nil {
+		return
+	}
+	if err = c.pwm.setEnable(true); err != nil {
+		return
+	}
+	if err = c.pwm.setFrequency(pwmFrequency); err != nil {
+		return
+	}
+	if err = c.pwm.setDutycycle(0); err != nil {
+		return
+	}
+
+	return nil
+}
+
+func (c *Adaptor) closePWM() error {
+	pwm := c.pwm
+	if pwm != nil {
+		pwm.setFrequency(0)
+		pwm.setDutycycle(0)
+		pwm.setEnable(false)
+
+		if pwm.enableFile != nil {
+			pwm.enableFile.Close()
+		}
+		if pwm.periodFile != nil {
+			pwm.periodFile.Close()
+		}
+		if pwm.dutyFile != nil {
+			pwm.dutyFile.Close()
+		}
+		if pwm.polarityFile != nil {
+			pwm.polarityFile.Close()
+		}
+		if err := unexportPWM(); err != nil {
+			return err
+		}
+		c.pwm = nil
+	}
+	return nil
+}
+
+func (p *pwmControl) setPolarityInverted(invPolarity bool) error {
+	if !p.enabled {
+		polarityString := "normal"
+		if invPolarity {
+			polarityString = "inverted"
+		}
+		_, err := io.WriteString(p.polarityFile, polarityString)
+		return err
+	}
+	return fmt.Errorf("Cannot set PWM polarity when enabled")
+}
+
+func (p *pwmControl) setDutycycle(duty float64) error {
+	p.duty = uint32((float64(p.periodNanos) * (duty / 100.0)))
+	if p.enabled {
+		//fmt.Printf("PWM: Setting duty cycle to %v (%v)\n", p.duty, duty)
+		_, err := io.WriteString(p.dutyFile, fmt.Sprintf("%v", p.duty))
+		return err
+	}
+	return fmt.Errorf("Cannot set PWM duty cycle when disabled")
+}
+
+func (p *pwmControl) setFrequency(freq float64) error {
+	periodNanos := uint32(1e9 / freq)
+	if p.enabled && (p.periodNanos != periodNanos) {
+		p.periodNanos = periodNanos
+		_, err := io.WriteString(p.periodFile, fmt.Sprintf("%v", periodNanos))
+		return err
+	}
+	return fmt.Errorf("Cannot set PWM frequency when disabled")
+}
+
+func (p *pwmControl) setEnable(enabled bool) error {
+	if p.enabled != enabled {
+		p.enabled = enabled
+		enableVal := 0
+		if enabled {
+			enableVal = 1
+		}
+		_, err := io.WriteString(p.enableFile, fmt.Sprintf("%v", enableVal))
+		return err
+	}
+	return nil
+}