From 1d9a29ed1a492cfd88e712039abb9a92893b35d1 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Sat, 6 Nov 2021 15:06:38 -0700 Subject: [PATCH 1/7] Beach Blitz #1 --- src/main/java/frc/robot/Constants.java | 3 +- .../java/frc/robot/commands/DriveTest.java | 31 ------------------- .../commands/ExitStartingConfiguration.java | 12 ++++--- src/main/java/frc/robot/commands/Outtake.java | 2 +- .../robot/commands/autos/ScorePreloads.java | 10 +++--- .../frc/robot/subsystems/ArmSubsystem.java | 23 ++++++++++++-- .../frc/robot/subsystems/DriveSubsystem.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 11 ++++--- .../java/friarLib2/commands/TimedCommand.java | 6 ++++ 9 files changed, 51 insertions(+), 49 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveTest.java create mode 100644 src/main/java/friarLib2/commands/TimedCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8cf11e8..ac17561 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,6 +60,7 @@ public final static class Arm { public static final double START_CONFIG_ANGLE = 0; // Angle that arm is in when robot is turned on public static final double STOWED_ANGLE = 0; // Angle where robot fits under the Wheel of Fortune (WOF) public static final double INTAKE_ANGLE = 0; // Angle required for intaking power cells + public static final double OUTTAKE_ANGLE = 15; public static final double VISION_SEEK_ANGLE = 100; // Angle for finding targets /******** Arm Motion Constraints ********/ @@ -114,7 +115,7 @@ public final static class Serializer { public static final int ACCLERATOR_MOTOR_ID = 15; /******** Tuning Constants ********/ - public static final double BRUSH_POWER = .75; + public static final double BRUSH_POWER = .6; public static final double ACCLERATOR_POWER = 1.0; public static final double ROLLER_POWER = 1.0; public static final double BRUSH_REVERSE_POWER = .75; diff --git a/src/main/java/frc/robot/commands/DriveTest.java b/src/main/java/frc/robot/commands/DriveTest.java deleted file mode 100644 index ed25c49..0000000 --- a/src/main/java/frc/robot/commands/DriveTest.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DriveSubsystem; - -public class DriveTest extends CommandBase { - DriveSubsystem drive; - - public DriveTest (DriveSubsystem drive) { - this.drive = drive; - - addRequirements(drive); - } - - @Override - public void execute() { - drive.setDriveSpeeds(1.5, -1.5); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - drive.stop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ExitStartingConfiguration.java b/src/main/java/frc/robot/commands/ExitStartingConfiguration.java index 02f2cc4..07bc70d 100644 --- a/src/main/java/frc/robot/commands/ExitStartingConfiguration.java +++ b/src/main/java/frc/robot/commands/ExitStartingConfiguration.java @@ -1,9 +1,11 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; +/** + * Unclip the arm from its starrting position + */ public class ExitStartingConfiguration extends CommandBase { ArmSubsystem arm; @@ -17,16 +19,18 @@ public ExitStartingConfiguration (ArmSubsystem arm) { // Called when the command is initially scheduled. @Override public void initialize() { - arm.setArmPosition(Constants.Arm.INTAKE_ANGLE); + arm.setArmPower(-.2); } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + arm.stopArm(); + } // Returns true when the command should end. @Override public boolean isFinished() { - return arm.isArmAtPosition(); + return arm.getArmPosition() <= 50; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Outtake.java b/src/main/java/frc/robot/commands/Outtake.java index daeab1f..63724d0 100644 --- a/src/main/java/frc/robot/commands/Outtake.java +++ b/src/main/java/frc/robot/commands/Outtake.java @@ -23,7 +23,7 @@ public Outtake (IntakeSubsystem intake, SerializerSubsystem serializer, ArmSubsy // Called when the command is initially scheduled. @Override public void initialize() { - arm.setArmPosition(Constants.Arm.INTAKE_ANGLE); + arm.setArmPosition(Constants.Arm.OUTTAKE_ANGLE); intake.outtake(); serializer.reverseBrushesAndRoller(); } diff --git a/src/main/java/frc/robot/commands/autos/ScorePreloads.java b/src/main/java/frc/robot/commands/autos/ScorePreloads.java index 29998c5..c088f86 100644 --- a/src/main/java/frc/robot/commands/autos/ScorePreloads.java +++ b/src/main/java/frc/robot/commands/autos/ScorePreloads.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; import frc.robot.commands.AimAndShoot; +import frc.robot.commands.ExitStartingConfiguration; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.SerializerSubsystem; @@ -12,13 +13,14 @@ public class ScorePreloads extends SequentialCommandGroup { public ScorePreloads (ArmSubsystem arm, DriveSubsystem drive, SerializerSubsystem serializer, ShooterSubsystem shooter) { addCommands( + new ExitStartingConfiguration(arm), new RunForTime( new AimAndShoot( Constants.Shooter.SHOOT_FROM_STARTING_LINE, - () -> true, - arm, serializer, shooter), - 14), - new DriveForward(-.25, .5, drive) + () -> shooter.getMainFlywheelSpeed() >= 200, + arm, serializer, shooter), + 12), + new DriveForward(-.25, 1.5, drive) ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index ecc5dcd..4ad219a 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.Controller; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -33,6 +34,7 @@ public class ArmSubsystem extends SubsystemBase { private Timer timer = new Timer(); private TrapezoidProfile.State targetState; private double currentSetDegrees = 0; + private boolean inClosedLoopMode = true; public ArmSubsystem () { // Set up motor @@ -73,6 +75,8 @@ public ArmSubsystem () { * @param degrees */ public void setArmPosition (double degrees) { + inClosedLoopMode = true; + if(degrees == currentSetDegrees){ return; } @@ -110,6 +114,19 @@ public boolean isArmAtPosition () { return profile.totalTime() > timer.get(); } + public void setArmPower (double power) { + inClosedLoopMode = false; + armMotor.set(ControlMode.PercentOutput, power); + } + + public void stopArm () { + if (!inClosedLoopMode) { + armMotor.stopMotor(); + setArmPositionToCurrentPosition(); + inClosedLoopMode = true; + } + } + /** * Used to prevent jerking the arm when the robot's been disabled and * the arm has fallen/changed positons @@ -134,13 +151,12 @@ public void aim () { public void periodic() { targetState = profile.calculate(timer.get()); - //if(isArmPositionSynced){ - // Set motor closed loop target to the TrapezoidProfile's calculated position + if (inClosedLoopMode) { armMotor.set( ControlMode.Position, UnitConversions.Arm.armDegreesToEncoderTicks(targetState.position) ); - //} + } SmartDashboard.putNumber("Arm setpoint", armMotor.getClosedLoopTarget()); SmartDashboard.putNumber("Arm setpoint degrees", UnitConversions.Arm.armEncoderTicksToDegrees(armMotor.getClosedLoopTarget())); @@ -148,5 +164,6 @@ public void periodic() { SmartDashboard.putNumber("Distance", Vision.getDistanceFromTarget(this)); SmartDashboard.putNumber("Aim angle", regression.evaluate(Vision.getDistanceFromTarget(this))); SmartDashboard.putNumber("Arm power", armMotor.getMotorOutputPercent()); + SmartDashboard.putBoolean("Arm closed loop?", inClosedLoopMode); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 709f91a..6f16f87 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -71,7 +71,7 @@ public void setDriveSpeeds (double left, double right) { * @param turn positive is right */ public void setDrivePowerArcade (double throttle, double turn) { - setDrivePower(throttle + turn, throttle - turn); + setDrivePower(-throttle + turn, -throttle - turn); } /** diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5d300fd..51571c6 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -40,13 +40,15 @@ public ShooterSubsystem () { configureMotorPair( mainFlywheelMaster, mainFlywheelSlave, - Constants.Shooter.MAIN_FLYWHEEL_PID); + Constants.Shooter.MAIN_FLYWHEEL_PID, + false); // Configure outer flywheel motors configureMotorPair( outerFlywheelMaster, outerFlywheelSlave, - Constants.Shooter.OUTER_FLYWHEEL_PID); + Constants.Shooter.OUTER_FLYWHEEL_PID, + true); t = new Timer(); t.start(); @@ -99,14 +101,15 @@ public void stopFlywheels () { outerFlywheelMaster.stopMotor(); } - private void configureMotorPair (TalonFX master, TalonFX slave, PIDParameters pid) { + private void configureMotorPair (TalonFX master, TalonFX slave, PIDParameters pid, boolean inverted) { master.configFactoryDefault(); master.setNeutralMode(NeutralMode.Coast); + master.setInverted(inverted); pid.configureMotorPID(master); //master.config_IntegralZone(0, UnitConversions.Shooter.mainFlywheelRPMToEncoderTicksPer100ms(RPM)) slave.follow(master); - slave.setInverted(true); + slave.setInverted(!inverted); } @Override diff --git a/src/main/java/friarLib2/commands/TimedCommand.java b/src/main/java/friarLib2/commands/TimedCommand.java new file mode 100644 index 0000000..8f1161d --- /dev/null +++ b/src/main/java/friarLib2/commands/TimedCommand.java @@ -0,0 +1,6 @@ +package friarLib2.commands; + +//TODO: Parent class for commands to run for some amount of time +public class TimedCommand { + +} From 86f98a97c4f7cca18d241d3f0c851f51653d8f36 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Sat, 6 Nov 2021 19:58:59 -0700 Subject: [PATCH 2/7] Beach Blitz #2 (end of saturday) --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/commands/autos/ScorePreloads.java | 1 + src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac17561..9517f26 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -115,7 +115,7 @@ public final static class Serializer { public static final int ACCLERATOR_MOTOR_ID = 15; /******** Tuning Constants ********/ - public static final double BRUSH_POWER = .6; + public static final double BRUSH_POWER = .80; public static final double ACCLERATOR_POWER = 1.0; public static final double ROLLER_POWER = 1.0; public static final double BRUSH_REVERSE_POWER = .75; @@ -133,7 +133,7 @@ public final static class Shooter { public static final int OUTER_FLYWHEEL_SLAVE_ID = 17; /******** PID Constants ********/ - public static final PIDParameters MAIN_FLYWHEEL_PID = new PIDParameters(1.3, 0.00005, 5, "Main flywheel PID"); + public static final PIDParameters MAIN_FLYWHEEL_PID = new PIDParameters(1.4, 0.00005, 5, "Main flywheel PID"); public static final PIDParameters OUTER_FLYWHEEL_PID = new PIDParameters(0.5, 0.00015, 6, "Outer flywheel PID"); /******** PID Constants ********/ @@ -141,9 +141,9 @@ public final static class Shooter { public static final double OUTER_FLYWHEEL_GEAR_RATIO = 1.0 / 1.0; /******** Tuning Constants ********/ - public static final int MAIN_FLYWHEEL_SPEED_TOLERANCE = 250; // Only shoot powercells if flywheels are within this range of their target speed + public static final int MAIN_FLYWHEEL_SPEED_TOLERANCE = 500; // Only shoot powercells if flywheels are within this range of their target speed public static final double MAIN_FLYWHEEL_ROC_TOLERANCE = 5000; // Only shoot powercells if the flywheels' speeds' rate of change is below this value - public static final int OUTER_FLYWHEEL_SPEED_TOLERANCE = 250; + public static final int OUTER_FLYWHEEL_SPEED_TOLERANCE = 500; public static final double OUTER_FLYWHEEL_ROC_TOLERANCE = 5000; public static final double MAIN_FLYWHEEL_SHOOTING_RPM = 5500; diff --git a/src/main/java/frc/robot/commands/autos/ScorePreloads.java b/src/main/java/frc/robot/commands/autos/ScorePreloads.java index c088f86..32a88e5 100644 --- a/src/main/java/frc/robot/commands/autos/ScorePreloads.java +++ b/src/main/java/frc/robot/commands/autos/ScorePreloads.java @@ -19,6 +19,7 @@ public ScorePreloads (ArmSubsystem arm, DriveSubsystem drive, SerializerSubsyste Constants.Shooter.SHOOT_FROM_STARTING_LINE, () -> shooter.getMainFlywheelSpeed() >= 200, arm, serializer, shooter), + /*:)*/ 12), new DriveForward(-.25, 1.5, drive) ); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 51571c6..643a769 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -135,5 +135,6 @@ public void periodic() { SmartDashboard.putNumber("Outer flywheel power", outerFlywheelMaster.getMotorOutputPercent()); SmartDashboard.putNumber("Main flywheel ROC", mainFlywheelSpeedROC); SmartDashboard.putNumber("Outer flywheel ROC", outerFlywheelSpeedROC); + SmartDashboard.putBoolean("Shooter up to speed?", isFlywheelUpToSpeed()); } } From 378e91d7e5d86421008ab9d72d4859e9efa626e9 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Sun, 7 Nov 2021 17:04:37 -0800 Subject: [PATCH 3/7] Beach Bitz #3 (end of sunday) --- README.md | 2 + src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 14 ++++++- .../frc/robot/commands/BrakeFlywheels.java | 33 +++++++++++++++++ .../robot/commands/MoveArmUpForDefense.java | 37 +++++++++++++++++++ .../robot/commands/autos/ScorePreloads.java | 6 +-- .../robot/subsystems/ShooterSubsystem.java | 11 +++++- 7 files changed, 98 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/BrakeFlywheels.java create mode 100644 src/main/java/frc/robot/commands/MoveArmUpForDefense.java diff --git a/README.md b/README.md index e5d3dfd..e8956cd 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,8 @@ The driver uses two large joysticks and the operator uses an xbox controller. **Xbox controller (port 2)**: - Left bumper: deploy climber - Left trigger (only after climber has been deployed): activate climber winch + - Right bumper: zero arm encoder + - Right trigger: brake flywheels - Up d-pad: TBD - Down d-pad: Automatically aim - Left d-pad: TBD diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9517f26..215fe87 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -151,7 +151,7 @@ public final static class Shooter { /******** Firing soulutions ********/ public static final FiringSolution LOB_INTO_GOAL = new FiringSolution(5500, 3000, 20); - public static final FiringSolution SHOOT_FROM_STARTING_LINE = new FiringSolution(100); + public static final FiringSolution SHOOT_FROM_STARTING_LINE = new FiringSolution(107); /** * A 2D array for tuning the shooter. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 328515c..77fadf6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,10 +10,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.commands.AimAndShoot; +import frc.robot.commands.BrakeFlywheels; import frc.robot.commands.Climb; import frc.robot.commands.DriveTeleop; import frc.robot.commands.Intake; import frc.robot.commands.LineUpWithTarget; +import frc.robot.commands.MoveArmUpForDefense; import frc.robot.commands.Outtake; import frc.robot.commands.ReverseSerializerRoller; import frc.robot.commands.autos.ScorePreloads; @@ -44,13 +46,15 @@ public class RobotContainer { private final ShooterSubsystem shooter = new ShooterSubsystem(); private SendableChooser autoChooser = new SendableChooser(); - private final ScorePreloads preloadAuto = new ScorePreloads(arm, drive, serializer, shooter); + private final ScorePreloads scorePreloadsTowardsDriverStation = new ScorePreloads(-.25, arm, drive, serializer, shooter); + private final ScorePreloads scorePreloadsAwayFromDriverStation = new ScorePreloads(.25, arm, drive, serializer, shooter); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - autoChooser.setDefaultOption("Preload auto", preloadAuto); + autoChooser.setDefaultOption("Move towards driver station", scorePreloadsTowardsDriverStation); + autoChooser.addOption("Move away from driver station", scorePreloadsAwayFromDriverStation); SmartDashboard.putData(autoChooser); configureDefaultCommands(); @@ -112,6 +116,12 @@ private void configureButtonBindings() { new LambdaTrigger(() -> OperatorInterface.OperatorController.getBumper(Hand.kRight)) .whileActiveContinuous(() -> arm.zeroArmPosition(), arm); + + new LambdaTrigger(() -> OperatorInterface.OperatorController.getTriggerAxis(Hand.kRight) >= .1) + .whileActiveContinuous(new BrakeFlywheels(shooter)); + + new LambdaTrigger(() -> OperatorInterface.OperatorController.getStartButton()) + .whileActiveContinuous(new MoveArmUpForDefense(arm)); } /** diff --git a/src/main/java/frc/robot/commands/BrakeFlywheels.java b/src/main/java/frc/robot/commands/BrakeFlywheels.java new file mode 100644 index 0000000..4be1045 --- /dev/null +++ b/src/main/java/frc/robot/commands/BrakeFlywheels.java @@ -0,0 +1,33 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ShooterSubsystem; + +public class BrakeFlywheels extends CommandBase { + + ShooterSubsystem shooter; + + public BrakeFlywheels (ShooterSubsystem shooter) { + this.shooter = shooter; + + addRequirements(shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize () { + shooter.brakeFlywheels(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooter.stopFlywheels(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/MoveArmUpForDefense.java b/src/main/java/frc/robot/commands/MoveArmUpForDefense.java new file mode 100644 index 0000000..af60b59 --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveArmUpForDefense.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.ArmSubsystem; + +/** + * Move the arm all the way up to block other robots' shots + */ +public class MoveArmUpForDefense extends CommandBase { + + ArmSubsystem arm; + + public MoveArmUpForDefense (ArmSubsystem arm) { + this.arm = arm; + + addRequirements(arm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + arm.setArmPosition(180); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + arm.setArmPosition(Constants.Arm.STOWED_ANGLE); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/autos/ScorePreloads.java b/src/main/java/frc/robot/commands/autos/ScorePreloads.java index 32a88e5..8c8f768 100644 --- a/src/main/java/frc/robot/commands/autos/ScorePreloads.java +++ b/src/main/java/frc/robot/commands/autos/ScorePreloads.java @@ -11,17 +11,17 @@ import friarLib2.commands.RunForTime; public class ScorePreloads extends SequentialCommandGroup { - public ScorePreloads (ArmSubsystem arm, DriveSubsystem drive, SerializerSubsystem serializer, ShooterSubsystem shooter) { + public ScorePreloads (double drivePower, ArmSubsystem arm, DriveSubsystem drive, SerializerSubsystem serializer, ShooterSubsystem shooter) { addCommands( new ExitStartingConfiguration(arm), new RunForTime( new AimAndShoot( Constants.Shooter.SHOOT_FROM_STARTING_LINE, - () -> shooter.getMainFlywheelSpeed() >= 200, + () -> shooter.getMainFlywheelSpeed() >= 2000, arm, serializer, shooter), /*:)*/ 12), - new DriveForward(-.25, 1.5, drive) + new DriveForward(drivePower, 1.5, drive) ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 643a769..648b835 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -85,7 +85,7 @@ public double getOuterFlywheelSpeed () { * are below their tuned values */ public boolean isFlywheelUpToSpeed () { - boolean isRunning = getMainFlywheelSpeed() >= 50 && getOuterFlywheelSpeed() >= 50; + boolean isRunning = getMainFlywheelSpeed() >= 500 && getOuterFlywheelSpeed() >= 500; boolean mainFlywheelUpToSpeed = (mainFlywheelMaster.getClosedLoopError() <= Constants.Shooter.MAIN_FLYWHEEL_SPEED_TOLERANCE) && (mainFlywheelSpeedROC <= Constants.Shooter.MAIN_FLYWHEEL_ROC_TOLERANCE); @@ -97,6 +97,15 @@ public boolean isFlywheelUpToSpeed () { } public void stopFlywheels () { + mainFlywheelMaster.setNeutralMode(NeutralMode.Coast); + outerFlywheelMaster.setNeutralMode(NeutralMode.Coast); + mainFlywheelMaster.stopMotor(); + outerFlywheelMaster.stopMotor(); + } + + public void brakeFlywheels () { + mainFlywheelMaster.setNeutralMode(NeutralMode.Brake); + outerFlywheelMaster.setNeutralMode(NeutralMode.Brake); mainFlywheelMaster.stopMotor(); outerFlywheelMaster.stopMotor(); } From 567e3d94a42498238659d7bbd48d889272369b7b Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Tue, 9 Nov 2021 17:27:21 -0800 Subject: [PATCH 4/7] Document command bindings --- src/main/java/frc/robot/RobotContainer.java | 27 +++++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77fadf6..9bcb5d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.commands.Outtake; import frc.robot.commands.ReverseSerializerRoller; import frc.robot.commands.autos.ScorePreloads; -import frc.robot.commands.autos.TestAuto; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; @@ -77,25 +76,29 @@ private void configureButtonBindings() { new LambdaTrigger(() -> OperatorInterface.OperatorController.getBumper(Hand.kLeft)) .whenActive(() -> climber.deploy(), climber); - /** When the left trigger is pressed, activate the winch motor */ + /** While the left trigger is pressed, activate the winch motor */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getTriggerAxis(Hand.kLeft) >= .1) .whileActiveContinuous(new Climb(climber)); + /** While the A button is pressed, intake powercells */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getAButton()) .whileActiveContinuous(new Intake(intake, serializer, arm)); + /** While the X button is pressed, reverse the intake and serializer */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getXButton()) .whileActiveContinuous(new Outtake(intake, serializer, arm)); + /** While the Y button is pressed, reverse the serializer motor */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getYButton()) .whileActiveContinuous(new ReverseSerializerRoller(serializer)); + /** While d-pad down is pressed, automatically aim and shoot */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 180) .whileActiveContinuous( new AimAndShoot(() -> OperatorInterface.OperatorController.getBButton(), arm, serializer, shooter)); - // Lobbing into target + /** While d-pad up is pressed, shoot from against driverstation wall */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 0) .whileActiveContinuous( new AimAndShoot( @@ -103,7 +106,7 @@ private void configureButtonBindings() { () -> OperatorInterface.OperatorController.getBButton(), arm, serializer, shooter)); - // Shooting from starting line + /** While d-pad right is pressed, shoot from initiation line */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 90) .whileActiveContinuous( new AimAndShoot( @@ -111,15 +114,30 @@ private void configureButtonBindings() { () -> OperatorInterface.OperatorController.getBButton(), arm, serializer, shooter)); + /** While driver joystick trigger(s) are pressed, line up the robot with the target */ new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() && OperatorInterface.DriverRight.getTrigger()) .whileActiveContinuous(new LineUpWithTarget(drive)); + /** + * When right bumper is pressed, zero the arm encoder. + * + * This is in case the arm gearbox slips, which is mostly resolved, but + * still a good safety net + */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getBumper(Hand.kRight)) .whileActiveContinuous(() -> arm.zeroArmPosition(), arm); + /** While the right trigger is pressed, slow down the flywheels quickly */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getTriggerAxis(Hand.kRight) >= .1) .whileActiveContinuous(new BrakeFlywheels(shooter)); + /** + * While the start button on the xbox controller is pressed, move + * the arm all the way up to block opponents' shots. + * + * Used sucessfully in a Beach Blitz 2021 playoff match + * TODO: Find out what match that is + */ new LambdaTrigger(() -> OperatorInterface.OperatorController.getStartButton()) .whileActiveContinuous(new MoveArmUpForDefense(arm)); } @@ -134,7 +152,6 @@ public Command getAutonomousCommand() { } public Command getTeleopInitCommand() { - System.out.println("Teleop init"); arm.setArmPositionToCurrentPosition(); return new InstantCommand(arm::setArmPositionToCurrentPosition, arm); } From 06e86064fce9839ad66bb2990299c22ff0e70d98 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Tue, 9 Nov 2021 17:51:23 -0800 Subject: [PATCH 5/7] Documentation and code cleanup --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/Vision.java | 3 --- .../java/frc/robot/commands/ExitStartingConfiguration.java | 7 ++++--- .../java/frc/robot/subsystems/SerializerSubsystem.java | 6 +++--- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 -- src/main/java/frc/robot/util/FiringSolution.java | 3 ++- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 215fe87..fb5e3fd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,6 +66,9 @@ public final static class Arm { /******** Arm Motion Constraints ********/ public static final double MAX_VELOCITY_DEGREES_PER_SEC = 700; public static final double MAX_ACCLERATION_DEGREES_PER_SEC_SQUARED = 400; + + public static final double EXIT_STARTING_CONFIG_POWER = -.2; + public static final double EXIT_STARTInG_CONFIG_ANGLE = 50; // Arm stops moving out of starting config at this angle } /** diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java index 9eb0a28..e10f135 100644 --- a/src/main/java/frc/robot/Vision.java +++ b/src/main/java/frc/robot/Vision.java @@ -1,8 +1,5 @@ package frc.robot; -import org.photonvision.PhotonCamera; - -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Units; import frc.robot.subsystems.ArmSubsystem; import friarLib2.vision.PhotonCameraWrapper; diff --git a/src/main/java/frc/robot/commands/ExitStartingConfiguration.java b/src/main/java/frc/robot/commands/ExitStartingConfiguration.java index 07bc70d..59a45cd 100644 --- a/src/main/java/frc/robot/commands/ExitStartingConfiguration.java +++ b/src/main/java/frc/robot/commands/ExitStartingConfiguration.java @@ -1,10 +1,11 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; /** - * Unclip the arm from its starrting position + * Unclip the arm from its starting position */ public class ExitStartingConfiguration extends CommandBase { @@ -19,7 +20,7 @@ public ExitStartingConfiguration (ArmSubsystem arm) { // Called when the command is initially scheduled. @Override public void initialize() { - arm.setArmPower(-.2); + arm.setArmPower(Constants.Arm.EXIT_STARTING_CONFIG_POWER); } // Called once the command ends or is interrupted. @@ -31,6 +32,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return arm.getArmPosition() <= 50; + return arm.getArmPosition() <= Constants.Arm.EXIT_STARTInG_CONFIG_ANGLE; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SerializerSubsystem.java b/src/main/java/frc/robot/subsystems/SerializerSubsystem.java index a6e787d..b00c48f 100644 --- a/src/main/java/frc/robot/subsystems/SerializerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SerializerSubsystem.java @@ -11,8 +11,8 @@ /** * Represents the serializer subsystem * - *

