001/*----------------------------------------------------------------------------*/
002/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
003/* Open Source Software - may be modified and shared by FRC teams. The code   */
004/* must be accompanied by the FIRST BSD license file in the root directory of */
005/* the project.                                                               */
006/*----------------------------------------------------------------------------*/
007
008package edu.wpi.first.wpilibj;
009
010import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
011
012/**
013 * Allows multiple {@link SpeedController} objects to be linked together.
014 */
015public class SpeedControllerGroup extends SendableBase implements SpeedController {
016  private boolean m_isInverted = false;
017  private final SpeedController[] m_speedControllers;
018  private static int instances = 0;
019
020  /**
021   * Create a new SpeedControllerGroup with the provided SpeedControllers.
022   *
023   * @param speedControllers The SpeedControllers to add
024   */
025  public SpeedControllerGroup(SpeedController speedController,
026                              SpeedController... speedControllers) {
027    m_speedControllers = new SpeedController[speedControllers.length + 1];
028    m_speedControllers[0] = speedController;
029    addChild(speedController);
030    for (int i = 0; i < speedControllers.length; i++) {
031      m_speedControllers[i + 1] = speedControllers[i];
032      addChild(speedControllers[i]);
033    }
034    instances++;
035    setName("SpeedControllerGroup", instances);
036  }
037
038  @Override
039  public void set(double speed) {
040    for (SpeedController speedController : m_speedControllers) {
041      speedController.set(m_isInverted ? -speed : speed);
042    }
043  }
044
045  @Override
046  public double get() {
047    if (m_speedControllers.length > 0) {
048      return m_speedControllers[0].get() * (m_isInverted ? -1 : 1);
049    }
050    return 0.0;
051  }
052
053  @Override
054  public void setInverted(boolean isInverted) {
055    m_isInverted = isInverted;
056  }
057
058  @Override
059  public boolean getInverted() {
060    return m_isInverted;
061  }
062
063  @Override
064  public void disable() {
065    for (SpeedController speedController : m_speedControllers) {
066      speedController.disable();
067    }
068  }
069
070  @Override
071  public void stopMotor() {
072    for (SpeedController speedController : m_speedControllers) {
073      speedController.stopMotor();
074    }
075  }
076
077  @Override
078  public void pidWrite(double output) {
079    set(output);
080  }
081
082  @Override
083  public void initSendable(SendableBuilder builder) {
084    builder.setSmartDashboardType("Speed Controller");
085    builder.setSafeState(this::stopMotor);
086    builder.addDoubleProperty("Value", this::get, this::set);
087  }
088}