diff --git a/dist/hornverifier b/dist/hornverifier index 1bc25d4c..e6799777 100755 Binary files a/dist/hornverifier and b/dist/hornverifier differ diff --git a/dist/roverd b/dist/roverd index e20d2565..112ca07c 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index 9eab33a6..d148f25f 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/config.go b/pi/roverd/config.go index f9760ea1..ffb8c930 100644 --- a/pi/roverd/config.go +++ b/pi/roverd/config.go @@ -114,18 +114,24 @@ type NightVisionConfig struct { InitialOn bool `yaml:"initialOn" json:"initialOn"` } +type AutoSideBrushConfig struct { + Enabled bool `yaml:"enabled"` + Speed int `yaml:"speed"` +} + type Config struct { - Name string `yaml:"name"` - ServerURL string `yaml:"serverUrl"` - Serial SerialConfig `yaml:"serial"` - BRC BRCConfig `yaml:"brc"` - Battery BatteryConfig `yaml:"battery"` - MaxWheelMMs int `yaml:"maxWheelSpeed"` - Media MediaConfig `yaml:"media"` - CameraServo CameraServoConfig `yaml:"cameraServo"` - Audio AudioConfig `yaml:"audio"` - Horn HornConfig `yaml:"horn"` - NightVision NightVisionConfig `yaml:"nightVision" json:"nightVision"` + Name string `yaml:"name"` + ServerURL string `yaml:"serverUrl"` + Serial SerialConfig `yaml:"serial"` + BRC BRCConfig `yaml:"brc"` + Battery BatteryConfig `yaml:"battery"` + MaxWheelMMs int `yaml:"maxWheelSpeed"` + Media MediaConfig `yaml:"media"` + CameraServo CameraServoConfig `yaml:"cameraServo"` + Audio AudioConfig `yaml:"audio"` + Horn HornConfig `yaml:"horn"` + NightVision NightVisionConfig `yaml:"nightVision" json:"nightVision"` + AutoSideBrush AutoSideBrushConfig `yaml:"autoSideBrush"` } func LoadConfig(path string) (*Config, error) { @@ -187,6 +193,10 @@ func LoadConfig(path string) (*Config, error) { GPIOChip: "gpiochip0", InitialOn: true, }, + AutoSideBrush: AutoSideBrushConfig{ + Enabled: true, + Speed: 20, + }, } if err := yaml.Unmarshal(data, &cfg); err != nil { return nil, err @@ -243,6 +253,7 @@ func LoadConfig(path string) (*Config, error) { } validateAudioConfig(&cfg.Audio) validateHornConfig(&cfg.Horn) + validateAutoSideBrushConfig(&cfg.AutoSideBrush) return &cfg, nil } @@ -350,6 +361,13 @@ func validateNightVisionConfig(cfg *NightVisionConfig) error { return nil } +func validateAutoSideBrushConfig(cfg *AutoSideBrushConfig) { + if cfg.Speed == 0 { + return + } + cfg.Speed = clampInt(cfg.Speed, -127, 127) +} + func derivePublishURL(serverURL, streamName string, port int) (string, error) { if streamName == "" { return "", errors.New("missing stream name for publishUrl") diff --git a/pi/roverd/roverd.sample.yaml b/pi/roverd/roverd.sample.yaml index 5f1f7512..e11a772b 100644 --- a/pi/roverd/roverd.sample.yaml +++ b/pi/roverd/roverd.sample.yaml @@ -58,3 +58,6 @@ nightVision: gpioPin: 22 gpioChip: gpiochip0 initialOn: true +autoSideBrush: + enabled: true + speed: 20 diff --git a/pi/roverd/roverd.yaml b/pi/roverd/roverd.yaml index df8367cf..ebccee49 100644 --- a/pi/roverd/roverd.yaml +++ b/pi/roverd/roverd.yaml @@ -31,3 +31,6 @@ cameraServo: homeAngle: 0 nudgeDegrees: 2 allowRawPulse: false +autoSideBrush: + enabled: true + speed: 20 diff --git a/pi/roverd/wsclient.go b/pi/roverd/wsclient.go index 70b284b0..052c927f 100644 --- a/pi/roverd/wsclient.go +++ b/pi/roverd/wsclient.go @@ -26,6 +26,8 @@ type WSClient struct { recoverMu sync.Mutex recovering bool ttsQueue chan *ttsPayload + lastAux motorPWMPayload + autoSideOn bool connMu sync.Mutex connected bool disconnectT *time.Timer @@ -152,11 +154,17 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error { case msg.DriveDirect != nil: left := clamp(msg.DriveDirect.Left, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs) right := clamp(msg.DriveDirect.Right, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs) - return c.adapter.DriveDirect(left, right) + if err := c.adapter.DriveDirect(left, right); err != nil { + return err + } + c.applyAutoSideBrush(left, right) + return nil case msg.MotorPWM != nil: main := clamp(msg.MotorPWM.Main, -127, 127) side := clamp(msg.MotorPWM.Side, -127, 127) vac := clamp(msg.MotorPWM.Vacuum, 0, 127) + c.lastAux = motorPWMPayload{Main: main, Side: side, Vacuum: vac} + c.autoSideOn = false return c.adapter.MotorPWM(main, side, vac) case msg.SensorStream != nil: if msg.SensorStream.Enable { @@ -214,6 +222,49 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error { } } +func (c *WSClient) applyAutoSideBrush(left, right int) { + if c.cfg == nil || !c.cfg.AutoSideBrush.Enabled { + if c.autoSideOn { + c.autoSideOn = false + if err := c.adapter.MotorPWM(c.lastAux.Main, c.lastAux.Side, c.lastAux.Vacuum); err != nil { + c.log.Printf("auto side brush stop failed: %v", err) + } + } + return + } + + moving := left != 0 || right != 0 + if !moving { + if c.autoSideOn { + c.autoSideOn = false + if err := c.adapter.MotorPWM(c.lastAux.Main, c.lastAux.Side, c.lastAux.Vacuum); err != nil { + c.log.Printf("auto side brush stop failed: %v", err) + } + } + return + } + + if c.lastAux.Side != 0 { + c.autoSideOn = false + return + } + + autoSpeed := clampInt(c.cfg.AutoSideBrush.Speed, -127, 127) + if autoSpeed == 0 { + c.autoSideOn = false + return + } + if c.autoSideOn { + return + } + + if err := c.adapter.MotorPWM(c.lastAux.Main, autoSpeed, c.lastAux.Vacuum); err != nil { + c.log.Printf("auto side brush start failed: %v", err) + return + } + c.autoSideOn = true +} + func (c *WSClient) enqueueTTS(payload *ttsPayload) error { if c.ttsQueue == nil { return fmt.Errorf("tts disabled")