pca9685: Add support for pwm module (#296)

experimental/devices/pca9685/doc.go

@@ -0,0 +1,16 @@
+// Copyright 2018 The Periph Authors. All rights reserved.
+// Use of this source code is governed under the Apache License, Version 2.0
+// that can be found in the LICENSE file.
+
+// Package pca9685 includes utilities to controls pca9685 module and servo motors.
+//
+// More details
+//
+// Datasheet
+//
+// https://www.nxp.com/docs/en/data-sheet/PCA9685.pdf
+//
+// Product page:
+//
+// https://www.nxp.com/products/analog/interfaces/ic-bus/ic-led-controllers/16-channel-12-bit-pwm-fm-plus-ic-bus-led-controller:PCA9685
+package pca9685

experimental/devices/pca9685/example_test.go

@@ -0,0 +1,63 @@
+// Copyright 2018 The Periph Authors. All rights reserved.
+// Use of this source code is governed under the Apache License, Version 2.0
+// that can be found in the LICENSE file.
+
+package pca9685_test
+
+import (
+	"log"
+
+	"periph.io/x/periph/conn/i2c/i2creg"
+	"periph.io/x/periph/conn/physic"
+	"periph.io/x/periph/experimental/devices/pca9685"
+	"periph.io/x/periph/host"
+)
+
+func Example() {
+	_, err := host.Init()
+	if err != nil {
+		log.Fatal(err)
+	}
+
+	bus, err := i2creg.Open("")
+	if err != nil {
+		log.Fatal(err)
+	}
+
+	pca, err := pca9685.NewI2C(bus, pca9685.I2CAddr)
+	if err != nil {
+		log.Fatal(err)
+	}
+
+	if err := pca.SetPwmFreq(50 * physic.Hertz); err != nil {
+		log.Fatal(err)
+	}
+	if err := pca.SetAllPwm(0, 0); err != nil {
+		log.Fatal(err)
+	}
+	servos := pca9685.NewServoGroup(pca, 50, 650, 0, 180)
+
+	// This is an example of using with an Me Arm robot arm
+	gripServo := servos.GetServo(0)
+	baseServo := servos.GetServo(1)
+	elbowServo := servos.GetServo(2)
+	shoulderServo := servos.GetServo(3)
+
+	gripServo.SetMinMaxAngle(15, 120)
+	elbowServo.SetMinMaxAngle(50, 110)    // Set limit of the robot arm
+	shoulderServo.SetMinMaxAngle(60, 140) // Set limit of the robot arm
+
+	// Set all in the middle in a MeArm robot arm
+	if err := gripServo.SetAngle(90); err != nil {
+		log.Fatal(err)
+	}
+	if err := baseServo.SetAngle(90); err != nil {
+		log.Fatal(err)
+	}
+	if err := elbowServo.SetAngle(90); err != nil {
+		log.Fatal(err)
+	}
+	if err := shoulderServo.SetAngle(90); err != nil {
+		log.Fatal(err)
+	}
+}

experimental/devices/pca9685/pca9685.go

@@ -0,0 +1,150 @@
+// Copyright 2018 The Periph Authors. All rights reserved.
+// Use of this source code is governed under the Apache License, Version 2.0
+// that can be found in the LICENSE file.
+
+package pca9685
+
+import (
+	"time"
+
+	"periph.io/x/periph/conn/gpio"
+	"periph.io/x/periph/conn/i2c"
+	"periph.io/x/periph/conn/physic"
+)
+
+// I2CAddr i2c default address.
+const I2CAddr uint16 = 0x40
+
+// PCA9685 Commands
+const (
+	mode1      byte = 0x00
+	mode2      byte = 0x01
+	subAdr1    byte = 0x02
+	subAdr2    byte = 0x03
+	subAdr3    byte = 0x04
+	prescale   byte = 0xFE
+	led0OnL    byte = 0x06
+	led0OnH    byte = 0x07
+	led0OffL   byte = 0x08
+	led0OffH   byte = 0x09
+	allLedOnL  byte = 0xFA
+	allLedOnH  byte = 0xFB
+	allLedOffL byte = 0xFC
+	allLedOffH byte = 0xFD
+
+	// Bits
+	restart byte = 0x80
+	sleep   byte = 0x10
+	allCall byte = 0x01
+	invrt   byte = 0x10
+	outDrv  byte = 0x04
+)
+
+// Dev is a handler to pca9685 controller
+type Dev struct {
+	dev *i2c.Dev
+}
+
+// NewI2C returns a Dev object that communicates over I2C.
+//
+// To use on the default address, pca9685.I2CAddr must be passed as argument.
+func NewI2C(bus i2c.Bus, address uint16) (*Dev, error) {
+	dev := &Dev{
+		dev: &i2c.Dev{Bus: bus, Addr: address},
+	}
+	err := dev.init()
+	if err != nil {
+		return nil, err
+	}
+
+	return dev, nil
+}
+
+func (d *Dev) init() error {
+	if err := d.SetAllPwm(0, 0); err != nil {
+		return err
+	}
+
+	if _, err := d.dev.Write([]byte{mode2, outDrv}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{mode1, allCall}); err != nil {
+		return err
+	}
+
+	time.Sleep(100 * time.Millisecond)
+
+	modeRead := [1]byte{}
+	if err := d.dev.Tx([]byte{mode1}, modeRead[:]); err != nil {
+		return err
+	}
+
+	mode := modeRead[0] & ^sleep
+	if _, err := d.dev.Write([]byte{mode1, mode}); err != nil {
+		return err
+	}
+
+	time.Sleep(5 * time.Millisecond)
+
+	return d.SetPwmFreq(50 * physic.Hertz)
+}
+
+// SetPwmFreq set the pwm frequency
+func (d *Dev) SetPwmFreq(freqHz physic.Frequency) error {
+	p := (25*physic.MegaHertz/4096 + freqHz/2) / freqHz
+
+	modeRead := [1]byte{}
+	if err := d.dev.Tx([]byte{mode1}, modeRead[:]); err != nil {
+		return err
+	}
+
+	oldmode := modeRead[0]
+	if _, err := d.dev.Write([]byte{mode1, byte((oldmode & 0x7F) | 0x10)}); err != nil { // go to sleep;
+		return err
+	}
+	if _, err := d.dev.Write([]byte{prescale, byte(p)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{mode1, oldmode}); err != nil {
+		return err
+	}
+
+	time.Sleep(100 * time.Millisecond)
+
+	_, err := d.dev.Write([]byte{mode1, (byte)(oldmode | 0x80)})
+	return err
+}
+
+// SetAllPwm set a pwm value for all outputs
+func (d *Dev) SetAllPwm(on, off gpio.Duty) error {
+	if _, err := d.dev.Write([]byte{allLedOnL, byte(on)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{allLedOnH, byte(on >> 8)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{allLedOffL, byte(off)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{allLedOffH, byte(off >> 8)}); err != nil {
+		return err
+	}
+	return nil
+}
+
+// SetPwm set a pwm value for given pca9685 channel
+func (d *Dev) SetPwm(channel int, on, off gpio.Duty) error {
+	if _, err := d.dev.Write([]byte{led0OnL + byte(4*channel), byte(on)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{led0OnH + byte(4*channel), byte(on >> 8)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{led0OffL + byte(4*channel), byte(off)}); err != nil {
+		return err
+	}
+	if _, err := d.dev.Write([]byte{led0OffH + byte(4*channel), byte(off >> 8)}); err != nil {
+		return err
+	}
+	return nil
+}

experimental/devices/pca9685/servo.go

@@ -0,0 +1,90 @@
+// Copyright 2018 The Periph Authors. All rights reserved.
+// Use of this source code is governed under the Apache License, Version 2.0
+// that can be found in the LICENSE file.
+
+package pca9685
+
+import (
+	"periph.io/x/periph/conn/gpio"
+	"periph.io/x/periph/conn/physic"
+)
+
+// ServoGroup a group of servos connected to a pca9685 module
+type ServoGroup struct {
+	*Dev
+	minPwm   gpio.Duty
+	maxPwm   gpio.Duty
+	minAngle physic.Angle
+	maxAngle physic.Angle
+}
+
+// Servo individual servo from a group of servos connected to a pca9685 module
+type Servo struct {
+	group    *ServoGroup
+	channel  int
+	minAngle physic.Angle
+	maxAngle physic.Angle
+}
+
+// NewServoGroup returns a servo group connected throught the pca9685 module
+// some pwm and angle limits can be set
+func NewServoGroup(dev *Dev, minPwm, maxPwm gpio.Duty, minAngle, maxAngle physic.Angle) *ServoGroup {
+	return &ServoGroup{
+		Dev:      dev,
+		minPwm:   minPwm,
+		maxPwm:   maxPwm,
+		minAngle: minAngle,
+		maxAngle: maxAngle,
+	}
+}
+
+// SetMinMaxPwm change pwm and angle limits
+func (s *ServoGroup) SetMinMaxPwm(minAngle, maxAngle physic.Angle, minPwm, maxPwm gpio.Duty) {
+	s.maxPwm = maxPwm
+	s.minPwm = minPwm
+	s.minAngle = minAngle
+	s.maxAngle = maxAngle
+}
+
+// SetAngle set an angle in a given channel of the servo group
+func (s *ServoGroup) SetAngle(channel int, angle physic.Angle) error {
+	value := mapValue(int(angle), int(s.minAngle), int(s.maxAngle), int(s.minPwm), int(s.maxPwm))
+	return s.Dev.SetPwm(channel, 0, gpio.Duty(value))
+}
+
+// GetServo returns a individual Servo to be controlled
+func (s *ServoGroup) GetServo(channel int) *Servo {
+	return &Servo{
+		group:    s,
+		channel:  channel,
+		minAngle: s.minAngle,
+		maxAngle: s.maxAngle,
+	}
+}
+
+// SetMinMaxAngle change angle limits for the servo
+func (s *Servo) SetMinMaxAngle(min, max physic.Angle) {
+	s.minAngle = min
+	s.maxAngle = max
+}
+
+// SetAngle set an angle on the servo
+// will consider the angle limits set
+func (s *Servo) SetAngle(angle physic.Angle) error {
+	if angle < s.minAngle {
+		angle = s.minAngle
+	}
+	if angle > s.maxAngle {
+		angle = s.maxAngle
+	}
+	return s.group.SetAngle(s.channel, angle)
+}
+
+// SetPwm set an pmw value to the servo
+func (s *Servo) SetPwm(pwm gpio.Duty) error {
+	return s.group.SetPwm(s.channel, 0, pwm)
+}
+
+func mapValue(x, inMin, inMax, outMin, outMax int) int {
+	return (x-inMin)*(outMax-outMin)/(inMax-inMin) + outMin
+}