From 25ede0e235b9c7d4c207b2aba7cc019f9e1b6dd2 Mon Sep 17 00:00:00 2001 From: David Tian Date: Sat, 26 Oct 2024 09:30:52 -0700 Subject: [PATCH] upload --- .../com/team841/calliope/RobotContainer.java | 15 ++++++----- .../team841/calliope/drive/Drivetrain.java | 26 ++++--------------- .../superstructure/hanger/Hanger.java | 2 +- .../superstructure/shooter/Shooter.java | 2 +- .../superstructure/shooter/ShooterIO.java | 3 +++ .../shooter/ShooterIOTalonFX.java | 7 +++++ 6 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/team841/calliope/RobotContainer.java b/src/main/java/com/team841/calliope/RobotContainer.java index 2e0b9d5..546a754 100644 --- a/src/main/java/com/team841/calliope/RobotContainer.java +++ b/src/main/java/com/team841/calliope/RobotContainer.java @@ -92,8 +92,6 @@ public static RobotContainer getInstance() { } public RobotContainer() { - registerNamedCommands(); - switch (RC.robotType) { default -> { if (RC.robot == RC.Robot.CALLIOPE) @@ -118,9 +116,12 @@ public RobotContainer() { } } + registerNamedCommands(); + + if (RC.robot == RC.Robot.NIKE){ this.sticksXbox = new CommandXboxController[1]; - this.sticksPS5 = new CommandPS5Controller[1]; + this.sticksPS5 = new CommandPS5Controller[0]; this.sticksXbox[0] = new CommandXboxController(RC.Controllers.soloStick); } else { this.sticksPS5 = new CommandPS5Controller[1]; @@ -153,9 +154,9 @@ public RobotContainer() { this.drivetrain.setDefaultCommand(bioDrive); if (RC.robot == RC.Robot.NIKE){ - configureDuoStick(); - } else { configureSoloStick(); + } else { + configureDuoStick(); } } @@ -205,8 +206,8 @@ private void configureSoloStick() { new SequentialCommandGroup( new InstantCommand(indexer::stopIndexer), new InstantCommand(shooter::stopShooter))); - //sticksXbox[0].rightStick().whileTrue(new InstantCommand(hanger::ExtendHanger)).onFalse(new InstantCommand(hanger::StopHanger)); - //sticksXbox[0].leftStick().whileTrue(new InstantCommand(hanger::RetractHanger)).onFalse(new InstantCommand(hanger::StopHanger)); + sticksXbox[0].rightStick().whileTrue(new InstantCommand(hanger::ExtendHanger)).onFalse(new InstantCommand(hanger::StopHanger)); + sticksXbox[0].leftStick().whileTrue(new InstantCommand(hanger::RetractHanger)).onFalse(new InstantCommand(hanger::StopHanger)); sticksXbox[0].povCenter().whileTrue(new InstantCommand(hanger::StopHanger)); sticksXbox[0].povLeft().whileTrue(new InstantCommand(hanger::toggleHanger)); sticksXbox[0] diff --git a/src/main/java/com/team841/calliope/drive/Drivetrain.java b/src/main/java/com/team841/calliope/drive/Drivetrain.java index 5db85f1..0972e65 100644 --- a/src/main/java/com/team841/calliope/drive/Drivetrain.java +++ b/src/main/java/com/team841/calliope/drive/Drivetrain.java @@ -42,12 +42,6 @@ public class Drivetrain extends SwerveDrivetrain implements Subsystem { StructPublisher limelightPublisher = limelightTopic.publish(); StructPublisher ctrePublisher = ctreTopic.publish(); - LoggedTunableNumber kp = new LoggedTunableNumber("ki"); - LoggedTunableNumber ki = new LoggedTunableNumber("ki"); - LoggedTunableNumber kd = new LoggedTunableNumber("kd"); - - Double oldKP, oldKI, oldKD; - private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -116,13 +110,17 @@ private void ConfigureMotors() { } } + public void emtpyReturn(Pose2d input){ + return; + } + public void configurePathplanner() { double driveBaseRadius = 0; AutoBuilder.configureHolonomic( () -> this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto + this::emtpyReturn, // Consumer for seeding pose against auto this::getCurrentRobotChassisSpeeds, (speeds) -> this.setControl( @@ -267,19 +265,5 @@ public void periodic() { SmartDashboard.putBoolean("In Dinstance", inRangeToSpeaker()); SmartDashboard.putNumber("tune-target", 0.00); SmartDashboard.putNumber("tune-angle", this.getState().Pose.getRotation().getDegrees()); - - if (Swerve.controller != null && oldKP == null){ - this.oldKP = Swerve.controller.getP(); - this.oldKI = Swerve.controller.getI(); - this.oldKD = Swerve.controller.getD(); - } - - if (kp.hasChanged() || ki.hasChanged() || kd.hasChanged()){ - if (Swerve.controller != null){ - Swerve.controller.setP(kp.get()); - Swerve.controller.setI(ki.get()); - Swerve.controller.setD(kd.get()); - } - } } } diff --git a/src/main/java/com/team841/calliope/superstructure/hanger/Hanger.java b/src/main/java/com/team841/calliope/superstructure/hanger/Hanger.java index 4488158..4cc5533 100644 --- a/src/main/java/com/team841/calliope/superstructure/hanger/Hanger.java +++ b/src/main/java/com/team841/calliope/superstructure/hanger/Hanger.java @@ -29,7 +29,7 @@ public void periodic() { } public void ExtendHanger() { - io.setVelocity(1.00); + io.setVelocity(-1.00); } public void RetractHanger() { diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java b/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java index a67f57f..fa6633a 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java @@ -62,7 +62,7 @@ public boolean isHighShot() { } public void stopShooter() { - io.setMotionMagicVelocityVoltageOutput(-0.015); + io.setDutyCycle(-0.05); } public Command runShooter(double velocity) { diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java index f3ee7ed..88e291f 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java @@ -9,11 +9,14 @@ public class ShooterIOInputs{ public double bottomVelocity; public double topMotionMagicVoltageVelocity; public double bottomMotionMagicVoltageVelocity; + public double dutyCycleOut; } public default void updateInputs(ShooterIOInputs inputs) { } + public default void setDutyCycle(double value) { } + public default void setMotionMagicVelocityVoltageOutput(double velocity) { } public default void setMotionMagicVelocityVoltageOutput(double topVelocity, double bottomVelocity) { } diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java index ccf170d..91017e2 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java @@ -29,6 +29,7 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.bottomVelocity = bottomShooter.getVelocity().getValue(); inputs.bottomMotionMagicVoltageVelocity = bottomControl.Velocity; inputs.topMotionMagicVoltageVelocity = topControl.Velocity; + inputs.dutyCycleOut = topShooter.getDutyCycle().getValue(); } @Override @@ -43,6 +44,12 @@ public void setMotionMagicVelocityVoltageOutput(double topVelocity, double botto topShooter.setControl(topControl.withVelocity(bottomVelocity)); } + @Override + public void setDutyCycle(double value){ + topShooter.set(value); + bottomShooter.set(value); + } + @Override public void stop() { bottomShooter.stopMotor();