auto sidebrush

pi/roverd/config.go

@@ -114,6 +114,11 @@ 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"`
@@ -126,6 +131,7 @@ type Config struct {
 	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")

pi/roverd/roverd.sample.yaml

@@ -58,3 +58,6 @@ nightVision:
   gpioPin: 22
   gpioChip: gpiochip0
   initialOn: true
+autoSideBrush:
+  enabled: true
+  speed: 20

pi/roverd/roverd.yaml

@@ -31,3 +31,6 @@ cameraServo:
   homeAngle: 0
   nudgeDegrees: 2
   allowRawPulse: false
+autoSideBrush:
+  enabled: true
+  speed: 20

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")