From 4bd9e25fc4de0ecffa9135ffd115a50d14fc10ec Mon Sep 17 00:00:00 2001 From: legop3 Date: Mon, 17 Nov 2025 17:02:09 -0500 Subject: [PATCH] beginnings of the servo --- dist/roverd | Bin 6815928 -> 6815928 bytes pi/install_roverd.sh | 33 ++++++++++ pi/roverd/cmd/servoverifier/main.go | 95 ++++++++++++++++++++++++++++ pi/roverd/go.mod | 1 + pi/roverd/go.sum | 2 + 5 files changed, 131 insertions(+) create mode 100644 pi/roverd/cmd/servoverifier/main.go diff --git a/dist/roverd b/dist/roverd index 7d008d09d495490b24ef6e23c751ba6f1a89d78d..be92bc49dae53748f87e81bf01a8fde03a8ac51c 100755 GIT binary patch delta 589 zcmbWxyHXPY7>421MbRkcLy?1L5agi9W;dJN1OXGWMofqcCUWo)lie)NsEi~wc3|;E zY6>VV87pms!r?|N9mX4=NaKwEHCQ~|TfF-lU(|i^?Q?jz^rp1t)ZEwUvSF`glg`2i z_uW|WZGAq7uhq4jky^5%>6($uZha4T#tZdn*R3S;nVSF6ORX-C=HHi;7DZ7ygeo%m zqO!QrPS&sHYANc0PePMjR(pqJ=m?oFrO_Q^aW^M4Tbc5^cmeB1}l% zPMjw$5FNxtqLb(%x``g*5^H* zcZo4VBO-)O7(|pXi5OuKHW4Qh#5i$}m>?#JDdIknBvQmQk^TYa@XwG@@h#oZHQkFw zji4HshFS3u9y?^;)y7TqIfD6#G+Ud%i@`ME?$V2q9k7JY0dJl(0}i4%kD^HU9AXD_@XL; Ky>)fP{`(JS&A@#C delta 589 zcmbWxIa3n>0EXen;*lukQxV1c7EolfyV)ank_`~iAc4f&z~(?_GJ`TWgQ?Ks!^X;j z*22z48=?0XXlv{77gFeq-*2#Zy0>^YUcNInP*v5(kK^b!Y%gTx`?FmZ%9N*p7O6DNq1#3|x5afUcc^b!5U0CA2uPYeVqYhCSJZ^D_etW4U;*r`>sos&@+lX1z*b(xSGa#IR2 zDYs-wiZU%TQj)UVmOFA+?#X?5AP?n{RAhGDY|nj9{P*sfb_Z5hj7X|dle#q4SB!c8 F??1CFzPSJZ diff --git a/pi/install_roverd.sh b/pi/install_roverd.sh index f380b9c8..c83caec9 100755 --- a/pi/install_roverd.sh +++ b/pi/install_roverd.sh @@ -94,6 +94,38 @@ install_video_deps() { apt-get install -y --no-install-recommends libcamera-apps ffmpeg } +find_boot_config() { + if [[ -f /boot/firmware/config.txt ]]; then + printf "/boot/firmware/config.txt" + return 0 + fi + if [[ -f /boot/config.txt ]]; then + printf "/boot/config.txt" + return 0 + fi + return 1 +} + +ensure_pwm_overlay() { + local boot_config + if ! boot_config="$(find_boot_config)"; then + log "WARNING: unable to locate /boot config.txt; please ensure dtoverlay=pwm-2chan is added manually for servo support" + return + fi + if grep -Eq '^\s*dtoverlay=pwm(-2chan)?\b' "$boot_config"; then + log "PWM overlay already present in $boot_config" + return + fi + local backup="${boot_config}.roverd.$(date +%Y%m%d%H%M%S).bak" + cp "$boot_config" "$backup" + { + echo "" + echo "# Added by roverd installer to expose PWM hardware for camera servo control" + echo "dtoverlay=pwm-2chan" + } >> "$boot_config" + log "Enabled dtoverlay=pwm-2chan in $boot_config (backup at $backup). Reboot required for changes to apply." +} + ensure_user roverd "dialout,gpio,video,render" install -o roverd -g roverd -m 0755 "$BINARY_SRC" /usr/local/bin/roverd log "Installed roverd binary" @@ -112,6 +144,7 @@ install -m 0644 pi/systemd/roverd.service /etc/systemd/system/roverd.service log "Installed roverd systemd unit" install_video_deps +ensure_pwm_overlay # Install video publisher assets install -D -o root -g root -m 0755 pi/bin/video-publisher.sh /usr/local/bin/video-publisher diff --git a/pi/roverd/cmd/servoverifier/main.go b/pi/roverd/cmd/servoverifier/main.go new file mode 100644 index 00000000..ded66c25 --- /dev/null +++ b/pi/roverd/cmd/servoverifier/main.go @@ -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 +} diff --git a/pi/roverd/go.mod b/pi/roverd/go.mod index a69b6c2a..d3611c75 100644 --- a/pi/roverd/go.mod +++ b/pi/roverd/go.mod @@ -3,6 +3,7 @@ module multiroombarover/pi/roverd go 1.25.4 require ( + github.com/stianeikeland/go-rpio/v4 v4.6.0 github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07 github.com/warthog618/go-gpiocdev v0.9.1 gopkg.in/yaml.v3 v3.0.1 diff --git a/pi/roverd/go.sum b/pi/roverd/go.sum index 819bfeeb..e8c7baeb 100644 --- a/pi/roverd/go.sum +++ b/pi/roverd/go.sum @@ -4,6 +4,8 @@ github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4= github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= +github.com/stianeikeland/go-rpio/v4 v4.6.0 h1:eAJgtw3jTtvn/CqwbC82ntcS+dtzUTgo5qlZKe677EY= +github.com/stianeikeland/go-rpio/v4 v4.6.0/go.mod h1:A3GvHxC1Om5zaId+HqB3HKqx4K/AqeckxB7qRjxMK7o= github.com/stretchr/testify v1.9.0 h1:HtqpIVDClZ4nwg75+f6Lvsy/wHu+3BoSGCbBAcpTsTg= github.com/stretchr/testify v1.9.0/go.mod h1:r2ic/lqez/lEtzL7wO/rwa5dbSLXVDPFyf8C91i36aY= github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07 h1:UyzmZLoiDWMRywV4DUYb9Fbt8uiOSooupjTq10vpvnU=