DCMP Final Changes

FRC1360 · Apr 17, 2023 · fe52fd0 · fe52fd0
1 parent 8a33c2d
commit fe52fd0
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -65,7 +65,7 @@ public static final class Drivetrain {
     public static final int SHOULDER_MOTOR_SLAVE = 51;
     public static final int SHOULDER_ENCODER = 0;
     //public static final double SHOULDER_ENCODER_OFFSET = 0.542;
-    public static final double SHOULDER_ENCODER_OFFSET = 0.139;
+    public static final double SHOULDER_ENCODER_OFFSET = 0.095;
     public static final double SHOULDER_GEAR_RATIO = (11.0 / 52.0) * (30.0 / 68.0) * (12.0 / 60.0);
     public static final double SHOULDER_MANUAL_OVERRIDE_RANGE = 20.0;
     public static final double MAX_SHOULDER_ANGLE = 90.0; 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -157,8 +157,9 @@ private void configureButtonBindings() {
     new Trigger(() -> operatorController.getRightTriggerAxis() > 0.05)
      .whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> operatorController.getRightTriggerAxis())); 
 
-    left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY));
+    //left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY));
     right_controller.button(1).whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> 1.0)); 
+    left_controller.button(3).whileTrue(new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = true)).whileFalse( new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = false));
   }
 
 

diff --git a/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java b/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java
@@ -30,7 +30,7 @@ public ConeHighAndDriveAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder,
                                         StateMachine sm) { 
 
         addCommands(new ConeScoreHighAuto(dt, shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger, ledSubsystem, sm), 
-                    new Drive(dt, 8.0, 0.0)
+                    new Drive(dt, 8.2, 0.0)
                         .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0))
                         .raceWith(new WristHoldCommand(wrist, () -> 0.0))
                         .raceWith(new ArmHoldCommand(arm))

diff --git a/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java b/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java
@@ -34,7 +34,7 @@ public void end(boolean interruptible) {
 
     @Override
     public boolean isFinished() { 
-        return this.timer.getTimeDeltaSec() > 1.0; 
+        return this.timer.getTimeDeltaSec() > 0.3; 
     }
 
 

diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
@@ -85,6 +85,8 @@ public class DrivetrainSubsystem extends SubsystemBase {
 
   private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
 
+  public boolean lockWheels;
+
   //private SwerveDriveOdometry2022 odometry = new SwerveDriveOdometry2022(m_kinematics, getGyroscopeRotation(), new Pose2d(2.3, 4.5, Rotation2d.fromDegrees(0)));
   private SwerveDriveOdometry2022 odometry = new SwerveDriveOdometry2022(m_kinematics, getGyroscopeRotation(), new Pose2d(2.3, 1.5, Rotation2d.fromDegrees(0)));
 
@@ -93,6 +95,8 @@ public class DrivetrainSubsystem extends SubsystemBase {
   public OrbitPID driveRotPID; 
 
   public DrivetrainSubsystem() {
+    lockWheels = false;
+
     ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain");
 
         m_frontLeftModule = Mk4iSwerveModuleHelper.createNeo(
@@ -203,6 +207,22 @@ public OrbitPID getDrivePID() {
   @Override
   public void periodic() {
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(m_chassisSpeeds);
+
+    //SwerveModuleState[] xstates = new SwerveModuleState[4];
+    //xstates[0] = new SwerveModuleState
+
+    if(lockWheels) {
+        for(int i = 0; i < 4; i++) {
+                states[i].speedMetersPerSecond = 0.0;
+        }
+
+        states[0].angle = Rotation2d.fromDegrees(45.0);
+        states[1].angle = Rotation2d.fromDegrees(-45.0);
+        states[2].angle = Rotation2d.fromDegrees(-45.0);
+        states[3].angle = Rotation2d.fromDegrees(45.0);
+    }
+
+
     SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VELOCITY_METERS_PER_SECOND); // this was normalizeWheelSpeeds but WPILib doesn't like that
 
     m_frontLeftModule.set(states[0].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[0].angle.getRadians());