From b2ab8ea899efe8e64fd11b8d1765ee3f13fd5465 Mon Sep 17 00:00:00 2001 From: Alvaro Viebrantz Date: Tue, 30 Oct 2018 08:22:30 -0400 Subject: [PATCH] pca9685: Add support for pwm module (#296) --- experimental/devices/pca9685/doc.go | 16 ++ experimental/devices/pca9685/example_test.go | 63 ++++++++ experimental/devices/pca9685/pca9685.go | 150 +++++++++++++++++++ experimental/devices/pca9685/servo.go | 90 +++++++++++ 4 files changed, 319 insertions(+) create mode 100644 experimental/devices/pca9685/doc.go create mode 100644 experimental/devices/pca9685/example_test.go create mode 100644 experimental/devices/pca9685/pca9685.go create mode 100644 experimental/devices/pca9685/servo.go diff --git a/experimental/devices/pca9685/doc.go b/experimental/devices/pca9685/doc.go new file mode 100644 index 0000000..5962a4b --- /dev/null +++ b/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 diff --git a/experimental/devices/pca9685/example_test.go b/experimental/devices/pca9685/example_test.go new file mode 100644 index 0000000..6f81f2d --- /dev/null +++ b/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) + } +} diff --git a/experimental/devices/pca9685/pca9685.go b/experimental/devices/pca9685/pca9685.go new file mode 100644 index 0000000..a1eb4ff --- /dev/null +++ b/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 +} diff --git a/experimental/devices/pca9685/servo.go b/experimental/devices/pca9685/servo.go new file mode 100644 index 0000000..5322a91 --- /dev/null +++ b/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 +}