From 7ae7591951d16ec46e903c92b4da3eb6501330cb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 13:59:10 -0600 Subject: [PATCH] manage storage + alliance (needs testing) --- src/main/java/frc4388/robot/Robot.java | 8 ++ .../java/frc4388/robot/RobotContainer.java | 7 ++ .../robot/commands/StorageCommands/Drum.java | 0 .../StorageCommands/ManageStorage.java | 76 +++++++++++++++++++ .../SpitOutWrongColor.java | 23 +++--- .../frc4388/robot/subsystems/Storage.java | 10 ++- .../java/frc4388/robot/subsystems/Turret.java | 10 +-- 7 files changed, 114 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/Drum.java create mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java rename src/main/java/frc4388/robot/commands/{ShooterCommands => StorageCommands}/SpitOutWrongColor.java (88%) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 30e77dd..cb70709 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1e6cd5b..ebd9715 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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) // ); diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java b/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java new file mode 100644 index 0000000..0639307 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java similarity index 88% rename from src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java rename to src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java index 7be2329..5c79247 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java +++ b/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() { diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index adcda74..ee1311e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/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 /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index cb07d7f..c0bf99b 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/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();