pov building the stage takes so long u tune autos from match vid also…

… some untested feed stuff
MillenniumFalcons · Oct 25, 2024 · 0ea3505 · 0ea3505
1 parent 3520e0c
commit 0ea3505
Show file tree

Hide file tree

Showing 9 changed files with 1,586 additions and 1,512 deletions.
diff --git a/choreo.chor b/choreo.chor
diff --git a/src/main/deploy/choreo/shoot3 to f2.1.traj b/src/main/deploy/choreo/shoot3 to f2.1.traj
diff --git a/src/main/deploy/choreo/shoot3 to f2.traj b/src/main/deploy/choreo/shoot3 to f2.traj
diff --git a/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
@@ -76,10 +76,14 @@ public Command driveVisionTeleop(
                         var rotation = motionTurnComponent;
                         swerve.drive(translation.getX(), translation.getY(), rotation);
                     } else {
+
                         if (mode != DriveMode.NONE && enabeld) {
                             motionTurnComponent = autoDriveTwist2d.dtheta;
                         }
-                        SmartDashboard.putNumber("theta", autoDriveTwist2d.dtheta);
+                        if (mode == DriveMode.FEED && enabeld){
+                            motionTurnComponent = autoDriveTwist2d.dtheta + motionTurnComponent;
+                        }
+                        // SmartDashboard.putNumber("theta", autoDriveTwist2d.dtheta);
 
                         var translation = new Translation2d(motionXComponent, motionYComponent);
 

diff --git a/src/main/java/team3647/frc2024/commands/ShooterCommands.java b/src/main/java/team3647/frc2024/commands/ShooterCommands.java
@@ -100,8 +100,8 @@ public Command characterize() {
                     var b = linReg.intercept();
                     var kS = -b / a;
                     var kV = a;
-                    SmartDashboard.putNumber("shooter kS", kS);
-                    SmartDashboard.putNumber("shooter kV", kV);
+                    // SmartDashboard.putNumber("shooter kS", kS);
+                    // SmartDashboard.putNumber("shooter kV", kV);
                 },
                 shooterLeft);
     }

diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java
@@ -347,8 +347,8 @@ public void configureSmartDashboardLogging() {
         // printer.addBoolean("pivot ready", superstructure::pivotReady);
         // printer.addBoolean("swerve ready", superstructure::swerveReady);
         // printer.addDouble("tx", detector::getTX);
-        SmartDashboard.putNumber("left", 28);
-        SmartDashboard.putNumber("right", 18);
+        // SmartDashboard.putNumber("left", 28);
+        // SmartDashboard.putNumber("right", 18);
         // printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta);
     }
 

diff --git a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java
@@ -228,6 +228,7 @@ public void periodic() {
         Logger.recordOutput(
                 "Drive/Current",
                 this.Modules[0].getDriveMotor().getStatorCurrent().getValueAsDouble());
+        Logger.recordOutput("Robot/Under stage", underStage());
         readPeriodicInputs();
         writePeriodicOutputs();
     }

diff --git a/src/main/java/team3647/frc2024/util/AutoDrive.java b/src/main/java/team3647/frc2024/util/AutoDrive.java
@@ -16,6 +16,8 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -148,7 +150,7 @@ public boolean swerveAimed() {
     }
 
     public double flywheelThreshold() {
-        return getShootSpeedLeft() - (1 + MathUtil.clamp(8 - 2 * targeting.distance(), 0, 5));
+        return getShootSpeedLeft() - (1 + MathUtil.clamp(RobotController.getBatteryVoltage()*0.75 - 2 * targeting.distance(), 0, 5));
     }
 
     private void setTargetPose(Pose2d targetPose) {

diff --git a/src/main/java/team3647/frc2024/util/VisionController.java b/src/main/java/team3647/frc2024/util/VisionController.java
@@ -2,6 +2,8 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.function.Consumer;
@@ -65,11 +67,13 @@ public void periodic() {
             botPoseAcceptor.accept(list.get(0));
 
             Logger.recordOutput("Robot/Vision", list.get(0).pose);
+
             Logger.recordOutput("Robot/Camera", list.get(0).name);
             Logger.recordOutput("stddev", list.get(0).stdDevs.get(0, 0));
         } else {
             Logger.recordOutput("Robot/Vision", new Pose2d(1, 1, new Rotation2d()));
             Logger.recordOutput("stddev", 0.0);
         }
+        Logger.recordOutput("Robot/Has Pose", !new Trigger(() -> list.isEmpty()).debounce(1).getAsBoolean());
     }
 }