Photon vision (#33)

* save energy by not applying voltage when arm wants to be at min angle * update open loop ramp rates * fix camera offsets * no accel limit for alignment * Added workaround for switching speaker and amp tag poses in Field.java depending on alliance (only works every time you deploy for some reason) * Revert "Added workaround for switching speaker and amp tag poses in Field.java depending on alliance (only works every time you deploy for some reason)"
StuyPulse · Aug 27, 2024 · 9754007 · 9754007
1 parent b29338a
commit 9754007
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 8 deletions.
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java
@@ -64,7 +64,7 @@ public SwerveDriveDriveAligned(Gamepad driver) {
                 // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST
                 // so that angular velocity doesn't oscillate
                 x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST),
-                new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL),
+                // new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL),
                 x -> SLMath.clamp(x, -Settings.Swerve.MAX_ANGULAR_VELOCITY, Settings.Swerve.MAX_ANGULAR_VELOCITY),
                 x -> -x
             );

diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java
@@ -27,7 +27,7 @@ public interface Limelight {
         new CameraConfig(
             "tower-cam",
             new Pose3d(
-                new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-3.333797), Units.inchesToMeters(23.929362)),
+                new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)),
                 new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180))
             ),
             "11",
@@ -37,7 +37,7 @@ public interface Limelight {
         new CameraConfig(
             "plate-cam",
             new Pose3d(
-                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(4.863591), Units.inchesToMeters(19.216471)),
+                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)),
                 new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0))
             ),
             "96",

diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java
@@ -39,8 +39,8 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) {
     /** Classes to store all of the values a motor needs */
 
     public interface Arm {
-        CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.0); 
-        CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.0); 
+        CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25); 
+        CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25); 
     }
 
     public interface Intake {
@@ -50,9 +50,9 @@ public interface Intake {
     }
 
     public interface Shooter {
-        CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 1.0);
-        CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 1.0);
-        CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 1.0);
+        CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5);
+        CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5);
+        CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25);
     }
 
     /* Configurations */

diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
@@ -178,6 +178,10 @@ public void periodic() {
             setVoltage(-1.5);
             controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get());
         }
+        else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTriggered.get()) {
+            setVoltage(0);
+            controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get());
+        }
         else {
             controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees());
             if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) {