beginnings of the servo

2026-09-16 01:21:20 -04:00 · 2025-11-17 17:02:09 -05:00
parent 3bd723202c
commit 4bd9e25fc4
5 changed files with 131 additions and 0 deletions
@@ -0,0 +1,95 @@
+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
+}