recalibs?

MOERobotics · Mar 15, 2024 · dd56974 · dd56974
1 parent c535c81
commit dd56974
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/FortissiMOEContainer.java b/src/main/java/frc/robot/FortissiMOEContainer.java
@@ -127,7 +127,7 @@ public class FortissiMOEContainer{
     /////////////////////////////////////////////////////////////////////////////arm subsystem start
     private final Arm armSubsystem = new Arm(4, 15,14, 35, 36,
             2.0e-2, 2.0e-3, 4.0e-4, 8.0e-3, 0, 1.0e-4, 23.839, 14.231,
-            Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-32), 70,30);
+            Rotation2d.fromDegrees(88), Rotation2d.fromDegrees(-47), 70,30);
 
     /////////////////////////////////////////////////////////////////////////// arm subsystem end
 
@@ -307,12 +307,12 @@ private void configureBindings() {
                         || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9))));
         //podium shot
 
-        /*new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-38.5)), Set.of(armSubsystem))
+        new JoystickButton(functionJoystick, 4).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-47.5)), Set.of(armSubsystem))
                 .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) ||
                         functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) ||
                         functionJoystick.getRawButton(1)||buttonBox.getRawButton(1)|| buttonBox.getRawButton(2)
-                        || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9))));*/
-        //mid shot
+                        || functionJoystick.getRawButton(10) || functionJoystick.getRawButton(9))));
+        //all the shots smh until roshik chooses a new random button to be his favorite
 
         new JoystickButton(functionJoystick, 9).onTrue(Commands.defer(() ->armSubsystem.goToPoint(Rotation2d.fromDegrees(112), Rotation2d.fromDegrees(-31)), Set.of(armSubsystem))
                 .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) ||
@@ -344,18 +344,20 @@ private void configureBindings() {
 
 //        new JoystickButton(driverJoystick, 7).onTrue(turnToAmp.until(()->(Math.abs(driverJoystick.getRawAxis(2)) >= .2)));
 //        new JoystickButton(driverJoystick, 8).onTrue(turnToSource.until(()->(Math.abs(driverJoystick.getRawAxis(2)) >= .2)));
-        new JoystickButton(functionJoystick, 4).onTrue(
+        /*new JoystickButton(functionJoystick, 4).onTrue(
                 Commands.parallel(
                 Commands.defer(()->armSubsystem.goToPoint(
                 Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getX()),
-                Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem))
+                Rotation2d.fromDegrees(armSubsystem.autoAim(swerveSubsystem::getEstimatedPose).getY())), Set.of(armSubsystem)).andThen(
+                        Commands.run(()-> armSubsystem.holdPos(armSubsystem.shoulderPosRel(), armSubsystem.wristPosRel()))
+                        )
                         .until(()->(functionJoystick.getRawButton(7) || functionJoystick.getRawButtonPressed(3) ||
                         functionJoystick.getRawButtonPressed(2) || functionJoystick.getRawButton(8) ||
                         functionJoystick.getRawButton(1) || functionJoystick.getRawButton(3))),
                         Commands.run(()->swerveSubsystem.setDesiredYaw(swerveSubsystem.getAngleBetweenSpeaker(
                                 ()->swerveSubsystem.getEstimatedPose().getTranslation()).getDegrees())).until(
                                 ()->Math.abs(driverJoystick.getRawAxis(2)) >= .1
-                        ))); //auto aim shot
+                        ))); //auto aim shot*/
         //104,-41
       //  new JoystickButton(driverJoystick, 7).whileTrue(setHeading.until(()->Math.abs(driverJoystick.getRawAxis(2))>= .1));
     }

diff --git a/src/main/java/frc/robot/commands/ArmPathFollow.java b/src/main/java/frc/robot/commands/ArmPathFollow.java
@@ -64,7 +64,9 @@ public void execute() {
             s = desiredPoint.getDistance(startPoint)+1;
         }
         SmartDashboard.putNumber("ArmPathFollow writePos", wristPos);
-        SmartDashboard.putNumber("ArmPathFollow desiredWrite", desiredPoint.getY());
+        SmartDashboard.putNumber("ArmPathFollow shoulderPos", shoulderPos);
+        SmartDashboard.putNumber("ArmPathFollow desiredWrist", desiredPoint.getY());
+        SmartDashboard.putNumber("ArmPathFollow desiredShoulder", desiredPoint.getX());
         armSubsystem.pathFollow(Rotation2d.fromDegrees(shoulderPos), Rotation2d.fromDegrees(wristPos));
         armSubsystem.setWristDestState(wristPos);
         armSubsystem.setShoulderDesState(shoulderPos);

diff --git a/src/main/java/frc/robot/commands/ShooterControllerCommand.java b/src/main/java/frc/robot/commands/ShooterControllerCommand.java
@@ -47,7 +47,7 @@ public void execute() {
             onState %= 2;
         }
         if (onState%2 == 1){
-            shooterSpeedTop = 4000; shooterSpeedBottom = 4000;
+            shooterSpeedTop = 3500; shooterSpeedBottom = 3500;
             if (desShoulder.get() <= 85) {shooterSpeedTop = 3000; shooterSpeedBottom = 3000;}
             subsystem.setShooterSpeeds(shooterSpeedTop, shooterSpeedBottom);
         } else{

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -138,7 +138,7 @@ public Command goToPoint(Rotation2d shoulderPos, Rotation2d wristPos) {
 
     public Translation2d autoAim(Supplier<Pose2d> robotPos){
         double dist = AllianceFlip.apply(UsefulPoints.Points.middleOfSpeaker).getDistance(robotPos.get().getTranslation());
-        return new Translation2d(0.466*dist + 112, 5.56*dist - 69.1);
+        return new Translation2d(112, 3.8*dist-53.1);
     }