From 5593924945d7acd856467c25e273707473cdbd49 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20Agsj=C3=B6?= Date: Tue, 24 Jan 2017 00:40:33 +0100 Subject: [PATCH] Added PWM0 support to c.h.i.p MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Erik Agsjö Updated C.H.I.P README Signed-off-by: Erik Agsjö --- platforms/chip/README.md | 3 + platforms/chip/chip_adaptor.go | 46 +++++++- platforms/chip/chip_pwm.go | 188 +++++++++++++++++++++++++++++++++ 3 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 platforms/chip/chip_pwm.go diff --git a/platforms/chip/README.md b/platforms/chip/README.md index 7805c0a0..0bae7099 100644 --- a/platforms/chip/README.md +++ b/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. diff --git a/platforms/chip/chip_adaptor.go b/platforms/chip/chip_adaptor.go index 4558083c..54c7dc2d 100644 --- a/platforms/chip/chip_adaptor.go +++ b/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 diff --git a/platforms/chip/chip_pwm.go b/platforms/chip/chip_pwm.go new file mode 100644 index 00000000..369da341 --- /dev/null +++ b/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 +}