89 lines
2.2 KiB
Go
89 lines
2.2 KiB
Go
package main
|
|
|
|
import (
|
|
"gobot.io/x/gobot/drivers/i2c"
|
|
"log"
|
|
"math"
|
|
)
|
|
|
|
type Servo int
|
|
|
|
const (
|
|
ServoHeadHorizontal Servo = iota
|
|
ServoHeadVertical
|
|
ServoCount
|
|
)
|
|
|
|
type ServoChannel struct {
|
|
controller *i2c.PCA9685Driver
|
|
channel int
|
|
}
|
|
|
|
var servoController1 *i2c.PCA9685Driver
|
|
var servoController2 *i2c.PCA9685Driver
|
|
|
|
// var servos = new([ServoCount]*gpio.ServoDriver)
|
|
var servos = new([ServoCount]*ServoChannel)
|
|
|
|
func InitServo() {
|
|
adaptor := GetAdaptor()
|
|
servoController1 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x40))
|
|
servoController2 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x41))
|
|
|
|
createServo(ServoHeadHorizontal, servoController2, 1)
|
|
createServo(ServoHeadVertical, servoController2, 0)
|
|
|
|
err := servoController1.Start()
|
|
if err != nil {
|
|
log.Print("Could not start servo controller 1: ", err)
|
|
}
|
|
err = servoController2.Start()
|
|
if err != nil {
|
|
log.Print("Could not start servo controller 1: ", err)
|
|
}
|
|
|
|
err = servoController1.SetPWMFreq(50)
|
|
if err != nil {
|
|
log.Print("Could not set servo controller 1 frequency: ", err)
|
|
}
|
|
err = servoController2.SetPWMFreq(50)
|
|
if err != nil {
|
|
log.Print("Could not set servo controller 2 frequency: ", err)
|
|
}
|
|
// Halt the controllers to stop any current movement
|
|
ServosOff()
|
|
log.Print("Started servos")
|
|
}
|
|
|
|
func ServosOff() {
|
|
err := servoController1.Halt()
|
|
if err != nil {
|
|
log.Print("Could not stop servo controller 1: ", err)
|
|
}
|
|
err = servoController2.Halt()
|
|
if err != nil {
|
|
log.Print("Could not stop servo controller 2: ", err)
|
|
}
|
|
}
|
|
|
|
// SetServoAngle Sets the angle to a value of 0 - 1
|
|
func SetServoAngle(servo Servo, angle float64) {
|
|
pulseDuration := angle + 1
|
|
pulseDurationRatio := pulseDuration / 20
|
|
pulseDurationTicks := pulseDurationRatio * 4095
|
|
servoDriver := servos[servo]
|
|
err := servoDriver.controller.SetPWM(servoDriver.channel, 0, uint16(math.Floor(pulseDurationTicks)))
|
|
if err != nil {
|
|
log.Printf("Could not set servo %d to angle %0.1f: %v\n", servo, angle, err)
|
|
}
|
|
}
|
|
|
|
func createServo(servo Servo, pwmDriver *i2c.PCA9685Driver, channel int) {
|
|
//servoDriver := gpio.NewServoDriver(pwmDriver, channel)
|
|
//servos[servo] = servoDriver
|
|
servos[servo] = &ServoChannel{
|
|
controller: pwmDriver,
|
|
channel: channel,
|
|
}
|
|
}
|