From 3da5cb5e3364b438a273ba273fd05cd129d8028d Mon Sep 17 00:00:00 2001 From: juliashimshock <24shimshockjulia@bprep.org> Date: Sat, 17 Feb 2024 14:45:40 -0800 Subject: [PATCH 1/5] bad linkage code --- .../frc/robot/commands/RunExtendIntake.java | 26 +++++++++++++------ .../java/frc/robot/subsystems/Linkage.java | 2 +- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/RunExtendIntake.java b/src/main/java/frc/robot/commands/RunExtendIntake.java index 2bfbbf7c..57ef79a9 100644 --- a/src/main/java/frc/robot/commands/RunExtendIntake.java +++ b/src/main/java/frc/robot/commands/RunExtendIntake.java @@ -20,7 +20,8 @@ enum IntakeCases {CHECK_ROBOT_EMPTY, EXTEND_INTAKE, WAIT_FOR_SENSOR, UP_TO_SHOOT private static XboxController operatorCont = new XboxController(1); private Timer timer = new Timer(); private Timer sensorTimer = new Timer(); - private double setPoint; + private boolean isAuto; + private boolean spiked = false; private IntakeCases state = IntakeCases.CHECK_ROBOT_EMPTY; @@ -33,11 +34,11 @@ public RunExtendIntake(Intake intake) { addRequirements(intake); } - // public RunExtendIntake(boolean isAuto) { - // inAuto = isAuto; - // // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(intake); - // } + public RunExtendIntake(Intake intake, boolean isAuto) { + this.isAuto = isAuto; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intake); + } // Called when the command is initially scheduled. @Override @@ -58,8 +59,13 @@ public void execute() { break; case EXTEND_INTAKE: intake.run(.65); // we should extend too but idk how we should implement this - //linkage.setAngle(180); + if(isAuto) { + linkage.setAngle(180); // dunno if this value is right + } if(intake.getAmps() > 20 && timer.get() > .25) { + spiked = true; + } + if(linkage.isAtSetpoint() && spiked) { sensorTimer.start(); state = IntakeCases.WAIT_FOR_SENSOR; } @@ -69,7 +75,7 @@ public void execute() { // if(!intake.getHighSensor()) { // state = IntakeCases.UP_TO_SHOOTER_P1; // } - if(sensorTimer.get() > 1) { + if(sensorTimer.get() > 1.0) { state = IntakeCases.EXTEND_INTAKE; sensorTimer.reset(); } @@ -154,9 +160,13 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { if(state == IntakeCases.RETRACT_STOP) { + linkage.setAngle(0); // dunno if this value is right System.out.print("COMMAND ENDED"); + if(linkage.isAtSetpoint()) { return true; + } } return false; } } +//pretty sure all of this is garbage idk if my logic is right at all lol \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Linkage.java b/src/main/java/frc/robot/subsystems/Linkage.java index dc320910..8b7fc275 100644 --- a/src/main/java/frc/robot/subsystems/Linkage.java +++ b/src/main/java/frc/robot/subsystems/Linkage.java @@ -43,7 +43,7 @@ public Linkage(LinkageIO io) { } public boolean isAtSetpoint() { - return false; + return this.getAngle() == this.positionSetpoint ? true : false; } public void run(double speed) { From 72e219aa7921756a39b106826cabe531496012e0 Mon Sep 17 00:00:00 2001 From: juliashimshock <24shimshockjulia@bprep.org> Date: Sat, 17 Feb 2024 17:59:06 -0800 Subject: [PATCH 2/5] fixed linkage --- .../frc/robot/commands/RunExtendIntake.java | 18 +++++++++--------- .../java/frc/robot/subsystems/Linkage.java | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/RunExtendIntake.java b/src/main/java/frc/robot/commands/RunExtendIntake.java index 57ef79a9..2a64d5f0 100644 --- a/src/main/java/frc/robot/commands/RunExtendIntake.java +++ b/src/main/java/frc/robot/commands/RunExtendIntake.java @@ -28,16 +28,19 @@ enum IntakeCases {CHECK_ROBOT_EMPTY, EXTEND_INTAKE, WAIT_FOR_SENSOR, UP_TO_SHOOT /** Creates a new Java. */ - public RunExtendIntake(Intake intake) { + public RunExtendIntake(Intake intake, Linkage linkage) { this.intake = intake; + this.linkage = linkage; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(intake); + addRequirements(intake, linkage); } - public RunExtendIntake(Intake intake, boolean isAuto) { + public RunExtendIntake(Intake intake, Linkage linkage, boolean isAuto) { this.isAuto = isAuto; + this.intake = intake; + this.linkage = linkage; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(intake); + addRequirements(intake, linkage); } // Called when the command is initially scheduled. @@ -60,12 +63,9 @@ public void execute() { case EXTEND_INTAKE: intake.run(.65); // we should extend too but idk how we should implement this if(isAuto) { - linkage.setAngle(180); // dunno if this value is right + linkage.setAngle(0.0); // dunno if this value is right } if(intake.getAmps() > 20 && timer.get() > .25) { - spiked = true; - } - if(linkage.isAtSetpoint() && spiked) { sensorTimer.start(); state = IntakeCases.WAIT_FOR_SENSOR; } @@ -160,7 +160,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { if(state == IntakeCases.RETRACT_STOP) { - linkage.setAngle(0); // dunno if this value is right + // dunno if this value is right System.out.print("COMMAND ENDED"); if(linkage.isAtSetpoint()) { return true; diff --git a/src/main/java/frc/robot/subsystems/Linkage.java b/src/main/java/frc/robot/subsystems/Linkage.java index 8b7fc275..554c422f 100644 --- a/src/main/java/frc/robot/subsystems/Linkage.java +++ b/src/main/java/frc/robot/subsystems/Linkage.java @@ -43,7 +43,7 @@ public Linkage(LinkageIO io) { } public boolean isAtSetpoint() { - return this.getAngle() == this.positionSetpoint ? true : false; + return Math.abs(this.getAngle()-this.positionSetpoint) < 1.0; } public void run(double speed) { From 738f0f8469fc35dfda1cf0361ed320295404917e Mon Sep 17 00:00:00 2001 From: juliashimshock <24shimshockjulia@bprep.org> Date: Sat, 17 Feb 2024 18:52:34 -0800 Subject: [PATCH 3/5] Update RobotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc4a1673..04e02489 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -241,6 +241,7 @@ public void configureCharacterizationBindings(){ driverController.y().whileTrue(drivetrain.sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); } public void onDisable() { + climber.stop(); flywheel.stop(); intake.stop(); linkage.stop(); From d98cda87c48d8aaa0b7013f95b5e20e74cce5067 Mon Sep 17 00:00:00 2001 From: juliashimshock <24shimshockjulia@bprep.org> Date: Sun, 18 Feb 2024 12:57:03 -0800 Subject: [PATCH 4/5] got rid of the comment yw james --- src/main/java/frc/robot/commands/RunExtendIntake.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/RunExtendIntake.java b/src/main/java/frc/robot/commands/RunExtendIntake.java index 2a64d5f0..ff6050cd 100644 --- a/src/main/java/frc/robot/commands/RunExtendIntake.java +++ b/src/main/java/frc/robot/commands/RunExtendIntake.java @@ -169,4 +169,3 @@ public boolean isFinished() { return false; } } -//pretty sure all of this is garbage idk if my logic is right at all lol \ No newline at end of file From 7313b4d4eb400c9f01dd908db9ae73cf90edea20 Mon Sep 17 00:00:00 2001 From: juliashimshock <24shimshockjulia@bprep.org> Date: Sun, 18 Feb 2024 15:01:37 -0800 Subject: [PATCH 5/5] l]inkage intake changse --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- src/main/java/frc/robot/commands/PowerIntake.java | 12 ++++++------ .../java/frc/robot/commands/RunExtendIntake.java | 4 ++-- .../{SetLinkageTalon.java => SetLinkage.java} | 4 ++-- src/main/java/frc/robot/subsystems/Intake.java | 1 + src/main/java/frc/robot/utils/CommandFactory.java | 2 +- 6 files changed, 16 insertions(+), 13 deletions(-) rename src/main/java/frc/robot/commands/{SetLinkageTalon.java => SetLinkage.java} (92%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04e02489..d8c32e65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,7 @@ import frc.robot.commands.PowerIntake; import frc.robot.commands.PowerLinkage; import frc.robot.commands.SetIntake; -import frc.robot.commands.SetLinkageTalon; +import frc.robot.commands.SetLinkage; import frc.robot.commands.ShootInSpeaker; import frc.robot.commands.PowerFlywheel; import frc.robot.commands.RobotOrientedDrive; @@ -171,7 +171,7 @@ public RobotContainer() { private final void initializeCommands() { fieldOrientedDrive = new FieldOrientedDrive(drivetrain); robotOrientedDrive = new RobotOrientedDrive(drivetrain); - runExtendIntake = new RunExtendIntake(intake); + runExtendIntake = new RunExtendIntake(intake, linkage); powerIntakeReversed = new PowerIntakeReversed(intake); powerIntake = new PowerIntake(intake); powerFlywheel = new PowerFlywheel(flywheel); @@ -204,6 +204,7 @@ private void configureDefaultCommands() { // flywheel.setDefaultCommand(powerFlywheel); // drivetrain.setDefaultCommand(fieldOrientedDrive); climber.setDefaultCommand(powerClimber); + //intake.setDefaultCommand(powerIntake); } /** @@ -227,6 +228,7 @@ private void configureBindings() { operatorController.a().onTrue(levelClimbers); operatorController.b().onTrue(new InstantCommand(() -> climber.zeroBoth(), climber)); + operatorController.y().toggleOnTrue(runExtendIntake); // driverController.x().whileTrue(new InstantCommand(() -> // drivetrain.zero(),drivetrain)); diff --git a/src/main/java/frc/robot/commands/PowerIntake.java b/src/main/java/frc/robot/commands/PowerIntake.java index 892cfdd2..76a12bfc 100644 --- a/src/main/java/frc/robot/commands/PowerIntake.java +++ b/src/main/java/frc/robot/commands/PowerIntake.java @@ -28,13 +28,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // if(operatorCont.getRightTriggerAxis() > .75) { - // intake.run(-.5); - // } else { - // intake.run(-.15); - // } + if(operatorCont.getRightTriggerAxis() > .75) { + intake.run(.5); + } else { + intake.run(.15); + } - intake.run(1.0); + // intake.run(1.0); CommandLogger.logCommandRunning(this); } diff --git a/src/main/java/frc/robot/commands/RunExtendIntake.java b/src/main/java/frc/robot/commands/RunExtendIntake.java index ff6050cd..4fb40735 100644 --- a/src/main/java/frc/robot/commands/RunExtendIntake.java +++ b/src/main/java/frc/robot/commands/RunExtendIntake.java @@ -57,11 +57,11 @@ public void execute() { switch(state){ case CHECK_ROBOT_EMPTY: if(intake.getSideSensor()) { - state = IntakeCases.EXTEND_INTAKE; + state = IntakeCases.WAIT_FOR_SENSOR; } break; case EXTEND_INTAKE: - intake.run(.65); // we should extend too but idk how we should implement this + intake.run(.7); // we should extend too but idk how we should implement this if(isAuto) { linkage.setAngle(0.0); // dunno if this value is right } diff --git a/src/main/java/frc/robot/commands/SetLinkageTalon.java b/src/main/java/frc/robot/commands/SetLinkage.java similarity index 92% rename from src/main/java/frc/robot/commands/SetLinkageTalon.java rename to src/main/java/frc/robot/commands/SetLinkage.java index 85721d09..b75f4490 100644 --- a/src/main/java/frc/robot/commands/SetLinkageTalon.java +++ b/src/main/java/frc/robot/commands/SetLinkage.java @@ -8,11 +8,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Linkage; -public class SetLinkageTalon extends Command { +public class SetLinkage extends Command { final Linkage linkage; /** Creates a new SetLinkageTa\ * lon. */ - public SetLinkageTalon(Linkage linkage) { + public SetLinkage(Linkage linkage) { // Use addRequirements() here to declare subsystem dependencies. this.linkage = linkage; addRequirements(linkage); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 09381ce9..b57e0e4d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -64,6 +64,7 @@ public double getAmps() { public void periodic() { // This method will be called once per scheduler run io.updateInputs(inputs); + SmartDashboard.putNumber("Amps", getAmps()); Logger.processInputs("Intake", inputs); } } diff --git a/src/main/java/frc/robot/utils/CommandFactory.java b/src/main/java/frc/robot/utils/CommandFactory.java index aa562ee7..f4881dac 100644 --- a/src/main/java/frc/robot/utils/CommandFactory.java +++ b/src/main/java/frc/robot/utils/CommandFactory.java @@ -50,7 +50,7 @@ public PowerLinkage powerLinkage() { // returns type runExtendIntake public RunExtendIntake runExtendIntake() { - return new RunExtendIntake(intake); + return new RunExtendIntake(intake, linkage); } // returns type runLinkage