001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.motorcontrol;
006
007import edu.wpi.first.util.sendable.Sendable;
008import edu.wpi.first.util.sendable.SendableBuilder;
009import edu.wpi.first.util.sendable.SendableRegistry;
010import java.util.Arrays;
011
012/** Allows multiple {@link MotorController} objects to be linked together. */
013public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
014  private boolean m_isInverted;
015  private final MotorController[] m_motorControllers;
016  private static int instances;
017
018  /**
019   * Create a new MotorControllerGroup with the provided MotorControllers.
020   *
021   * @param motorController The first MotorController to add
022   * @param motorControllers The MotorControllers to add
023   */
024  public MotorControllerGroup(
025      MotorController motorController, MotorController... motorControllers) {
026    m_motorControllers = new MotorController[motorControllers.length + 1];
027    m_motorControllers[0] = motorController;
028    System.arraycopy(motorControllers, 0, m_motorControllers, 1, motorControllers.length);
029    init();
030  }
031
032  public MotorControllerGroup(MotorController[] motorControllers) {
033    m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
034    init();
035  }
036
037  private void init() {
038    for (MotorController controller : m_motorControllers) {
039      SendableRegistry.addChild(this, controller);
040    }
041    instances++;
042    SendableRegistry.addLW(this, "MotorControllerGroup", instances);
043  }
044
045  @Override
046  public void close() {
047    SendableRegistry.remove(this);
048  }
049
050  @Override
051  public void set(double speed) {
052    for (MotorController motorController : m_motorControllers) {
053      motorController.set(m_isInverted ? -speed : speed);
054    }
055  }
056
057  @Override
058  public double get() {
059    if (m_motorControllers.length > 0) {
060      return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
061    }
062    return 0.0;
063  }
064
065  @Override
066  public void setInverted(boolean isInverted) {
067    m_isInverted = isInverted;
068  }
069
070  @Override
071  public boolean getInverted() {
072    return m_isInverted;
073  }
074
075  @Override
076  public void disable() {
077    for (MotorController motorController : m_motorControllers) {
078      motorController.disable();
079    }
080  }
081
082  @Override
083  public void stopMotor() {
084    for (MotorController motorController : m_motorControllers) {
085      motorController.stopMotor();
086    }
087  }
088
089  @Override
090  public void initSendable(SendableBuilder builder) {
091    builder.setSmartDashboardType("Motor Controller");
092    builder.setActuator(true);
093    builder.setSafeState(this::stopMotor);
094    builder.addDoubleProperty("Value", this::get, this::set);
095  }
096}