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 void setVoltage(double outputVolts) {
059    for (MotorController motorController : m_motorControllers) {
060      motorController.setVoltage(m_isInverted ? -outputVolts : outputVolts);
061    }
062  }
063
064  @Override
065  public double get() {
066    if (m_motorControllers.length > 0) {
067      return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
068    }
069    return 0.0;
070  }
071
072  @Override
073  public void setInverted(boolean isInverted) {
074    m_isInverted = isInverted;
075  }
076
077  @Override
078  public boolean getInverted() {
079    return m_isInverted;
080  }
081
082  @Override
083  public void disable() {
084    for (MotorController motorController : m_motorControllers) {
085      motorController.disable();
086    }
087  }
088
089  @Override
090  public void stopMotor() {
091    for (MotorController motorController : m_motorControllers) {
092      motorController.stopMotor();
093    }
094  }
095
096  @Override
097  public void initSendable(SendableBuilder builder) {
098    builder.setSmartDashboardType("Motor Controller");
099    builder.setActuator(true);
100    builder.setSafeState(this::stopMotor);
101    builder.addDoubleProperty("Value", this::get, this::set);
102  }
103}