add cube high shooting preset and autos using it, change arm and wrist

constants, fix charge station auto, add safety to arm and wrist
Spartronics4915 · Mar 6, 2023 · 35be8f6 · 35be8f6
1 parent 383f1c3
commit 35be8f6
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
     }