First implementation

vendor/periph.io/x/periph/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

vendor/periph.io/x/periph/experimental/devices/pca9685/pca9685.go

@@ -0,0 +1,138 @@
+// 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 registers.
+const (
+	mode1    byte = 0x00
+	mode2    byte = 0x01
+	prescale byte = 0xFE
+	// Each channel has two 12-bit registers (on & off time).
+	led0OnL   byte = 0x06 // Start address for setting channel 0.
+	allLedOnL byte = 0xFA // Start address for setting all channels.
+)
+
+// Mode register 1, mode1.
+const (
+	restart byte = 0x80
+	ai      byte = 0x20 // Auto-increment register after each read and write.
+	sleep   byte = 0x10
+	allCall byte = 0x01
+)
+
+// Mode register 2, mode2.
+const (
+	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) | ai
+	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, (oldmode & ^restart) | sleep}); err != nil {
+		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, oldmode | restart})
+	return err
+}
+
+// setPWM writes a PWM value in a specific register.
+func (d *Dev) setPWM(register uint8, on, off gpio.Duty) error {
+	// Chained writes are possible due to auto-increment.
+	_, err := d.dev.Write([]byte{
+		register,
+		byte(on),
+		byte(on >> 8),
+		byte(off),
+		byte(off >> 8),
+	})
+	return err
+}
+
+// SetAllPwm set a PWM value for all outputs.
+func (d *Dev) SetAllPwm(on, off gpio.Duty) error {
+	return d.setPWM(allLedOnL, on, off)
+}
+
+// SetPwm set a PWM value for a given PCA9685 channel.
+func (d *Dev) SetPwm(channel int, on, off gpio.Duty) error {
+	return d.setPWM(led0OnL+byte(4*channel), on, off)
+}

vendor/periph.io/x/periph/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 through 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
+}