Factor out polarity setting for motors

rover/control.go

@@ -45,24 +45,7 @@ func (d *driveMode) Set(value string) error {
 func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
 	log.Printf("control loop started (drive mode: %v)", mode)
 
-	polarity, err := leftMotor.Polarity()
-	if err != nil {
-		return err
-	}
-	if polarity != ev3dev.Inversed {
-		leftMotor.SetPolarity(ev3dev.Inversed)
-	}
-	if err = leftMotor.Err(); err != nil {
-		return err
-	}
-	polarity, err = rightMotor.Polarity()
-	if err != nil {
-		return err
-	}
-	if polarity != ev3dev.Inversed {
-		rightMotor.SetPolarity(ev3dev.Inversed)
-	}
-	if err = rightMotor.Err(); err != nil {
+	if err := setPolarity(ev3dev.Inversed, leftMotor, rightMotor); err != nil {
 		return err
 	}
 
@@ -110,6 +93,22 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 	}
 }
 
+func setPolarity(polarity ev3dev.Polarity, motors ...*ev3dev.TachoMotor) error {
+	for _, motor := range motors {
+		p, err := motor.Polarity()
+		if err != nil {
+			return err
+		}
+		if p != polarity {
+			motor.SetPolarity(polarity)
+		}
+		if err := motor.Err(); err != nil {
+			return err
+		}
+	}
+	return nil
+}
+
 func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
 	if e.IsAxisY() {
 		speed := int(float64(motor.MaxSpeed()) * e.Fraction())