manage storage + alliance (needs testing)

src/main/java/frc4388/robot/Robot.java

@@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -49,6 +50,8 @@ public class Robot extends TimedRobot {
 
   private static DesmosServer desmosServer = new DesmosServer(8000);
 
+  public static Alliance alliance;
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -231,6 +234,8 @@ public class Robot extends TimedRobot {
   public void autonomousInit() {
     LOGGER.fine("autonomousInit()");
 
+    Robot.alliance = DriverStation.getAlliance();
+
     selectedOdo = odoChooser.getSelected();
     if (selectedOdo == null) {
       selectedOdo = m_robotContainer.getOdometry();
@@ -256,6 +261,9 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     LOGGER.fine("teleopInit()");
+
+    Robot.alliance = DriverStation.getAlliance();
+
     m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
 
     selectedOdo = odoChooser.getSelected();

src/main/java/frc4388/robot/RobotContainer.java

@@ -67,6 +67,7 @@ import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
 import frc4388.robot.commands.ShooterCommands.AimToCenter;
 import frc4388.robot.commands.ShooterCommands.Shoot;
 import frc4388.robot.commands.ShooterCommands.TrackTarget;
+import frc4388.robot.commands.StorageCommands.ManageStorage;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Extender;
@@ -164,6 +165,12 @@ public class RobotContainer {
             getOperatorController().getLeftTriggerAxis(), 
             getOperatorController().getRightTriggerAxis()),
             m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
+    
+    m_robotStorage.setDefaultCommand(
+        new ManageStorage(m_robotStorage, 
+                          m_robotBoomBoom, 
+                          m_robotTurret).withName("Storage ManageStorage defaultCommand"));
+    
     // m_robotClimber.setDefaultCommand(
     //   new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber)
     // );

src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java

@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.StorageCommands;
+
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
+import frc4388.robot.Robot;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Turret;
+
+public class ManageStorage extends CommandBase {
+
+  // subsystems
+  private Storage storage;
+  private BoomBoom drum;
+  private Turret turret;
+
+  private Alliance alliance;
+  private boolean rightColor;
+
+  /** Creates a new ManageStorage. */
+  public ManageStorage(Storage storage, BoomBoom drum, Turret turret) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.storage = storage;
+    this.drum = drum;
+    this.turret = turret;
+
+    rightColor = true;
+
+    addRequirements(this.storage, this.drum, this.turret);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+  
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    checkColor();
+    
+    if (rightColor) {
+      this.storage.manageStorage();
+    } else {
+
+      // * CommandScheduler.getInstance().schedule(new ExampleCommand());
+      // * new ExampleCommand().schedule();
+      // * new ExampleCommand().execute();
+      
+      new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command
+    }
+  }
+
+  private void checkColor() {
+    this.alliance = this.storage.getColor();
+    rightColor = this.alliance.equals(Robot.alliance);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java

@@ -2,21 +2,20 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc4388.robot.commands.ShooterCommands;
+package frc4388.robot.commands.StorageCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Storage;
 import frc4388.robot.subsystems.Turret;
-import frc4388.utility.RobotTime;
 
 public class SpitOutWrongColor extends CommandBase {
 
   // subsystems
+  private Storage storage;
   private BoomBoom drum;
   private Turret turret;
-  private Storage storage;
 
   // time (in milliseconds)
   private long initialTime;
@@ -27,27 +26,27 @@ public class SpitOutWrongColor extends CommandBase {
   private int spitVelocity;
 
   /** Creates a new SpitOutWrongColor. */
-  public SpitOutWrongColor(BoomBoom drum, Turret turret, Storage storage) {
+  public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) {
     // Use addRequirements() here to declare subsystem dependencies.
 
+    this.storage = storage;
     this.drum = drum;
     this.turret = turret;
-    this.storage = storage;
 
+    addRequirements(this.storage, this.drum, this.turret);
+  }
+  
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
     initialTime = System.currentTimeMillis();
     elapsedTime = 0;
     threshold = 2000;
 
     initialTurret = this.turret.getEncoderPosition();
     spitVelocity = 2000;
-
-    addRequirements(this.drum, this.turret, this.storage);
   }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
+  
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -6,7 +6,12 @@ package frc4388.robot.subsystems;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
 import com.revrobotics.ColorSensorV3;
 import edu.wpi.first.wpilibj.util.Color;
 
@@ -58,7 +63,10 @@ public class Storage extends SubsystemBase {
     // return (m_colorSensor.getBlue() >= 200  && m_colorSensor.getRed() < 100  && m_colorSensor.getGreen() < 100);
     return (m_colorSensor.getColor() == Color.kBlue);
   }
-  
+
+  public Alliance getColor() {
+    return (getRed() ? Alliance.Red : Alliance.Blue);
+  }  
 
   @Override
   /**

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -31,10 +31,7 @@ public class Turret extends SubsystemBase {
                                            // MotorType.kBrushless);
   public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS;
 
-  SparkMaxPIDController m_boomBoomRotatePIDController;
-  public RelativeEncoder m_boomBoomRotateEncoder;
-
-  SparkMaxPIDController m_boomBoomRotatePIDController;
+  public SparkMaxPIDController m_boomBoomRotatePIDController;
   public RelativeEncoder m_boomBoomRotateEncoder;
 
   SparkMaxLimitSwitch m_boomBoomLeftLimit;
@@ -141,9 +138,8 @@ public class Turret extends SubsystemBase {
   }
 
   public double getBoomBoomAngleDegrees() {
-    return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
-        / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
-  } // TODO: does this method work?
+    return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
+  }
 
   public double getCurrent(){
     return m_boomBoomRotateMotor.getOutputCurrent();