Merge pull request #14 from robototes/master

Tolerate missing control panel & climb (#62)
robototes · Feb 23, 2020 · 1834282 · 1834282
2 parents 6f8d0a3 + 4387d51
commit 1834282
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 21 deletions.
diff --git a/src/main/java/frc/team2412/robot/OI.java b/src/main/java/frc/team2412/robot/OI.java
@@ -117,17 +117,21 @@ public OI(RobotContainer robotContainer) {
 		intakeBackOffButton.whenPressed(new IntakeBackBothOffCommandGroup(robotContainer.m_intakeUpDownSubsystem,
 				robotContainer.m_intakeMotorOnOffSubsystem));
 
-		// CONTROL PANEL
-		controlPanelSpinThreeTimesButton
-				.whenPressed(new RotateControlPanelCommand(robotContainer.m_controlPanelColorSubsystem));
-		controlPanelSetToTargetButton
-				.whenPressed(new SetToTargetColorCommand(robotContainer.m_controlPanelColorSubsystem));
-
-		climbDeployRailsButton.whenActive(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem));
-		climbExtendArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem));
-		climbRetractArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem));
-		climbRetractRailsButton.whenActive(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem));
-		climbStopArmButton.whenActive(new ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem));
+		if (RobotMap.CONTROL_PANEL_CONNECTED) {
+			// CONTROL PANEL
+			controlPanelSpinThreeTimesButton
+					.whenPressed(new RotateControlPanelCommand(robotContainer.m_controlPanelColorSubsystem));
+			controlPanelSetToTargetButton
+					.whenPressed(new SetToTargetColorCommand(robotContainer.m_controlPanelColorSubsystem));
+		}
+
+		if (RobotMap.CLIMB_CONNECTED) {
+			climbDeployRailsButton.whenActive(new ClimbDeployRailsCommand(robotContainer.m_climbLiftSubsystem));
+			climbExtendArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem));
+			climbRetractArmButton.whenActive(new ClimbExtendArmCommand(robotContainer.m_climbMotorSubsystem));
+			climbRetractRailsButton.whenActive(new ClimbRetractRailsCommand(robotContainer.m_climbLiftSubsystem));
+			climbStopArmButton.whenActive(new ClimbStopArmCommand(robotContainer.m_climbMotorSubsystem));
+		}
 
 		indexerStopButton.whenPressed(new IndexSpitCommand(robotContainer.m_indexerSensorSubsystem,
 				robotContainer.m_indexerMotorSubsystem, robotContainer.m_intakeMotorOnOffSubsystem));

diff --git a/src/main/java/frc/team2412/robot/RobotContainer.java b/src/main/java/frc/team2412/robot/RobotContainer.java
@@ -39,8 +39,10 @@ public class RobotContainer implements Loggable {
 	SendableChooser<Command> m_chooser = new SendableChooser<>();
 
 	public RobotContainer() {
-		m_climbLiftSubsystem = new ClimbLiftSubsystem(RobotMap.climbLeftPneumatic, RobotMap.climbRightPneumatic);
-		m_climbMotorSubsystem = new ClimbMotorSubsystem(RobotMap.leftClimbMotor, RobotMap.rightClimbMotor);
+		if (RobotMap.CLIMB_CONNECTED) {
+			m_climbLiftSubsystem = new ClimbLiftSubsystem(RobotMap.climbLeftPneumatic, RobotMap.climbRightPneumatic);
+			m_climbMotorSubsystem = new ClimbMotorSubsystem(RobotMap.leftClimbMotor, RobotMap.rightClimbMotor);
+		}
 
 		m_indexerSensorSubsystem = new IndexerSensorSubsystem(RobotMap.intakeFront, RobotMap.front, RobotMap.frontMid,
 				RobotMap.frontInner, RobotMap.backInner, RobotMap.backMid, RobotMap.back, RobotMap.intakeBack);
@@ -58,7 +60,9 @@ public RobotContainer() {
 		m_intakeUpDownSubsystem = new IntakeUpDownSubsystem(RobotMap.frontIntakeliftSolenoid,
 				RobotMap.backIntakeLiftSolenoid);
 
-		m_controlPanelColorSubsystem = new ControlPanelColorSubsystem(RobotMap.colorSensor, RobotMap.colorSensorMotor);
+		if (RobotMap.CONTROL_PANEL_CONNECTED) {
+			m_controlPanelColorSubsystem = new ControlPanelColorSubsystem(RobotMap.colorSensor, RobotMap.colorSensorMotor);
+		}
 
 		m_LimelightSubsystem = new LimelightSubsystem(RobotMap.limelight);
 		m_turretSubsystem = new TurretSubsystem(RobotMap.turretMotor, m_LimelightSubsystem);

diff --git a/src/main/java/frc/team2412/robot/RobotMap.java b/src/main/java/frc/team2412/robot/RobotMap.java
@@ -29,6 +29,9 @@
 //remember to declare robot container at the bottom of this class
 public class RobotMap {
 
+    public static boolean CLIMB_CONNECTED = false;
+    public static boolean CONTROL_PANEL_CONNECTED = false;
+
     public static enum CANBus {
         INTAKE1(11),
         INDEX1(12),
@@ -144,12 +147,12 @@ public int getIndexCANID(){
 
 
     // climb Pneumatics
-    public static Solenoid climbLeftPneumatic = new Solenoid(PneumaticPort.CLIMB_LEFT.id);
-    public static Solenoid climbRightPneumatic = new Solenoid(PneumaticPort.CLIMB_RIGHT.id);
+    public static Solenoid climbLeftPneumatic = CLIMB_CONNECTED ? new Solenoid(PneumaticPort.CLIMB_LEFT.id) : null;
+    public static Solenoid climbRightPneumatic = CLIMB_CONNECTED ? new Solenoid(PneumaticPort.CLIMB_RIGHT.id) : null;
 
     // Motors
-    public static CANSparkMax leftClimbMotor = new CANSparkMax(CANBus.CLIMB1.id, MotorType.kBrushless);
-    public static CANSparkMax rightClimbMotor = new CANSparkMax(CANBus.CLIMB2.id, MotorType.kBrushless);
+    public static CANSparkMax leftClimbMotor = CLIMB_CONNECTED ? new CANSparkMax(CANBus.CLIMB1.id, MotorType.kBrushless) : null;
+    public static CANSparkMax rightClimbMotor = CLIMB_CONNECTED  ? new CANSparkMax(CANBus.CLIMB2.id, MotorType.kBrushless) : null;
 
     // INDEX SUBSYSTEM
     // ---------------------------------------------------------------------------
@@ -224,12 +227,12 @@ private CANSparkMax tryGetMotor(int motorId) {
     // CONTROL PANEL SUBSYSTEM
     // ----------------------------------------------------------------------
     // Control Panel I2C
-    public static I2C.Port COLOR_SESNOR_PORT = I2C.Port.kOnboard;
+    public static I2C.Port COLOR_SENSOR_PORT = I2C.Port.kOnboard;
 
-    public static ColorSensorV3 colorSensor = new ColorSensorV3(COLOR_SESNOR_PORT);
+    public static ColorSensorV3 colorSensor = CONTROL_PANEL_CONNECTED ? new ColorSensorV3(COLOR_SENSOR_PORT) : null;
     public static ColorMatch colorMatcher = new ColorMatch();
 
-    public static WPI_TalonFX colorSensorMotor = new WPI_TalonFX(CANBus.CONTROL_PANEL.id);
+    public static WPI_TalonFX colorSensorMotor = CONTROL_PANEL_CONNECTED ? new WPI_TalonFX(CANBus.CONTROL_PANEL.id) : null;
 
     // Limelight subsystem
     // ----------------------------------------------------------------------------------------------