diff --git a/src/main/java/com/spartronics4915/frc2023/Constants.java b/src/main/java/com/spartronics4915/frc2023/Constants.java index 696071b..0b7cc7d 100644 --- a/src/main/java/com/spartronics4915/frc2023/Constants.java +++ b/src/main/java/com/spartronics4915/frc2023/Constants.java @@ -365,7 +365,7 @@ public ArmMotorConstants(int MotorID, double PositionConversionFactor, boolean I public static final ArmMotorConstants kPivotMotorConstants = new ArmMotorConstants( 15, //actual value 15 Math.PI * 2, false, - 0.25, 0, 0, 0.04, + 0.30, 0, 0, 0.05,// 0.25, ..., 0.04 Math.PI/8, 1, 0, Rotation2d.fromDegrees(66), 10, false, MotorType.kBrushless @@ -374,7 +374,7 @@ public ArmMotorConstants(int MotorID, double PositionConversionFactor, boolean I public static final ArmMotorConstants kWristMotorConstants = new ArmMotorConstants( 19, Math.PI * 2, true, - 0.3, 0, 0, 0.03, + 0.4, 0, 0, 0.04,// 0.3, ..., 0.03 1, 1, 0, //maybe try lowering max velocity, maybe add limiter variables for smart motion Rotation2d.fromDegrees(136),-1, true, MotorType.kBrushed diff --git a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java index 5d9e867..8b2220b 100644 --- a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java @@ -161,6 +161,15 @@ mArmCommands.new ReleasePiece(ArmState.FLOOR_POS), new ChargeStationCommands.AutoChargeStationClimb() ) ), + mAutos.new Strategy( + "High cube, Balance", + (Pose2d initialPose) -> new SequentialCommandGroup( + mArmCommands.new ReleasePiece(ArmState.SHOOT_HIGH_CUBE), + mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE), + new WaitCommand(1), + new ChargeStationCommands.AutoChargeStationClimb() + ) + ), // mAutos.new Strategy( // "Drop, Leave, Pick-up", // (Pose2d initialPose) -> new SequentialCommandGroup( @@ -175,7 +184,7 @@ mArmCommands.new ReleasePiece(ArmState.FLOOR_POS), // ) // ), mAutos.new Strategy( - "Drop, Leave, Pick-up", + "Drop, Leave, Pick-up (do not use)", (Pose2d initialPose) -> new SequentialCommandGroup( mArmCommands.new ReleasePiece(ArmState.FLOOR_POS), mSwerveTrajectoryFollowerCommands.new FollowStaticTrajectory( @@ -206,6 +215,12 @@ mAutos.new Strategy( mArmCommands.new ReleasePiece(ArmState.FLOOR_POS) ) ), + mAutos.new Strategy( + "high cube, do nothing", + (Pose2d initialPose) -> new SequentialCommandGroup( + mArmCommands.new ReleasePiece(ArmState.SHOOT_HIGH_CUBE) + ) + ), mAutos.new Strategy( "place, leave community", (Pose2d initialPose) -> new SequentialCommandGroup( @@ -217,6 +232,17 @@ mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE), mSwerve.driveCommand(new ChassisSpeeds(), false, true) ) ), + mAutos.new Strategy( + "cube high, leave community", + (Pose2d initialPose) -> new SequentialCommandGroup( + mArmCommands.new ReleasePiece(ArmState.SHOOT_HIGH_CUBE), + mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE), + new WaitCommand(1), + mSwerve.driveCommand(new ChassisSpeeds(-2, 0, 0), false, true), + new WaitCommand(2.5), + mSwerve.driveCommand(new ChassisSpeeds(), false, true) + ) + ), mAutos.new Strategy( "cube high (test)", (Pose2d initialPose) -> new SequentialCommandGroup( diff --git a/src/main/java/com/spartronics4915/frc2023/commands/ChargeStationCommands.java b/src/main/java/com/spartronics4915/frc2023/commands/ChargeStationCommands.java index 0cfa0f9..23621e3 100644 --- a/src/main/java/com/spartronics4915/frc2023/commands/ChargeStationCommands.java +++ b/src/main/java/com/spartronics4915/frc2023/commands/ChargeStationCommands.java @@ -101,8 +101,8 @@ public void execute() { } case GRIP_TO_PLATFORM: { - final double grip_to_platform_speed_m_s = -2. * 0.175; - final double grip_to_platform_target_roll_deg = 8; + final double grip_to_platform_speed_m_s = -2. * 0.1875; + final double grip_to_platform_target_roll_deg = 9; final double grip_to_platform_time_allowed = 5; if (mCurrStateTimer.hasElapsed(grip_to_platform_time_allowed)) { mSwerve.stop(); diff --git a/src/main/java/com/spartronics4915/frc2023/subsystems/MotorAbsEncoderComboSubsystem.java b/src/main/java/com/spartronics4915/frc2023/subsystems/MotorAbsEncoderComboSubsystem.java index da148d2..4b77046 100644 --- a/src/main/java/com/spartronics4915/frc2023/subsystems/MotorAbsEncoderComboSubsystem.java +++ b/src/main/java/com/spartronics4915/frc2023/subsystems/MotorAbsEncoderComboSubsystem.java @@ -105,7 +105,7 @@ public void setArmReference(Rotation2d ref) { // This is designed to ignore unsafe arm positions. if (Math.abs(ref.getDegrees()) > 100) { - System.out.println("Unsafe arm position requested: " + ref); + System.out.println("Unsafe arm position requested (setArmReference): " + ref); return; } mModeledPosition = getRawPosition(); @@ -134,8 +134,12 @@ public Rotation2d nativeToArm(Rotation2d nativeRotation) { private void setNativeReference(Rotation2d ref) { - System.out.println("SetNativeReferenceCalled"); + if (Math.abs(mCurrentReference.minus(ref).getDegrees()) > 120) { + System.out.println("setNativeReference illegal request:" + ref); + return; + } mCurrentReference = ref; + mReferenceRadians = ref.getRadians(); mReferenceSet = true; }