mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 01:21:20 -04:00
96 lines
2.5 KiB
Go
96 lines
2.5 KiB
Go
package main
|
|
|
|
import (
|
|
"flag"
|
|
"log"
|
|
"time"
|
|
|
|
rpio "github.com/stianeikeland/go-rpio/v4"
|
|
)
|
|
|
|
func main() {
|
|
var (
|
|
pinNum = flag.Int("pin", 19, "BCM pin connected to the servo signal line")
|
|
freqHz = flag.Int("freq", 50, "Servo PWM frequency in Hz")
|
|
cycleLen = flag.Int("cycle", 20000, "PWM cycle length (counts per period)")
|
|
minPulse = flag.Int("min", 900, "Minimum pulse width in microseconds")
|
|
maxPulse = flag.Int("max", 2100, "Maximum pulse width in microseconds")
|
|
stepPulse = flag.Int("step", 100, "Pulse width increment in microseconds when sweeping")
|
|
sweeps = flag.Int("sweeps", 2, "How many full min→max→min sweeps to perform")
|
|
pause = flag.Duration("pause", 150*time.Millisecond, "Delay between pulse adjustments")
|
|
holdPulse = flag.Int("hold", 0, "Pulse width to hold before exiting (0 = midpoint of min/max)")
|
|
)
|
|
flag.Parse()
|
|
|
|
if *freqHz <= 0 || *cycleLen <= 0 {
|
|
log.Fatalf("invalid freq (%d) or cycle (%d)", *freqHz, *cycleLen)
|
|
}
|
|
if *minPulse <= 0 || *maxPulse <= 0 || *minPulse >= *maxPulse {
|
|
log.Fatalf("invalid min/max pulses (%d/%d)", *minPulse, *maxPulse)
|
|
}
|
|
if *stepPulse <= 0 {
|
|
log.Fatalf("step must be > 0 (got %d)", *stepPulse)
|
|
}
|
|
if *pause <= 0 {
|
|
log.Fatalf("pause must be > 0 (got %s)", pause)
|
|
}
|
|
if *sweeps < 0 {
|
|
log.Fatalf("sweeps must be >= 0 (got %d)", *sweeps)
|
|
}
|
|
|
|
if err := rpio.Open(); err != nil {
|
|
log.Fatalf("open gpio: %v", err)
|
|
}
|
|
defer rpio.Close()
|
|
|
|
pin := rpio.Pin(*pinNum)
|
|
pin.Mode(rpio.Pwm)
|
|
|
|
targetClock := *freqHz * *cycleLen
|
|
pin.Freq(targetClock)
|
|
log.Printf("Configured PWM pin %d at %d Hz (clock=%d Hz, cycle=%d)", *pinNum, *freqHz, targetClock, *cycleLen)
|
|
|
|
setPulse := func(us int) {
|
|
clamped := clamp(us, *minPulse, *maxPulse)
|
|
pin.DutyCycle(uint32(clamped), uint32(*cycleLen))
|
|
log.Printf("pulse -> %dµs", clamped)
|
|
}
|
|
|
|
mid := (*minPulse + *maxPulse) / 2
|
|
setPulse(mid)
|
|
|
|
runSweep := func() {
|
|
for pulse := *minPulse; pulse <= *maxPulse; pulse += *stepPulse {
|
|
setPulse(pulse)
|
|
time.Sleep(*pause)
|
|
}
|
|
for pulse := *maxPulse - *stepPulse; pulse >= *minPulse; pulse -= *stepPulse {
|
|
setPulse(pulse)
|
|
time.Sleep(*pause)
|
|
}
|
|
}
|
|
|
|
for i := 0; i < *sweeps; i++ {
|
|
log.Printf("Sweep %d/%d", i+1, *sweeps)
|
|
runSweep()
|
|
}
|
|
|
|
finalPulse := *holdPulse
|
|
if finalPulse <= 0 {
|
|
finalPulse = mid
|
|
}
|
|
setPulse(finalPulse)
|
|
log.Printf("Holding at %dµs", clamp(finalPulse, *minPulse, *maxPulse))
|
|
log.Print("Servo verification complete")
|
|
}
|
|
|
|
func clamp(value, min, max int) int {
|
|
if value < min {
|
|
return min
|
|
}
|
|
if value > max {
|
|
return max
|
|
}
|
|
return value
|
|
}
|