From c87af335dffcd4967eb03ebd6f91dca781ce4627 Mon Sep 17 00:00:00 2001 From: googol Date: Sat, 18 Apr 2020 09:48:26 +0300 Subject: [PATCH] impl servo --- README.md | 26 ++++++++++----- pca9685.go | 35 ++++++++++---------- servo.go | 97 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 133 insertions(+), 25 deletions(-) create mode 100644 servo.go diff --git a/README.md b/README.md index 494d245..12ba2e9 100644 --- a/README.md +++ b/README.md @@ -33,18 +33,28 @@ func main() { log.Fatal(err) } - pwm0 := pca9685.PWMNew(i2c, nil) - err = pwm0.Init() + pca0 := pca9685.PCANew(i2c, nil) + err = pca0.Init() if err != nil { log.Fatal(err) } - // For servo SG90 - pwm0.SetChannel(0, 0, 130) - time.Sleep(1 * time.Second) - pwm0.SetChannel(0, 0, 510) - - pwm0.Reset() + // Sets frequency for channel 0 + pca0.SetChannel(0, 0, 130) + time.Sleep(1 * time.Second) + + // Angle in degrees. Must be in the range `0` to `Range` + // Rotates from 0 to 130 degrees + servo1 := ServoNew(pca0, 0, nil) + for i := 0; i < 130; i++ { + servo1.Angle(i) + time.Sleep(10 * time.Millisecond) + } + + // Fraction as pulse width expressed between 0.0 `MinPulse` and 1.0 `MaxPulse` + servo1.Fraction(0.5) + + pca0.DeInit() } ``` diff --git a/pca9685.go b/pca9685.go index ea41ca0..82c8719 100644 --- a/pca9685.go +++ b/pca9685.go @@ -55,18 +55,20 @@ type PCA9685 struct { // Options for controller type Options struct { - Name string - Frequency float32 + Name string + Frequency float32 + ClockSpeed float32 } -// PWMNew creates a new driver with specified i2c interface -func PWMNew(i2c *i2c.I2C, optn *Options) *PCA9685 { +// PCANew creates a new driver with specified i2c interface +func PCANew(i2c *i2c.I2C, optn *Options) *PCA9685 { adr := i2c.GetAddr() pca := &PCA9685{ Conn: i2c, Optn: &Options{ - Name: "Controller" + fmt.Sprintf("-0x%x", adr), - Frequency: DefaultPWMFrequency, + Name: "Controller" + fmt.Sprintf("-0x%x", adr), + Frequency: DefaultPWMFrequency, + ClockSpeed: ReferenceClockSpeed, }, } if optn != nil { @@ -80,13 +82,10 @@ func (pca *PCA9685) Init() (err error) { if pca.Conn.GetAddr() == 0 { return fmt.Errorf(`device %v is not initiated`, pca.Optn.Name) } - if err := pca.SetFreq(pca.Optn.Frequency); err != nil { - return err - } - return pca.Reset() + return pca.SetFreq(pca.Optn.Frequency) } -// SetFreq sets the PWM frequency in Hz +// SetFreq sets the PWM frequency in Hz for controller func (pca *PCA9685) SetFreq(freq float32) (err error) { prescaleVal := ReferenceClockSpeed/StepCount/freq + 0.5 if prescaleVal < 3.0 { @@ -110,8 +109,13 @@ func (pca *PCA9685) SetFreq(freq float32) (err error) { return pca.Conn.WriteRegU8(Mode1, oldMode|0xA1) // Mode 1, autoincrement on) } -// Reset the chip -func (pca *PCA9685) Reset() (err error) { +// GetFreq returns frequency value +func (pca *PCA9685) GetFreq() float32 { + return pca.Optn.Frequency +} + +// DeInit reset the chip +func (pca *PCA9685) DeInit() (err error) { return pca.Conn.WriteRegU8(Mode1, 0x00) } @@ -129,8 +133,5 @@ func (pca *PCA9685) SetChannel(chn, on, off int) (err error) { buf := []byte{Led0On + byte(4*chn), byte(on) & 0xFF, byte(on >> 8), byte(off) & 0xFF, byte(off >> 8)} _, err = pca.Conn.WriteBytes(buf) - if err != nil { - return err - } - return + return err } diff --git a/servo.go b/servo.go new file mode 100644 index 0000000..e91b0a2 --- /dev/null +++ b/servo.go @@ -0,0 +1,97 @@ +/* +Copyright (c) 2020 +Author: Pavlo Lytvynoff + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +package pca9685 + +import ( + "fmt" +) + +const ( + // The specified pulse width range of a servo has historically been 1000-2000us, + // for a 90 degree range of motion. But nearly all modern servos have a 170-180 + // degree range, and the pulse widths can go well out of the range to achieve this + // extended motion. The default values here of `750` and `2250` typically give + // 135 degrees of motion. You can set `Range` to correspond to the + // actual range of motion you observe with your given `MinPulse` and `MaxPulse` values. + ServoRangeDef int = 135 + ServoMinPulseDef float32 = 750.0 + ServoMaxPulseDef float32 = 2250.0 +) + +// Servo structure +type Servo struct { + PWM *PCA9685 + Channel uint8 + Options *ServOptions +} + +// ServOptions for servo +type ServOptions struct { + Range int // actuation range + MinPulse float32 + MaxPulse float32 +} + +// ServNew creates a new servo driver +func ServoNew(p *PCA9685, chn uint8, o *ServOptions) *Servo { + s := &Servo{ + PWM: p, + Channel: chn, + Options: &ServOptions{ + Range: ServoRangeDef, + MinPulse: ServoMinPulseDef, + MaxPulse: ServoMaxPulseDef, + }, + } + if o != nil { + s.Options = o + } + return s +} + +// Angle in degrees. Must be in the range `0` to `Range`. +func (s *Servo) Angle(a int) (err error) { + if a < 0 || a > s.Options.Range { + return fmt.Errorf("Angle out of range") + } + return s.Fraction(float32(a) / float32(s.Options.Range)) +} + +// Fraction as pulse width expressed between 0.0 `MinPulse` and 1.0 `MaxPulse`. +// For conventional servos, corresponds to the servo position as a fraction +// of the actuation range. +func (s *Servo) Fraction(f float32) (err error) { + if f < 0.0 || f > 1.0 { + return fmt.Errorf("Must be 0.0 to 1.0") + } + + freq := s.PWM.GetFreq() + + minDuty := s.Options.MinPulse * freq / 1000000 * 0xFFFF + maxDuty := s.Options.MaxPulse * freq / 1000000 * 0xFFFF + dutyRange := maxDuty - minDuty + dutyCycle := (int(minDuty+f*dutyRange) + 1) >> 4 + + return s.PWM.SetChannel(int(s.Channel), 0, dutyCycle) +}