mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-11-16 19:52:17 -05:00
move from /pi to /rover because it isnt just pi anymore.
new json config stuff and ui
This commit is contained in:
@@ -0,0 +1,187 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/binary"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"os/exec"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
)
|
||||
|
||||
const twoPi = 2 * math.Pi
|
||||
|
||||
func main() {
|
||||
var (
|
||||
device = flag.String("device", "", "ALSA device (empty = default)")
|
||||
rate = flag.Int("rate", 48000, "Sample rate in Hz")
|
||||
channels = flag.Int("channels", 1, "Number of audio channels")
|
||||
duration = flag.Duration("duration", 2*time.Second, "Total horn duration")
|
||||
freqsRaw = flag.String("freqs", "440,550,660", "Comma-separated frequencies in Hz")
|
||||
volume = flag.Float64("volume", 0.25, "Output volume 0.0-1.0")
|
||||
attack = flag.Duration("attack", 20*time.Millisecond, "Attack time")
|
||||
release = flag.Duration("release", 60*time.Millisecond, "Release time")
|
||||
)
|
||||
flag.Parse()
|
||||
|
||||
if *rate <= 0 {
|
||||
log.Fatalf("rate must be > 0 (got %d)", *rate)
|
||||
}
|
||||
if *channels <= 0 {
|
||||
log.Fatalf("channels must be > 0 (got %d)", *channels)
|
||||
}
|
||||
if *duration <= 0 {
|
||||
log.Fatalf("duration must be > 0 (got %s)", *duration)
|
||||
}
|
||||
if *volume <= 0 || *volume > 1.0 {
|
||||
log.Fatalf("volume must be within (0,1] (got %.3f)", *volume)
|
||||
}
|
||||
if *attack < 0 || *release < 0 {
|
||||
log.Fatalf("attack/release must be >= 0")
|
||||
}
|
||||
|
||||
freqs, err := parseFreqs(*freqsRaw)
|
||||
if err != nil {
|
||||
log.Fatalf("parse freqs: %v", err)
|
||||
}
|
||||
if len(freqs) == 0 {
|
||||
log.Fatal("no frequencies provided")
|
||||
}
|
||||
|
||||
if *attack+*release > *duration {
|
||||
log.Fatalf("attack+release must be <= duration (%s + %s > %s)", *attack, *release, *duration)
|
||||
}
|
||||
|
||||
args := []string{"-q", "-f", "S16_LE", "-c", fmt.Sprintf("%d", *channels), "-r", fmt.Sprintf("%d", *rate), "-t", "raw"}
|
||||
if *device != "" {
|
||||
args = append(args, "-D", *device)
|
||||
}
|
||||
cmd := exec.Command("aplay", args...)
|
||||
stdin, err := cmd.StdinPipe()
|
||||
if err != nil {
|
||||
log.Fatalf("aplay stdin: %v", err)
|
||||
}
|
||||
if err := cmd.Start(); err != nil {
|
||||
log.Fatalf("start aplay: %v", err)
|
||||
}
|
||||
|
||||
writer := bufio.NewWriterSize(stdin, 32*1024)
|
||||
if err := synthChord(writer, freqs, *rate, *channels, *duration, *volume, *attack, *release); err != nil {
|
||||
_ = stdin.Close()
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("synth: %v", err)
|
||||
}
|
||||
if err := writer.Flush(); err != nil {
|
||||
_ = stdin.Close()
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("flush: %v", err)
|
||||
}
|
||||
if err := stdin.Close(); err != nil {
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("close stdin: %v", err)
|
||||
}
|
||||
if err := cmd.Wait(); err != nil {
|
||||
log.Fatalf("aplay failed: %v", err)
|
||||
}
|
||||
log.Print("Horn verification complete")
|
||||
}
|
||||
|
||||
func parseFreqs(raw string) ([]float64, error) {
|
||||
trimmed := strings.TrimSpace(raw)
|
||||
if trimmed == "" {
|
||||
return nil, nil
|
||||
}
|
||||
parts := strings.Split(trimmed, ",")
|
||||
freqs := make([]float64, 0, len(parts))
|
||||
for _, part := range parts {
|
||||
part = strings.TrimSpace(part)
|
||||
if part == "" {
|
||||
continue
|
||||
}
|
||||
value, err := strconv.ParseFloat(part, 64)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("invalid freq %q", part)
|
||||
}
|
||||
if value <= 0 {
|
||||
return nil, fmt.Errorf("freq must be > 0 (got %.3f)", value)
|
||||
}
|
||||
freqs = append(freqs, value)
|
||||
}
|
||||
return freqs, nil
|
||||
}
|
||||
|
||||
func synthChord(writer *bufio.Writer, freqs []float64, rate, channels int, duration time.Duration, volume float64, attack, release time.Duration) error {
|
||||
totalFrames := int(float64(rate) * duration.Seconds())
|
||||
if totalFrames <= 0 {
|
||||
return fmt.Errorf("duration too short")
|
||||
}
|
||||
|
||||
phase := make([]float64, len(freqs))
|
||||
increment := make([]float64, len(freqs))
|
||||
for i, f := range freqs {
|
||||
increment[i] = twoPi * f / float64(rate)
|
||||
}
|
||||
|
||||
attackFrames := int(float64(rate) * attack.Seconds())
|
||||
releaseFrames := int(float64(rate) * release.Seconds())
|
||||
steadyFrames := totalFrames - attackFrames - releaseFrames
|
||||
|
||||
framesPerChunk := 512
|
||||
buf := make([]byte, framesPerChunk*channels*2)
|
||||
sampleIndex := 0
|
||||
scale := volume / float64(len(freqs))
|
||||
|
||||
for framesLeft := totalFrames; framesLeft > 0; {
|
||||
framesNow := framesPerChunk
|
||||
if framesLeft < framesNow {
|
||||
framesNow = framesLeft
|
||||
}
|
||||
for i := 0; i < framesNow; i++ {
|
||||
env := envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames)
|
||||
sample := 0.0
|
||||
for j := range freqs {
|
||||
sample += sawFromPhase(phase[j])
|
||||
phase[j] += increment[j]
|
||||
if phase[j] > twoPi {
|
||||
phase[j] -= twoPi
|
||||
}
|
||||
}
|
||||
sample *= scale * env
|
||||
if sample > 1.0 {
|
||||
sample = 1.0
|
||||
} else if sample < -1.0 {
|
||||
sample = -1.0
|
||||
}
|
||||
intSample := int16(sample * math.MaxInt16)
|
||||
offset := i * channels * 2
|
||||
for ch := 0; ch < channels; ch++ {
|
||||
binary.LittleEndian.PutUint16(buf[offset+ch*2:], uint16(intSample))
|
||||
}
|
||||
sampleIndex++
|
||||
}
|
||||
if _, err := writer.Write(buf[:framesNow*channels*2]); err != nil {
|
||||
return err
|
||||
}
|
||||
framesLeft -= framesNow
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames int) float64 {
|
||||
if attackFrames > 0 && sampleIndex < attackFrames {
|
||||
return float64(sampleIndex) / float64(attackFrames)
|
||||
}
|
||||
if releaseFrames > 0 && sampleIndex >= attackFrames+steadyFrames {
|
||||
relIndex := sampleIndex - (attackFrames + steadyFrames)
|
||||
return float64(releaseFrames-relIndex) / float64(releaseFrames)
|
||||
}
|
||||
return 1.0
|
||||
}
|
||||
|
||||
func sawFromPhase(phase float64) float64 {
|
||||
return 2.0*(phase/twoPi) - 1.0
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/rover/roverd"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var portName string
|
||||
var baud int
|
||||
var timeout time.Duration
|
||||
var startupWait time.Duration
|
||||
var controlID string
|
||||
var rawValue string
|
||||
|
||||
flag.StringVar(&portName, "port", "", "serial device, for example /dev/ttyUSB0 or /dev/ttyACM0")
|
||||
flag.IntVar(&baud, "baud", 115200, "Firmata serial baud rate")
|
||||
flag.DurationVar(&timeout, "timeout", 5*time.Second, "timeout for each Firmata response")
|
||||
flag.DurationVar(&startupWait, "startup-wait", 2*time.Second, "time allowed for boards that reset when the port opens")
|
||||
flag.StringVar(&controlID, "control", "", "optional declared control ID to exercise")
|
||||
flag.StringVar(&rawValue, "value", "", "JSON value for -control, such as 90, true, or \"hello\"")
|
||||
flag.Parse()
|
||||
|
||||
if portName == "" {
|
||||
log.Fatal("-port is required")
|
||||
}
|
||||
if (controlID == "") != (rawValue == "") {
|
||||
log.Fatal("-control and -value must be provided together")
|
||||
}
|
||||
|
||||
port, err := serial.OpenPort(&serial.Config{
|
||||
Name: portName,
|
||||
Baud: baud,
|
||||
ReadTimeout: 100 * time.Millisecond,
|
||||
})
|
||||
if err != nil {
|
||||
log.Fatalf("open %s: %v", portName, err)
|
||||
}
|
||||
defer port.Close()
|
||||
|
||||
// CH340 and native-USB development boards may reset when the host opens the
|
||||
// port. Waiting here makes the same probe work with both connection styles
|
||||
// without baking that diagnostic delay into the production Firmata client.
|
||||
time.Sleep(startupWait)
|
||||
|
||||
rootContext, cancelRoot := context.WithCancel(context.Background())
|
||||
defer cancelRoot()
|
||||
client := roverd.NewFirmataClient(port)
|
||||
client.Start(rootContext)
|
||||
|
||||
firmware, err := withTimeout(timeout, client.QueryFirmware)
|
||||
if err != nil {
|
||||
log.Fatalf("query firmware: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata firmware: %s %d.%d\n", firmware.Name, firmware.Major, firmware.Minor)
|
||||
|
||||
capabilities, err := withTimeout(timeout, client.QueryCapabilities)
|
||||
if err != nil {
|
||||
log.Fatalf("query capabilities: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata pins described: %d\n", len(capabilities))
|
||||
|
||||
description, err := withTimeout(timeout, client.Describe)
|
||||
if err != nil {
|
||||
log.Fatalf("describe rover peripheral: %v", err)
|
||||
}
|
||||
formatted, err := json.MarshalIndent(description, "", " ")
|
||||
if err != nil {
|
||||
log.Fatalf("format description: %v", err)
|
||||
}
|
||||
fmt.Printf("Peripheral description:\n%s\n", formatted)
|
||||
|
||||
if controlID != "" {
|
||||
if err := exerciseControl(client, description, controlID, json.RawMessage(rawValue)); err != nil {
|
||||
log.Fatalf("exercise control %q: %v", controlID, err)
|
||||
}
|
||||
fmt.Fprintf(os.Stdout, "Control %q accepted.\n", controlID)
|
||||
}
|
||||
}
|
||||
|
||||
// withTimeout gives every boot-time exchange its own deadline. A missing board
|
||||
// therefore reports the exact handshake stage that failed instead of consuming
|
||||
// one shared timeout and obscuring which response was absent.
|
||||
func withTimeout[T any](timeout time.Duration, operation func(context.Context) (T, error)) (T, error) {
|
||||
ctx, cancel := context.WithTimeout(context.Background(), timeout)
|
||||
defer cancel()
|
||||
return operation(ctx)
|
||||
}
|
||||
|
||||
func exerciseControl(client *roverd.FirmataClient, description roverd.PeripheralDescription, controlID string, rawValue json.RawMessage) error {
|
||||
var selected *roverd.PeripheralControl
|
||||
for index := range description.Controls {
|
||||
if description.Controls[index].ID == controlID {
|
||||
selected = &description.Controls[index]
|
||||
break
|
||||
}
|
||||
}
|
||||
if selected == nil {
|
||||
return errors.New("control is not present in the device description")
|
||||
}
|
||||
|
||||
var value any
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return fmt.Errorf("parse -value as JSON: %w", err)
|
||||
}
|
||||
|
||||
// Standard outputs deliberately use standard Firmata commands. Only custom
|
||||
// callbacks use the rover-peripheral CONTROL operation, which is the central
|
||||
// distinction the probe is intended to validate on real hardware.
|
||||
switch selected.Output.Type {
|
||||
case "custom":
|
||||
return client.SendPeripheralControl(selected.ID, value)
|
||||
case "digital":
|
||||
enabled, ok := value.(bool)
|
||||
if !ok {
|
||||
return errors.New("digital control value must be true or false")
|
||||
}
|
||||
if selected.Output.ActiveLow {
|
||||
enabled = !enabled
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), roverd.FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.SetDigitalPin(byte(*selected.Output.Pin), enabled)
|
||||
case "pwm", "servo":
|
||||
number, ok := value.(float64)
|
||||
if !ok || number != float64(int(number)) {
|
||||
return errors.New("PWM and servo control values must be whole numbers")
|
||||
}
|
||||
mode := roverd.FirmataPinModePWM
|
||||
if selected.Output.Type == "servo" {
|
||||
mode = roverd.FirmataPinModeServo
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), mode); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.ExtendedAnalog(byte(*selected.Output.Pin), int(number))
|
||||
default:
|
||||
return fmt.Errorf("unsupported output %q", selected.Output.Type)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,177 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"sync/atomic"
|
||||
"syscall"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/rover/roverd"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var cfgPath string
|
||||
flag.StringVar(&cfgPath, "config", "/etc/roverd.json", "path to roverd JSON configuration")
|
||||
printDefaults := flag.Bool("print-default-config", false, "print a complete initial JSON configuration and exit")
|
||||
printSchema := flag.Bool("print-config-schema", false, "print the configuration JSON Schema and exit")
|
||||
checkConfig := flag.Bool("check-config", false, "validate configuration without opening hardware or connecting")
|
||||
flag.Parse()
|
||||
if *printDefaults {
|
||||
data, _ := json.MarshalIndent(roverd.DefaultConfig(), "", " ")
|
||||
fmt.Println(string(data))
|
||||
return
|
||||
}
|
||||
if *printSchema {
|
||||
fmt.Println(string(roverd.ConfigurationSchema()))
|
||||
return
|
||||
}
|
||||
cfg, err := roverd.LoadConfig(cfgPath)
|
||||
if err != nil {
|
||||
log.Fatalf("load config: %v", err)
|
||||
}
|
||||
if *checkConfig {
|
||||
fmt.Println("Configuration valid")
|
||||
return
|
||||
}
|
||||
if run(cfg, cfgPath) {
|
||||
executable, err := os.Executable()
|
||||
if err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
if err := syscall.Exec(executable, os.Args, os.Environ()); err != nil {
|
||||
log.Fatalf("restart roverd: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func run(cfg *roverd.Config, cfgPath string) bool {
|
||||
ctx, cancel := signal.NotifyContext(context.Background(), syscall.SIGINT, syscall.SIGTERM)
|
||||
defer cancel()
|
||||
logger := log.New(os.Stdout, "roverd: ", log.LstdFlags|log.Lmicroseconds|log.LUTC)
|
||||
events := make(chan roverd.RoverEvent, 128)
|
||||
console := roverd.NewConsoleNotifier(logger)
|
||||
console.SetEvents(events)
|
||||
var restarting atomic.Bool
|
||||
management := roverd.NewManagementClient(cfg, cfgPath, events, logger, func() { restarting.Store(true); cancel() })
|
||||
managementDone := make(chan struct{})
|
||||
go func() { defer close(managementDone); management.Run(ctx) }()
|
||||
// Hardware failures leave management connected until a corrected config
|
||||
// requests restart. runHardware unwinds its own resources on failure.
|
||||
media := roverd.NewMediaSupervisor(cfg.Media, logger)
|
||||
management.SetMedia(media)
|
||||
if media != nil {
|
||||
media.Start(ctx)
|
||||
}
|
||||
if err := runHardware(ctx, cfg, events, management, media, logger, console); err != nil {
|
||||
console.Notify(fmt.Sprintf("Hardware unavailable: %v. Management remains connected.", err))
|
||||
}
|
||||
<-ctx.Done()
|
||||
<-managementDone
|
||||
return restarting.Load()
|
||||
}
|
||||
|
||||
func runHardware(ctx context.Context, cfg *roverd.Config, eventStream chan roverd.RoverEvent, management *roverd.ManagementClient, mediaSupervisor *roverd.MediaSupervisor, logger *log.Logger, console *roverd.ConsoleNotifier) error {
|
||||
ctx, cancel := context.WithCancel(ctx)
|
||||
defer cancel()
|
||||
serialPort, err := roverd.OpenSerial(cfg.Serial)
|
||||
if err != nil {
|
||||
return fmt.Errorf("open serial: %w", err)
|
||||
}
|
||||
defer serialPort.Close()
|
||||
|
||||
// Peripheral discovery is intentionally a boot-time operation. The manager
|
||||
// keeps successful USB ports open across server WebSocket reconnects and is
|
||||
// rebuilt only when the roverd process itself restarts.
|
||||
peripherals, err := roverd.DiscoverPeripheralManager(ctx, cfg.Serial.Device, logger)
|
||||
if err != nil {
|
||||
console.Notify(fmt.Sprintf("Rover peripheral startup failed: %v", err))
|
||||
return fmt.Errorf("discover rover peripherals: %w", err)
|
||||
}
|
||||
defer peripherals.Close()
|
||||
|
||||
sensorFrames := make(chan []byte, 8)
|
||||
sensorSamples := make(chan roverd.SensorSample, 8)
|
||||
|
||||
streamer := roverd.NewSensorStreamer(serialPort, sensorFrames, sensorSamples, logger)
|
||||
go streamer.Run(ctx)
|
||||
|
||||
adapter := roverd.NewSerialAdapter(serialPort, logger)
|
||||
|
||||
// Resolve roles across external and internal peripherals without transport priority.
|
||||
hardwareControllers, err := roverd.ResolveRoverHardwareControllers(cfg, peripherals, logger)
|
||||
if err != nil {
|
||||
console.Notify(fmt.Sprintf("Rover peripheral startup failed while selecting hardware: %v", err))
|
||||
return fmt.Errorf("resolve rover hardware controllers: %w", err)
|
||||
}
|
||||
defer hardwareControllers.Close()
|
||||
if err := peripherals.StartInternal(ctx); err != nil {
|
||||
logger.Printf("internal peripheral: %v", err)
|
||||
console.Notify(fmt.Sprintf("Pi GPIO background function unavailable: %v", err))
|
||||
}
|
||||
for _, message := range peripherals.StartupBroadcasts() {
|
||||
console.Notify(message)
|
||||
}
|
||||
|
||||
for _, message := range hardwareControllers.StartupBroadcasts() {
|
||||
console.Notify(message)
|
||||
}
|
||||
|
||||
// A peripheral is never hot-reconnected. Report the first terminal serial
|
||||
// failure for each discovered board and tell the local operator exactly what
|
||||
// recovery action the fixed boot-time lifecycle requires.
|
||||
go func() {
|
||||
for {
|
||||
select {
|
||||
case failure := <-peripherals.Failures():
|
||||
console.Notify(fmt.Sprintf(
|
||||
"Rover peripheral %q (%s) disconnected: %v. Reconnect it and restart roverd.",
|
||||
failure.Name,
|
||||
failure.ID,
|
||||
failure.Err,
|
||||
))
|
||||
case <-ctx.Done():
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
|
||||
autoCharge := roverd.NewAutoChargeController(adapter, eventStream, logger)
|
||||
|
||||
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, hardwareControllers.CameraServo, hardwareControllers.Headlight, hardwareControllers.Laser, peripherals, logger, console)
|
||||
client.SetAutoCharge(autoCharge)
|
||||
go autoCharge.Run(ctx, sensorSamples)
|
||||
if !management.SetRuntime(client, mediaSupervisor) {
|
||||
return nil
|
||||
}
|
||||
defer client.PrepareShutdown()
|
||||
|
||||
// The control loop is ready after startup resolution. Individual unavailable
|
||||
// peripheral roles have already been reported and do not prevent connection.
|
||||
console.Notify("roverd started and hardware initialization completed.")
|
||||
defer console.Notify("roverd stopped.")
|
||||
|
||||
retryDelay := time.Second
|
||||
for ctx.Err() == nil {
|
||||
if err := client.Run(ctx); err != nil {
|
||||
logger.Printf("websocket loop ended: %v", err)
|
||||
}
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return nil
|
||||
case <-time.After(retryDelay):
|
||||
}
|
||||
|
||||
if retryDelay < 30*time.Second {
|
||||
retryDelay *= 2
|
||||
}
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
Reference in New Issue
Block a user