Mar 28 (#96)

* sysid feeder * use correct sysid mech * Add low pass filter onto distance in ToShoot * Manually tune feeder * Log requested voltage (to check with shooter current drops) * Tune shootalign + add pdh currents * Small change to SwerveDriveToShoot --------- Co-authored-by: Ivan Chen <[email protected]> Co-authored-by: Prog694 <[email protected]>
StuyPulse · Mar 30, 2024 · 4dcfc23 · 4dcfc23
1 parent ca90914
commit 4dcfc23
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 14 deletions.
diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
@@ -71,10 +71,14 @@ public void robotInit() {
     public void robotPeriodic() {
         scheduler.run();
 
-        SmartDashboard.putNumber("Total Power (W)", robot.pdp.getTotalPower());
-        SmartDashboard.putNumber("Total Current (A)", robot.pdp.getTotalCurrent());
-        SmartDashboard.putNumber("Battery Voltage (V)", robot.pdp.getVoltage());
-        SmartDashboard.putNumber("Calculated Resistance (R)", robot.pdp.getVoltage() / robot.pdp.getTotalCurrent());
+        SmartDashboard.putNumber("PDP/Total Power (watts)", robot.pdp.getTotalPower());
+        SmartDashboard.putNumber("PDP/Total Current (amps)", robot.pdp.getTotalCurrent());
+        SmartDashboard.putNumber("PDP/Battery Voltage (volts)", robot.pdp.getVoltage());
+        SmartDashboard.putNumber("PDP/Calculated Resistance (ohms)", robot.pdp.getVoltage() / robot.pdp.getTotalCurrent());
+
+        for (int i = 0; i < robot.pdp.getNumChannels(); i++) {
+            SmartDashboard.putNumber("PDP/Currents/" + i, robot.pdp.getCurrent(i));
+        }
     }
 
     public static boolean isBlue() {

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -43,7 +43,9 @@
 import com.stuypulse.robot.subsystems.vision.NoteVision;
 import com.stuypulse.robot.util.PathUtil.AutonConfig;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -86,6 +88,8 @@ public RobotContainer() {
 
         LiveWindow.disableAllTelemetry();
 
+        if (Robot.isReal()) CameraServer.startAutomaticCapture().setVideoMode(PixelFormat.kMJPEG, 80, 60, 30);
+
         SmartDashboard.putData("Gamepads/Driver", driver);
         SmartDashboard.putData("Gamepads/Operator", operator);
     }

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java
@@ -43,6 +43,7 @@ public static SwerveDriveToShoot withHigherDebounce() {
     private final AnglePIDController angleController;
     private final IStream velocityError;
 
+    private final IStream distanceToSpeaker;
     private final BStream isAligned;
 
     private final Number targetDistance;
@@ -74,6 +75,9 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) {
             .filtered(new LowPassFilter(0.05))
             .filtered(x -> Math.abs(x));
 
+        distanceToSpeaker = IStream.create(() -> getTranslationToSpeaker().getNorm())
+            .filtered(new LowPassFilter(0.05));
+
         isAligned = BStream.create(this::isAligned)
             .filtered(new BDebounceRC.Rising(debounce));
 
@@ -110,19 +114,22 @@ private boolean isAligned() {
             && velocityError.get() < velocityTolerance;
     }
 
+    private Translation2d getTranslationToSpeaker() {
+        return Field.getAllianceSpeakerPose().getTranslation().minus(odometry.getPose().getTranslation());
+    }
+
     @Override
     public void execute() {
-        Translation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation()
-            .minus(odometry.getPose().getTranslation());
+        Rotation2d toSpeaker = getTranslationToSpeaker().getAngle();
 
-        double speed = -distanceController.update(getTargetDistance(), toSpeaker.getNorm());
+        double speed = -distanceController.update(getTargetDistance(), distanceToSpeaker.get());
         double rotation = angleController.update(
-            Angle.fromRotation2d(toSpeaker.getAngle()).add(Angle.k180deg),
+            Angle.fromRotation2d(toSpeaker).add(Angle.k180deg),
             Angle.fromRotation2d(odometry.getPose().getRotation()));
 
         Translation2d speeds = new Translation2d(
             speed,
-            toSpeaker.getAngle());
+            toSpeaker);
 
         if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get())
             rotation = 0;
@@ -134,7 +141,7 @@ public void execute() {
                 rotation));
 
         SmartDashboard.putNumber("Alignment/Velocity Error", velocityError.get());
-        SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.getAngle().plus(Rotation2d.fromDegrees(180)).getDegrees());
+        SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees());
     }
 
     @Override

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -349,11 +349,13 @@ public interface PID {
 
     public interface Feeder {
         double GEARING = 18.0 / 30.0;
+        double POSITION_CONVERSION = GEARING;
+        double VELOCITY_CONVERSION = POSITION_CONVERSION / 60;
 
         public interface Feedforward {
             double kS = 0.71611;
             double kV = 0.0032;
-            double kA = 0.00040287;
+            double kA = 0.076981;
         }
 
         public interface PID {
@@ -420,9 +422,9 @@ public interface Rotation {
 
         public interface Shoot {
             public interface Translation {
-                SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 6.0);
+                SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 4.0);
                 SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0);
-                SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.02);
+                SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.05);
             }
 
             public interface Rotation {

diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java
@@ -68,6 +68,9 @@ protected ShooterImpl() {
 
         rpmChange = IStream.create(this::getAverageShooterRPM)
             .filtered(new HighPassFilter(Settings.Shooter.RPM_CHANGE_RC));
+
+        feederEncoder.setPositionConversionFactor(Feeder.POSITION_CONVERSION);
+        feederEncoder.setPositionConversionFactor(Feeder.VELOCITY_CONVERSION);
 
         Motors.disableStatusFrames(leftMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY);
         Motors.disableStatusFrames(rightMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY);
@@ -95,7 +98,9 @@ public double getFeederRPM() {
 
     @Override
     public boolean noteShot() {
-        return getLeftTargetRPM() > 0 && getRightTargetRPM() > 0 && rpmChange.get() < -Settings.Shooter.RPM_CHANGE_DIP_THRESHOLD;
+        return getLeftTargetRPM() > 0 && 
+               getRightTargetRPM() > 0 && 
+               rpmChange.get() < -Settings.Shooter.RPM_CHANGE_DIP_THRESHOLD;
     }
 
     @Override
@@ -110,10 +115,18 @@ public void periodic() {
             leftMotor.stopMotor();
             rightMotor.stopMotor();
             feederMotor.stopMotor();
+
+            SmartDashboard.putNumber("Shooter/Left Requested Voltage", 0);
+            SmartDashboard.putNumber("Shooter/Right Requested Voltage", 0);
+            SmartDashboard.putNumber("Shooter/Feeder Requested Voltage", 0);
         } else {
             leftMotor.setVoltage(leftController.getOutput());
             rightMotor.setVoltage(rightController.getOutput());
             feederMotor.setVoltage(feederController.getOutput());
+
+            SmartDashboard.putNumber("Shooter/Left Requested Voltage", leftController.getOutput());
+            SmartDashboard.putNumber("Shooter/Right Requested Voltage", rightController.getOutput());
+            SmartDashboard.putNumber("Shooter/Feeder Requested Voltage", feederController.getOutput());
         }
 
         SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM());