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}