pre loadin push

Vortx3735 · Apr 3, 2024 · 2269a64 · 2269a64
1 parent 23f0152
commit 2269a64
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 31 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -55,8 +55,8 @@ public void robotInit() {
 
     RobotContainer.drivebase.swerveDrive.setMotorIdleMode(true);
 
-    CameraServer.startAutomaticCapture("intake", 0);
-    CameraServer.startAutomaticCapture("back", 1);
+    // CameraServer.startAutomaticCapture("intake", 0);
+    // CameraServer.startAutomaticCapture("back", 1);
 
     enableLiveWindowInTest(true); //enabling test mode
     SmartDashboard.putData(CommandScheduler.getInstance());
@@ -156,12 +156,12 @@ public void teleopInit() {
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {
-    if(RobotContainer.con2.rb.getAsBoolean()) {
-      RobotContainer.led.blinkColor(Color.kGreen);
+    if(RobotContainer.con2.rt.getAsBoolean() || RobotContainer.con2.rb.getAsBoolean()) {
+      RobotContainer.led.blinkColor(Color.kRed);
     } else if (RobotContainer.con1.rt.getAsBoolean() || RobotContainer.con1.lt.getAsBoolean()) {
       RobotContainer.led.blinkColor(Color.kBlue);
-    } else if (RobotContainer.con2.lt.getAsBoolean() || RobotContainer.con2.rt.getAsBoolean()) {
-      RobotContainer.led.blinkColor(Color.kRed);
+    } else if (RobotContainer.con2.lt.getAsBoolean()) {
+      RobotContainer.led.blinkColor(Color.kGreen);
     } else {
       RobotContainer.led.noteCheck();
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -221,9 +221,9 @@ private void configureBindings() {
           )
       );      
 
-    con2.aButton.whileTrue(
-      autoaim.aimSpeaker()
-    );
+    // con2.aButton.whileTrue(
+    //   autoaim.aimSpeaker()
+    // );
 
 
     RunCommand moveArmToAmp = new RunCommand(
@@ -261,14 +261,14 @@ private void configureBindings() {
         )
     );
 
-    RunCommand moveArmToShot = new RunCommand(
-      () -> arm.moveToSetpoint(Constants.ArmConstants.armTravelPos, 4), 
-      arm
-      );
+    // RunCommand moveArmToShot = new RunCommand(
+    //   () -> arm.moveToSetpoint(Constants.ArmConstants.armTravelPos, 4), 
+    //   arm
+    //   );
 
-    con2.aButton.whileTrue(
-      moveArmToShot
-    );
+    // con2.aButton.whileTrue(
+    //   moveArmToShot
+    // );
 
     con1.lb.toggleOnTrue(
       new InstantCommand(

diff --git a/src/main/java/frc/robot/commands/AutoAim.java b/src/main/java/frc/robot/commands/AutoAim.java
@@ -44,8 +44,8 @@ public AutoAim(Arm a, Shooter s) {
   }
 
   public Command aimSpeaker() {
-    if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) {
-      double kP = 0.1;
+    // if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) {
+      double kP = 0.0001;
       double xDistLLToShooter = ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) 
                                 - ShooterConstants.shooterLength*Math.cos(RobotContainer.arm.getRadiansTravelled() 
                                                                             + ShooterConstants.differenceFromArm)
@@ -75,17 +75,16 @@ public Command aimSpeaker() {
           () -> RobotContainer.shooter.setShooterRPM(rpmNeeded),
           RobotContainer.shooter)
       );
-    } else {
-      System.out.println(((DriverStation.getAlliance().get() == DriverStation.Alliance.Red) ? "Red" : "Blue") + " Alliance Mid Speaker AprilTag Not Found!");
-      return new RunCommand(
-        () -> RobotContainer.arm.getDefaultCommand(),
-        RobotContainer.arm
-      ).alongWith(
-        new RunCommand(
-          () -> RobotContainer.shooter.getDefaultCommand(),
-          RobotContainer.shooter)
-      );
-    }
+    // } else {
+    //   return new RunCommand(
+    //     () -> RobotContainer.arm.hold(),
+    //     RobotContainer.arm
+    //   ).alongWith(
+    //     new RunCommand(
+    //       () -> RobotContainer.shooter.coast(),
+    //       RobotContainer.shooter)
+    //   );
+    // }
   }
 
   public Command aimSpeakerAuto() {

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -143,7 +143,7 @@ public double getArmPos() {
   }
 
   public double getRadiansTravelled() {
-    return armEncoder.getDistance()/(2*Math.PI);
+    return armEncoder.getDistance()/(Constants.ArmConstants.encoderPitchDiameter/2);
   }
 
   public BooleanSupplier getArmDown() {
@@ -189,6 +189,7 @@ public void periodic() {
       position = raw_position - offset;
     }
     SmartDashboard.putNumber("arm//ArmEncoder", position);
+    SmartDashboard.putNumber("arm//distance travelled", getRadiansTravelled());
     //SmartDashboard.putNumber("arm//ArmEncoder without math", armEncoder.getAbsolutePosition());
   }
 

diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -315,7 +315,7 @@ public static double trackApriltagDrive(double turning, double speed) {
       // if it is too high, the robot will oscillate.
       // this is a hand-tuned number that determines the aggressiveness of our proportional control loop
       // kP (constant of proportionality)
-    double kP = .001;
+      double kP = .001;
 
       // tx ranges from (-hfov/2) to (hfov/2) in degrees. If your target is on the rightmost edge of 
       // your limelight 3 feed, tx should return roughly 31 degrees.