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  @SuppressWarnings("this-escape")
025  public MotorControllerGroup(
026      MotorController motorController, MotorController... motorControllers) {
027    m_motorControllers = new MotorController[motorControllers.length + 1];
028    m_motorControllers[0] = motorController;
029    System.arraycopy(motorControllers, 0, m_motorControllers, 1, motorControllers.length);
030    init();
031  }
032
033  @SuppressWarnings("this-escape")
034  public MotorControllerGroup(MotorController[] motorControllers) {
035    m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
036    init();
037  }
038
039  private void init() {
040    for (MotorController controller : m_motorControllers) {
041      SendableRegistry.addChild(this, controller);
042    }
043    instances++;
044    SendableRegistry.addLW(this, "MotorControllerGroup", instances);
045  }
046
047  @Override
048  public void close() {
049    SendableRegistry.remove(this);
050  }
051
052  @Override
053  public void set(double speed) {
054    for (MotorController motorController : m_motorControllers) {
055      motorController.set(m_isInverted ? -speed : speed);
056    }
057  }
058
059  @Override
060  public void setVoltage(double outputVolts) {
061    for (MotorController motorController : m_motorControllers) {
062      motorController.setVoltage(m_isInverted ? -outputVolts : outputVolts);
063    }
064  }
065
066  @Override
067  public double get() {
068    if (m_motorControllers.length > 0) {
069      return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
070    }
071    return 0.0;
072  }
073
074  @Override
075  public void setInverted(boolean isInverted) {
076    m_isInverted = isInverted;
077  }
078
079  @Override
080  public boolean getInverted() {
081    return m_isInverted;
082  }
083
084  @Override
085  public void disable() {
086    for (MotorController motorController : m_motorControllers) {
087      motorController.disable();
088    }
089  }
090
091  @Override
092  public void stopMotor() {
093    for (MotorController motorController : m_motorControllers) {
094      motorController.stopMotor();
095    }
096  }
097
098  @Override
099  public void initSendable(SendableBuilder builder) {
100    builder.setSmartDashboardType("Motor Controller");
101    builder.setActuator(true);
102    builder.setSafeState(this::stopMotor);
103    builder.addDoubleProperty("Value", this::get, this::set);
104  }
105}