The physical subsystem includes the roller brushes and indexer - * wheel + *

The physical subsystem includes the brushes, gate wheel, and + * serializer roller */ public class SerializerSubsystem extends SubsystemBase { @@ -83,7 +83,7 @@ public void reverseBrushesAndRoller () { } /** - * Turn the acclerator on or off + * Turn the acclerator (gate) wheel on or off * * @param on */ diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 648b835..9834476 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -60,7 +60,6 @@ public ShooterSubsystem () { * @param RPM */ public void setFlywheelSpeeds (double mainRPM, double outerRPM) { - System.out.println(UnitConversions.Shooter.mainFlywheelRPMToEncoderTicksPer100ms(mainRPM)); mainFlywheelMaster.set(ControlMode.Velocity, UnitConversions.Shooter.mainFlywheelRPMToEncoderTicksPer100ms(mainRPM)); outerFlywheelMaster.set(ControlMode.Velocity, UnitConversions.Shooter.outerFlywheelRPMToEncoderTicksPer100ms(outerRPM)); } @@ -115,7 +114,6 @@ private void configureMotorPair (TalonFX master, TalonFX slave, PIDParameters pi master.setNeutralMode(NeutralMode.Coast); master.setInverted(inverted); pid.configureMotorPID(master); - //master.config_IntegralZone(0, UnitConversions.Shooter.mainFlywheelRPMToEncoderTicksPer100ms(RPM)) slave.follow(master); slave.setInverted(!inverted); diff --git a/src/main/java/frc/robot/util/FiringSolution.java b/src/main/java/frc/robot/util/FiringSolution.java index 99ec32c..0c2dc99 100644 --- a/src/main/java/frc/robot/util/FiringSolution.java +++ b/src/main/java/frc/robot/util/FiringSolution.java @@ -26,7 +26,8 @@ public FiringSolution (double armAngle) { } /** - * Set the arm and shooter speeds to + * Set the arm and shooter speeds to the values stored in this + * object * * @param arm * @param shooter From 197309a0410a359841e7f2e9955e5459e1599850 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Thu, 11 Nov 2021 18:47:32 -0800 Subject: [PATCH 6/7] Tune shooting from trench --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fb5e3fd..a5c078a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,6 +155,7 @@ public final static class Shooter { /******** Firing soulutions ********/ public static final FiringSolution LOB_INTO_GOAL = new FiringSolution(5500, 3000, 20); public static final FiringSolution SHOOT_FROM_STARTING_LINE = new FiringSolution(107); + public static final FiringSolution SHOOT_FROM_TRENCH = new FiringSolution(125); /** * A 2D array for tuning the shooter. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9bcb5d0..281975d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -114,6 +114,14 @@ private void configureButtonBindings() { () -> OperatorInterface.OperatorController.getBButton(), arm, serializer, shooter)); + /** While d-pad left is pressed, shoot from the trench */ + new LambdaTrigger(() -> OperatorInterface.OperatorController.getPOV() == 270) + .whileActiveContinuous( + new AimAndShoot( + Constants.Shooter.SHOOT_FROM_TRENCH, + () -> OperatorInterface.OperatorController.getBButton(), + arm, serializer, shooter)); + /** While driver joystick trigger(s) are pressed, line up the robot with the target */ new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() && OperatorInterface.DriverRight.getTrigger()) .whileActiveContinuous(new LineUpWithTarget(drive)); From 557dbc1d428d1f505173931d82dd9bc6899aea4d Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Thu, 11 Nov 2021 20:14:21 -0800 Subject: [PATCH 7/7] Tune automatic aiming --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/Vision.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a5c078a..68599f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -84,7 +84,7 @@ public final static class Drive { /******** PID Constants ********/ public static final PIDParameters WHEEL_PID_CONSTANTS = new PIDParameters(0.3, 0.0001, .6, "Drive wheel PID"); - public static final PIDController ROTATION_PID_CONTROLLER = new PIDController(0.02, 0, 0.0005); + public static final PIDController ROTATION_PID_CONTROLLER = new PIDController(0.023, 0.0001, 0.003); /******** Physical Constants ********/ public static final double GEAR_RATIO = 10.7; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 281975d..6f6897c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,7 +123,7 @@ private void configureButtonBindings() { arm, serializer, shooter)); /** While driver joystick trigger(s) are pressed, line up the robot with the target */ - new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() && OperatorInterface.DriverRight.getTrigger()) + new LambdaTrigger(() -> OperatorInterface.DriverLeft.getTrigger() || OperatorInterface.DriverRight.getTrigger()) .whileActiveContinuous(new LineUpWithTarget(drive)); /** diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java index e10f135..2863aa4 100644 --- a/src/main/java/frc/robot/Vision.java +++ b/src/main/java/frc/robot/Vision.java @@ -10,7 +10,7 @@ * Container for the vision systems */ public class Vision { - public static VisionCamera mainCamera = new PhotonCameraWrapper("gloworm"); + public static VisionCamera mainCamera = new PhotonCameraWrapper("camera"); private static double lastDistance = 0;