legop3/MultiRoombaRover
1
0
Fork 0
mirror of https://github.com/legop3/MultiRoombaRover.git synced 2026-09-15 17:12:59 -04:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

horn pass

Browse Source
This commit is contained in:
legop3
2026-01-31 13:44:35 -05:00
parent 92ab763b27
commit b7d636c70a
26 changed files with 676 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pi/roverd/command.go
+8
View File
@@ -8,6 +8,7 @@ type helloMessage struct {
Media MediaConfig `json:"media"`
CameraServo CameraServoConfig `json:"cameraServo"`
Audio AudioConfig `json:"audio"`
Horn HornConfig `json:"horn"`
NightVision NightVisionConfig `json:"nightVision"`
}
@@ -27,6 +28,7 @@ type inboundMessage struct {
Media *mediaCommand `json:"media,omitempty"`
Servo *servoPayload `json:"servo,omitempty"`
TTS *ttsPayload `json:"tts,omitempty"`
Horn *hornPayload `json:"horn,omitempty"`
NightVision *nightVisionPayload `json:"nightVision,omitempty"`
Song *songPayload `json:"song,omitempty"`
}
@@ -64,6 +66,12 @@ type ttsPayload struct {
Speak bool `json:"speak,omitempty"`
}
type hornPayload struct {
Action string `json:"action"`
Waveform string `json:"waveform,omitempty"`
Freqs []float64 `json:"freqs,omitempty"`
}
type nightVisionPayload struct {
Action string `json:"action"`
}
pi/roverd/config.go
+31
View File
@@ -65,6 +65,14 @@ type AudioConfig struct {
DefaultPitch int `yaml:"defaultPitch" json:"defaultPitch,omitempty"`
}
type HornConfig struct {
Enabled bool `yaml:"enabled" json:"enabled"`
Volume float64 `yaml:"volume" json:"-"`
SampleRate int `yaml:"sampleRate" json:"-"`
Channels int `yaml:"channels" json:"-"`
Device string `yaml:"device" json:"-"`
}
type MediaConfig struct {
PublishURL string `yaml:"publishUrl" json:"publishUrl,omitempty"`
AudioPublishURL string `yaml:"audioPublishUrl" json:"audioPublishUrl,omitempty"`
@@ -113,6 +121,7 @@ type Config struct {
Media MediaConfig `yaml:"media"`
CameraServo CameraServoConfig `yaml:"cameraServo"`
Audio AudioConfig `yaml:"audio"`
Horn HornConfig `yaml:"horn"`
NightVision NightVisionConfig `yaml:"nightVision" json:"nightVision"`
}
@@ -160,6 +169,12 @@ func LoadConfig(path string) (*Config, error) {
DefaultVoice: "rms",
DefaultPitch: 50,
},
Horn: HornConfig{
Enabled: false,
Volume: 0.25,
SampleRate: 48000,
Channels: 1,
},
NightVision: NightVisionConfig{
Enabled: true,
GPIOPin: 22,
@@ -221,6 +236,7 @@ func LoadConfig(path string) (*Config, error) {
return nil, fmt.Errorf("nightVision: %w", err)
}
validateAudioConfig(&cfg.Audio)
validateHornConfig(&cfg.Horn)
return &cfg, nil
}
@@ -291,6 +307,21 @@ func validateAudioConfig(cfg *AudioConfig) {
}
}
func validateHornConfig(cfg *HornConfig) {
if cfg.Volume <= 0 {
cfg.Volume = 0.25
}
if cfg.Volume > 1 {
cfg.Volume = 1
}
if cfg.SampleRate <= 0 {
cfg.SampleRate = 48000
}
if cfg.Channels <= 0 {
cfg.Channels = 1
}
}
func validateNightVisionConfig(cfg *NightVisionConfig) error {
if !cfg.Enabled {
return nil
pi/roverd/horn.go
+232
View File
@@ -0,0 +1,232 @@
package roverd
import (
"bufio"
"encoding/binary"
"fmt"
"log"
"math"
"os/exec"
"strings"
"sync"
"time"
)
const (
hornAttack = 20 * time.Millisecond
hornRelease = 60 * time.Millisecond
)
type HornSynth struct {
cfg HornConfig
log *log.Logger
mu sync.Mutex
stop chan struct{}
done chan struct{}
active bool
}
func NewHornSynth(cfg HornConfig, logger *log.Logger) *HornSynth {
return &HornSynth{
cfg: cfg,
log: logger,
}
}
func (h *HornSynth) HandlePayload(payload *hornPayload) error {
if payload == nil {
return fmt.Errorf("horn payload required")
}
action := strings.ToLower(strings.TrimSpace(payload.Action))
switch action {
case "start", "on", "honk":
waveform := strings.ToLower(strings.TrimSpace(payload.Waveform))
if waveform != "sine" && waveform != "saw" {
waveform = "saw"
}
freqs := sanitizeHornFreqs(payload.Freqs)
if len(freqs) == 0 {
h.Stop()
return nil
}
return h.Start(waveform, freqs)
case "stop", "off":
h.Stop()
return nil
default:
return fmt.Errorf("unsupported horn action: %s", payload.Action)
}
}
func (h *HornSynth) Start(waveform string, freqs []float64) error {
h.mu.Lock()
h.stopLocked()
stop := make(chan struct{})
done := make(chan struct{})
h.stop = stop
h.done = done
h.active = true
h.mu.Unlock()
go h.run(waveform, freqs, stop, done)
return nil
}
func (h *HornSynth) Stop() {
h.mu.Lock()
h.stopLocked()
h.mu.Unlock()
}
func (h *HornSynth) stopLocked() {
if !h.active {
return
}
if h.stop != nil {
close(h.stop)
}
if h.done != nil {
<-h.done
}
h.stop = nil
h.done = nil
h.active = false
}
func (h *HornSynth) run(waveform string, freqs []float64, stop <-chan struct{}, done chan<- struct{}) {
defer close(done)
rate := h.cfg.SampleRate
if rate <= 0 {
rate = 48000
}
channels := h.cfg.Channels
if channels <= 0 {
channels = 1
}
volume := h.cfg.Volume
if volume <= 0 {
volume = 0.25
}
if volume > 1 {
volume = 1
}
args := []string{"-q", "-f", "S16_LE", "-c", fmt.Sprintf("%d", channels), "-r", fmt.Sprintf("%d", rate), "-t", "raw"}
if h.cfg.Device != "" {
args = append(args, "-D", h.cfg.Device)
}
cmd := exec.Command("aplay", args...)
stdin, err := cmd.StdinPipe()
if err != nil {
h.log.Printf("horn: aplay stdin failed: %v", err)
return
}
if err := cmd.Start(); err != nil {
h.log.Printf("horn: aplay start failed: %v", err)
_ = stdin.Close()
return
}
writer := bufio.NewWriterSize(stdin, 32*1024)
if err := h.synthLoop(writer, waveform, freqs, rate, channels, volume, stop); err != nil {
h.log.Printf("horn: synth failed: %v", err)
}
_ = writer.Flush()
_ = stdin.Close()
if err := cmd.Wait(); err != nil {
h.log.Printf("horn: aplay exit: %v", err)
}
}
func (h *HornSynth) synthLoop(writer *bufio.Writer, waveform string, freqs []float64, rate, channels int, volume float64, stop <-chan struct{}) error {
phase := make([]float64, len(freqs))
increment := make([]float64, len(freqs))
for i, f := range freqs {
increment[i] = 2 * math.Pi * f / float64(rate)
}
attackFrames := int(float64(rate) * hornAttack.Seconds())
releaseFrames := int(float64(rate) * hornRelease.Seconds())
framesPerChunk := 512
buf := make([]byte, framesPerChunk*channels*2)
scale := volume / float64(len(freqs))
stopRequested := false
releaseStart := -1
sampleIndex := 0
for {
if !stopRequested {
select {
case <-stop:
stopRequested = true
releaseStart = sampleIndex
default:
}
}
for i := 0; i < framesPerChunk; i++ {
env := 1.0
if attackFrames > 0 && sampleIndex < attackFrames {
env = float64(sampleIndex) / float64(attackFrames)
} else if stopRequested && releaseFrames > 0 {
relIndex := sampleIndex - releaseStart
if relIndex >= releaseFrames {
return nil
}
env = float64(releaseFrames-relIndex) / float64(releaseFrames)
} else if stopRequested {
return nil
}
sample := 0.0
for j := range freqs {
switch waveform {
case "sine":
sample += math.Sin(phase[j])
default:
sample += sawFromPhase(phase[j])
}
phase[j] += increment[j]
if phase[j] > 2*math.Pi {
phase[j] -= 2 * math.Pi
}
}
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); err != nil {
return err
}
}
}
func sanitizeHornFreqs(freqs []float64) []float64 {
if len(freqs) == 0 {
return nil
}
out := make([]float64, 0, 4)
for _, f := range freqs {
if len(out) >= 4 {
break
}
if f <= 0 {
continue
}
out = append(out, f)
}
return out
}
func sawFromPhase(phase float64) float64 {
return 2.0*(phase/(2*math.Pi)) - 1.0
}
pi/roverd/roverd.sample.yaml
+5
View File
@@ -45,6 +45,11 @@ audio:
defaultEngine: flite
defaultVoice: rms
defaultPitch: 50
horn:
enabled: false
volume: 0.25
sampleRate: 48000
channels: 1
nightVision:
enabled: true
gpioPin: 22
pi/roverd/wsclient.go
+12
View File
@@ -20,6 +20,7 @@ type WSClient struct {
events chan RoverEvent
media *MediaSupervisor
servo *CameraServo
horn *HornSynth
nightVision *NightVisionLight
log *log.Logger
recoverMu sync.Mutex
@@ -38,6 +39,10 @@ func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, even
if cfg.Audio.TTSEnabled {
ttsQueue = make(chan *ttsPayload, 2)
}
var horn *HornSynth
if cfg.Horn.Enabled {
horn = NewHornSynth(cfg.Horn, logger)
}
return &WSClient{
cfg: cfg,
adapter: adapter,
@@ -45,6 +50,7 @@ func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, even
events: events,
media: media,
servo: servo,
horn: horn,
nightVision: nightVision,
log: logger,
ttsQueue: ttsQueue,
@@ -101,6 +107,7 @@ func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
Media: c.cfg.Media,
CameraServo: c.cfg.CameraServo,
Audio: c.cfg.Audio,
Horn: c.cfg.Horn,
NightVision: c.cfg.NightVision,
}
c.log.Printf("sending hello (camera servo enabled=%v pin=%d)", msg.CameraServo.Enabled, msg.CameraServo.Pin)
@@ -180,6 +187,11 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
return c.handleServoCommand(msg.Servo)
case msg.TTS != nil:
return c.enqueueTTS(msg.TTS)
case msg.Horn != nil:
if c.horn == nil {
return fmt.Errorf("horn disabled")
}
return c.horn.HandlePayload(msg.Horn)
case msg.NightVision != nil:
if c.nightVision == nil {
return fmt.Errorf("night vision disabled")