Changes part 1

team422 · Oct 24, 2023 · 07cfdd9 · 07cfdd9
1 parent ca46580
commit 07cfdd9
Show file tree

Hide file tree

Showing 11 changed files with 69 additions and 39 deletions.
diff --git a/src/main/deploy/pathplanner/move and shoot.path b/src/main/deploy/pathplanner/move and shoot.path
@@ -21,8 +21,8 @@
           "intakeOut"
         ],
         "executionBehavior": "parallel",
-        "waitBehavior": "minimum",
-        "waitTime": 0
+        "waitBehavior": "none",
+        "waitTime": 0.2
       }
     },
     {
@@ -52,16 +52,16 @@
     },
     {
       "anchorPoint": {
-        "x": 6.5684837845370145,
-        "y": 4.596241939011656
+        "x": 6.230496735802632,
+        "y": 4.620055957669168
       },
       "prevControl": {
-        "x": 7.055205753640675,
-        "y": 4.606215868681737
+        "x": 6.717218704906292,
+        "y": 4.630029887339249
       },
       "nextControl": {
-        "x": 6.081761815433354,
-        "y": 4.586268009341575
+        "x": 5.743774766698971,
+        "y": 4.610082027999087
       },
       "holonomicAngle": 0.5318504287843849,
       "isReversal": false,
@@ -77,24 +77,26 @@
     },
     {
       "anchorPoint": {
-        "x": 2.0436911806728024,
-        "y": 4.107064190690882
+        "x": 2.2427663175937043,
+        "y": 4.44667637426878
       },
       "prevControl": {
-        "x": 2.5169141070512535,
-        "y": 4.550990290706072
+        "x": 2.715989243972156,
+        "y": 4.890602474283969
       },
       "nextControl": {
-        "x": 1.5704682542943513,
-        "y": 3.663138090675692
+        "x": 1.7695433912152532,
+        "y": 4.002750274253589
       },
       "holonomicAngle": -179.6980380009168,
       "isReversal": false,
       "velOverride": null,
       "isLocked": false,
-      "isStopPoint": false,
+      "isStopPoint": true,
       "stopEvent": {
-        "names": [],
+        "names": [
+          "intakeOut"
+        ],
         "executionBehavior": "parallel",
         "waitBehavior": "none",
         "waitTime": 0
@@ -130,14 +132,14 @@
   "isReversed": null,
   "markers": [
     {
-      "position": 1.4600142045454554,
+      "position": 0.2666666666666394,
       "names": [
         "setpointCubeGround",
         "intakeIn"
       ]
     },
     {
-      "position": 2.1311079545454543,
+      "position": 2.2545454545454544,
       "names": [
         "cubeShoot"
       ]

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -77,7 +77,7 @@ public static final class ModuleConstants {
         public static final TunableNumber kDriveD = new TunableNumber("Drive D", 0.00);
         public static final TunableNumber kDriveFF = new TunableNumber("Drive FF", 2.96);
 
-        public static final TunableNumber kTurningP = new TunableNumber("TurnP", 0.15);
+        public static final TunableNumber kTurningP = new TunableNumber("TurnP", 0.10);
         public static final TunableNumber kTurningI = new TunableNumber("Turn I", 0.00);
         public static final TunableNumber kTurningD = new TunableNumber("Turn D", 0.005);
 
@@ -87,14 +87,14 @@ public final static class WristConstants {
         // TODO: copied from frc-23, change later
         public static final double kGearRatio = 3;
         public static final int kWristEncoderCPR = 8192;
-        public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(95);
-        public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(-100);
+        public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(80);
+        public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(-89);
         public static final double kToleranceRad = Units.degreesToRadians(2);
-        public static final double kOffset = Units.degreesToRadians(28.1); //-173.51 // 111
+        public static final double kOffset = 67; //-173.51 // 111 // 28.1
         public static final double kManualMoveVolts = 2.0;
 
         // Wrist PID, currently untuned
-        public static final TunableNumber kP = new TunableNumber("Wrist P", .9);
+        public static final TunableNumber kP = new TunableNumber("Wrist P", 5);
         public static final TunableNumber kI = new TunableNumber("Wrist I", 0.7);
         public static final TunableNumber kD = new TunableNumber("Wrist D", 0.005);
         public static final TunableNumber kWristVelo = new TunableNumber("Wrist Velo", 20);
@@ -105,7 +105,7 @@ public final static class WristConstants {
 
         // Wrist Feedforward, currently untuned
         public static final TunableNumber kWristks = new TunableNumber("Wrist ks", 0.05);
-        public static final TunableNumber kWristkg = new TunableNumber("Wrist kg", -.2);
+        public static final TunableNumber kWristkg = new TunableNumber("Wrist kg", -.4);
         public static final TunableNumber kWristkv = new TunableNumber("Wrist kv", 0.0);
         public static final ArmFeedforward wristFeedforward = new ArmFeedforward(
             kWristkg.get(), kWristkv.get(), kWristks.get());
@@ -156,7 +156,7 @@ public static final class Ports {
 
     public static final class Setpoints {
         // TODO: i made these up, change later
-        public static final Rotation2d kWristGrabCube = Rotation2d.fromDegrees(-55);
+        public static final Rotation2d kWristGrabCube = Rotation2d.fromDegrees(-50);
         public static final Rotation2d kWristShoot = Rotation2d.fromDegrees(70);
         public static final Rotation2d kWristStow = Rotation2d.fromDegrees(90);
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -166,20 +166,19 @@ private void configureButtonBindings() {
         driverControls.intakeButton().whileTrue(m_intake.intakeCommand());
         driverControls.outtakeButton().whileTrue(m_intake.outtakeCommand());
 
-        // if (Robot.isSimulation()) {
-            // all of these are unbound in real
-            // DO NOT REBIND THESE UNTIL WE HAVE A MIN ANGLE AND MAX ANGLE AND OFFSET IS SET
-            driverControls.wristButtonIntake().onTrue(m_wrist.setAngleCommand(Setpoints.kWristGrabCube));
-            driverControls.wristButtonShoot().onTrue(m_wrist.setAngleCommand(Setpoints.kWristShoot));
-            driverControls.wristButtonStow().onTrue(m_wrist.setAngleCommand(Setpoints.kWristStow));
-        // }
+        driverControls.wristButtonIntake().onTrue(m_wrist.setAngleCommand(Setpoints.kWristGrabCube));
+        driverControls.wristButtonShoot().onTrue(m_wrist.setAngleCommand(Setpoints.kWristShoot));
+        driverControls.wristButtonStow().onTrue(m_wrist.setAngleCommand(Setpoints.kWristStow));
+
 
         driverControls.wristManualUp().whileTrue(m_wrist.manualUpCommand());
         driverControls.wristManualDown().whileTrue(m_wrist.manualDownCommand());
 
         operatorControls.wristManualUp().whileTrue(m_wrist.manualUpCommand());
         operatorControls.wristManualDown().whileTrue(m_wrist.manualDownCommand());
 
+        driverControls.resetWristEncoder().onTrue(m_wrist.resetEncoderCommand());
+
     }
 
     /**

diff --git a/src/main/java/frc/robot/commands/autonomous/AutoFactory.java b/src/main/java/frc/robot/commands/autonomous/AutoFactory.java
@@ -70,7 +70,7 @@ public AutoFactory(Drive drive, Wrist wrist, Intake intake) {
         Commands.print("cubeGround"));
     Command cubeShoot = Commands.parallel(
         m_wrist.setAngleCommand(Setpoints.kWristShoot),
-        Commands.print("cubeGround"));
+        Commands.print("cubeShoot"));
     Command stopIntake = m_intake.setVoltageCommand(0.0);
     // Command coneGround = Commands.parallel(
     //     m_elevator.setHeightCommand(Setpoints.pickUpConeGroundCommandSetpoints[0]),
@@ -130,8 +130,8 @@ public AutoFactory(Drive drive, Wrist wrist, Intake intake) {
         Map.entry("cubeShoot",cubeShoot),
         Map.entry("wait", Commands.waitSeconds(.25)),
         Map.entry("intakeStop", stopIntake),
-        Map.entry("intakeIn", m_intake.setVoltageCommand(3.0)),
-        Map.entry("intakeOut", m_intake.setVoltageCommand(-12.0))
+        Map.entry("intakeIn", m_intake.setVoltageCommand(-12.0)),
+        Map.entry("intakeOut", m_intake.setVoltageCommand(3.0))
         // Map.entry("balance", balanceStation),
         // Map.entry("zeroHeading", zeroHeading),
         // Map.entry("setpointConeHighWait", autoConeHigh), Map.entry("cubeBumpFar", cubeBumpFar),

diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java
@@ -22,4 +22,6 @@ public interface DriverControls {
     public Trigger wristManualUp();
 
     public Trigger wristManualDown();
+
+    public Trigger resetWristEncoder();
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsController.java b/src/main/java/frc/robot/oi/DriverControlsController.java
@@ -66,5 +66,10 @@ public Trigger wristManualDown() {
         // TODO Auto-generated method stub
         return new Trigger();
     }
+
+    @Override
+    public Trigger resetWristEncoder() {
+        return m_controller.povUp();
+    }
 
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java b/src/main/java/frc/robot/oi/DriverControlsDualFlightStick.java
@@ -61,4 +61,9 @@ public Trigger wristManualUp() {
     public Trigger wristManualDown() {
         return m_leftJoystick.button(9);
     }
+
+    @Override
+    public Trigger resetWristEncoder() {
+        return new Trigger();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java
@@ -134,4 +134,12 @@ public Command manualDownCommand() {
         }, () -> setVoltage(0));
     }
 
+    public void resetEncoder() {
+        m_io.resetEncoder();
+    }
+
+    public Command resetEncoderCommand() {
+        return runOnce(this::resetEncoder);
+    }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java
@@ -27,4 +27,6 @@ public static class WristInputs {
     public boolean getBrakeMode();
 
     public double getOutputVoltage();
+
+    public void resetEncoder();
 }
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOCANSparkMax.java b/src/main/java/frc/robot/subsystems/wrist/WristIOCANSparkMax.java
@@ -19,7 +19,6 @@ public class WristIOCANSparkMax implements WristIO {
     private boolean m_brakeModeEnabled;
 
     public WristIOCANSparkMax(int leaderPort, int followerPort, double encoderOffset) {
-        double encoderOffsetVal = 67;
         m_wristLeader = new CANSparkMax(leaderPort, CANSparkMax.MotorType.kBrushless);
         m_wristFollower = new CANSparkMax(followerPort, CANSparkMax.MotorType.kBrushless);
         m_wristFollower.follow(m_wristLeader);
@@ -29,11 +28,11 @@ public WristIOCANSparkMax(int leaderPort, int followerPort, double encoderOffset
         m_brakeModeEnabled = true;
         m_encoder.setPositionConversionFactor(120); 
         m_encoder.setInverted(false);
-        m_encoder.setZeroOffset(encoderOffsetVal);
+        m_encoder.setZeroOffset(encoderOffset);
         m_relativeEncoder = m_wristLeader.getEncoder();
         // m_relativeEncoder.setInverted(true);
-        m_relativeEncoder.setPositionConversionFactor((15.714));
-        syncRelativeAndAbsolute(Rotation2d.fromDegrees(m_encoder.getPosition()).getDegrees());
+        m_relativeEncoder.setPositionConversionFactor(15.714);
+        syncRelativeAndAbsolute(m_encoder.getPosition());
     }
 
     public void syncRelativeAndAbsolute(double val){
@@ -88,5 +87,10 @@ public double getSpeed() {
     public double getOutputVoltage() {
         return m_wristLeader.getAppliedOutput() * m_wristLeader.getBusVoltage();
     }
+
+    @Override
+    public void resetEncoder() {
+        m_encoder.setZeroOffset(getAngle().getRadians());
+    }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java
@@ -66,4 +66,7 @@ public boolean getBrakeMode() {
     public double getOutputVoltage() {
         return m_voltage;
     }
+
+    @Override
+    public void resetEncoder() {}
 }