Changes from March 19 (#82)

* Put SwerveDriveToPose.toClimb into SwerveDriveToClimb, remove isFinished from climb * Shuffleboard config * Run conveyor to amp on climber up * Shuffleboard config * Log turn current * Change auton name to 3.5 * Reformat * Lower drive current limit * Log alignment target speed * Change global constraints, make almost all paths global constraints * Fix HGF paths * Move amp scoring back * Increase align debounce, increase max height lift * Change default path constraints * Leave shooter on all the time * Increase epsilon for leds at trap height * Tune paths, reduce auton shooting debounce * Reduce distance tolerance, increase shot distance * Remove shooter startup delays * Tune auto * Remove auto trap score * Decrease teleop std devs * Filter only center speaker tag when aligning * Undo tag filtering alignment * Adjust 5 piece second pickup * Up teleop std devs * Rewrite amper methods, add AmperScoreSpeed * Settings * Tuning changes * Tune 5 piece * Remove kD on shoot align * Tune hgf * Shoot align kD * Add velocity check to is aligned to speaker * Disable livewindow, controls * Fix trap conveyor to amp on driver * Increase climber deadband * Remove area weighted average * Decrease velocity tolerance for shooting alignment * Add leds when climb up * Add ADE and DE autos * Auto tuning * Remove specific shot tolerance, set default auto * Re zero back left * Changes from march 19 * Push DS logs from elim 1 --------- Co-authored-by: Prog694 <[email protected]>
StuyPulse · Mar 20, 2024 · 8342625 · 8342625
1 parent f4b80a7
commit 8342625
Show file tree

Hide file tree

Showing 14 changed files with 25 additions and 14 deletions.
diff --git a/dslogs/2024_03_16 13_43_58 Sat.dsevents b/dslogs/2024_03_16 13_43_58 Sat.dsevents
diff --git a/dslogs/2024_03_16 13_43_58 Sat.dslog b/dslogs/2024_03_16 13_43_58 Sat.dslog
diff --git a/dslogs/2024_03_16 13_44_45 Sat.dsevents b/dslogs/2024_03_16 13_44_45 Sat.dsevents
diff --git a/dslogs/2024_03_16 13_44_45 Sat.dslog b/dslogs/2024_03_16 13_44_45 Sat.dslog
diff --git a/dslogs/2024_03_16 13_58_15 Sat.dsevents b/dslogs/2024_03_16 13_58_15 Sat.dsevents
diff --git a/dslogs/2024_03_16 13_58_15 Sat.dslog b/dslogs/2024_03_16 13_58_15 Sat.dslog
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -148,7 +148,6 @@ private void configureDriverBindings() {
         // note to amper and align then score
         driver.getLeftBumper()
             .whileTrue(new AmpScoreRoutine())
-            .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT))
             .onFalse(new AmperStop());
 
         // score speaker no align

diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java
@@ -79,6 +79,10 @@ public AmpScoreRoutine() {
 
             AmperScore.untilDone(),
 
+            new WaitCommand(0.25),
+
+            new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT),
+
             new SwerveDriveDriveDirection(
                 new Vector2D(new Translation2d(
                     Score.DRIVE_AWAY_SPEED, 

diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperToConveyor.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperToConveyor.java
@@ -6,31 +6,33 @@
 
 package com.stuypulse.robot.commands.amper;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.amper.Amper;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AmperToConveyor extends Command {
 
     private final Amper amper;
+    private final Shooter shooter;
 
     public AmperToConveyor() {
         amper = Amper.getInstance();
+        shooter = Shooter.getInstance();
+
         addRequirements(amper);
     }
 
     @Override
     public void initialize() {
         amper.toConveyor();
+        shooter.setTargetSpeeds(Settings.Shooter.REVERSE);
     }
 
-    // @Override
-    // public boolean isFinished() {
-    //     return amper.hasNote();
-    // }
+    @Override
+    public void end(boolean interrupted) {
+        shooter.setTargetSpeeds(Settings.Shooter.PODIUM_SHOT);
+    }
 
-    // @Override
-    // public void end(boolean interrupted) {
-    //     amper.stopRoller();
-    // }
 }
diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java
@@ -51,7 +51,7 @@ public interface Swerve {
     }
 
     public interface Intake {
-        CANSparkConfig MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.1);
+        CANSparkConfig MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.25);
     }
 
     public interface Shooter {
@@ -60,7 +60,7 @@ public interface Shooter {
     }
 
     public interface Conveyor {
-        CANSparkConfig GANDALF_MOTOR = new CANSparkConfig(true, IdleMode.kBrake,500, 0.1);
+        CANSparkConfig GANDALF_MOTOR = new CANSparkConfig(true, IdleMode.kBrake,500, 0.25);
         CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.1);
     }
 

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -315,6 +315,8 @@ public interface Shooter {
             new SmartNumber("Shooter/Podium Feeder RPM", 3000));
 
         ShooterSpeeds HANDOFF = new ShooterSpeeds(3000, 3000);
+
+        ShooterSpeeds REVERSE = new ShooterSpeeds(-3000, -3000);
 
         double AT_RPM_EPSILON = 125;
 
@@ -365,7 +367,7 @@ public interface Detection {
 
     public interface Conveyor {
         SmartNumber GANDALF_SHOOTER_SPEED = new SmartNumber("Conveyor/Gandalf Shooter Speed", 1.0);
-        double GANDALF_AMP_SPEED = 0.5;
+        double GANDALF_AMP_SPEED = 0.75;
 
         SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.0);
         SmartNumber RECALL_DEBOUNCE = new SmartNumber("Conveyor/Recall Delay", 1.0);
@@ -386,7 +388,7 @@ public interface Alignment {
         double PODIUM_SHOT_MAX_ANGLE = 80;
 
         SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5));
-        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(23.5));
+        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(21.5)); // was 23.5 at comp
 
         SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(22.0));
         SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0));

diff --git a/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java
@@ -31,7 +31,7 @@ protected ConveyorImpl() {
 
         gandalfEncoder = new FilteredRelativeEncoder(gandalfMotor);
 
-        gandalfEncoder.setVelocityConversionFactor(0.5);
+        gandalfEncoder.setVelocityConversionFactor(1.0 / 2.0);
 
         Motors.disableStatusFrames(gandalfMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY);
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.numbers.N3;

diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java
@@ -22,6 +22,7 @@
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.util.FilteredRelativeEncoder;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -119,6 +120,8 @@ public void periodic() {
         SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM());
         SmartDashboard.putNumber("Shooter/Feeder RPM", getFeederRPM());
 
+        SmartDashboard.putNumber("Shooter/Feeder Linear Velocity", getFeederRPM() * Units.inchesToMeters(1.0) * Math.PI);
+
         SmartDashboard.putNumber("Shooter/Right Error", rightController.getError());
         SmartDashboard.putNumber("Shooter/Left Error", leftController.getError());
         SmartDashboard.putNumber("Shooter/Feeder Error", feederController.getError());