89 lines
2.0 KiB
Go
89 lines
2.0 KiB
Go
|
package main
|
||
|
|
|
||
|
|
import (
|
||
|
|
"log"
|
||
|
|
"time"
|
||
|
|
|
||
|
|
"github.com/hybridgroup/gobot"
|
||
|
|
"github.com/hybridgroup/gobot/platforms/i2c"
|
||
|
|
"github.com/hybridgroup/gobot/platforms/raspi"
|
||
|
|
)
|
||
|
|
|
||
|
|
var (
|
||
|
|
// Min pulse length out of 4096
|
||
|
|
servoMin = 150
|
||
|
|
// Max pulse length out of 4096
|
||
|
|
servoMax = 700
|
||
|
|
// Limiting the max this servo can rotate (in deg)
|
||
|
|
maxDegree = 180
|
||
|
|
// Number of degrees to increase per call
|
||
|
|
degIncrease = 10
|
||
|
|
yawDeg = 90
|
||
|
|
)
|
||
|
|
|
||
|
|
func degree2pulse(deg int) int32 {
|
||
|
|
pulse := servoMin
|
||
|
|
pulse += ((servoMax - servoMin) / maxDegree) * deg
|
||
|
|
return int32(pulse)
|
||
|
|
}
|
||
|
|
func adafruitServoMotorRunner(a *i2c.AdafruitMotorHatDriver) (err error) {
|
||
|
|
log.Printf("Servo Motor Run Loop...\n")
|
||
|
|
// Changing from the default 0x40 address because this configuration involves
|
||
|
|
// a Servo HAT stacked on top of a DC/Stepper Motor HAT on top of the Pi.
|
||
|
|
stackedHatAddr := 0x41
|
||
|
|
var channel byte = 1
|
||
|
|
deg := 90
|
||
|
|
|
||
|
|
// update the I2C address state
|
||
|
|
a.SetServoHatAddress(stackedHatAddr)
|
||
|
|
|
||
|
|
// Do not need to set this every run loop
|
||
|
|
freq := 60.0
|
||
|
|
if err = a.SetServoMotorFreq(freq); err != nil {
|
||
|
|
log.Printf("%s", err.Error())
|
||
|
|
return
|
||
|
|
}
|
||
|
|
// start in the middle of the 180-deg range
|
||
|
|
pulse := degree2pulse(deg)
|
||
|
|
if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
|
||
|
|
log.Printf(err.Error())
|
||
|
|
return
|
||
|
|
}
|
||
|
|
// INCR
|
||
|
|
pulse = degree2pulse(deg + degIncrease)
|
||
|
|
if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
|
||
|
|
log.Printf(err.Error())
|
||
|
|
return
|
||
|
|
}
|
||
|
|
<-time.After(2000 * time.Millisecond)
|
||
|
|
// DECR
|
||
|
|
pulse = degree2pulse(deg - degIncrease)
|
||
|
|
if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
|
||
|
|
log.Printf(err.Error())
|
||
|
|
return
|
||
|
|
}
|
||
|
|
return
|
||
|
|
}
|
||
|
|
func main() {
|
||
|
|
gbot := gobot.NewGobot()
|
||
|
|
r := raspi.NewRaspiAdaptor("raspi")
|
||
|
|
adaFruit := i2c.NewAdafruitMotorHatDriver(r, "adafruit")
|
||
|
|
|
||
|
|
work := func() {
|
||
|
|
gobot.Every(5*time.Second, func() {
|
||
|
|
|
||
|
|
adafruitServoMotorRunner(adaFruit)
|
||
|
|
})
|
||
|
|
}
|
||
|
|
|
||
|
|
robot := gobot.NewRobot("adaFruitBot",
|
||
|
|
[]gobot.Connection{r},
|
||
|
|
[]gobot.Device{adaFruit},
|
||
|
|
work,
|
||
|
|
)
|
||
|
|
|
||
|
|
gbot.AddRobot(robot)
|
||
|
|
|
||
|
|
gbot.Start()
|
||
|
|
}
|