done

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index ad7ef146ef5d9855d78a5cf8236119c7e045ba64..0a82decbf1b9cefa6dca9d7834865281603ab7a0 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -38,7 +38,8 @@ public class IdConstants {
     public static final int SPINDEXER_ID = 12;
 
     // Intake
-    public static final int INTAKE_BASE_ID = 13;
-    public static final int INTAKE_FLYWHEEL_ID = 14;
+    public static final int INTAKE_BASE_LEFT_ID = 13;
+    public static final int INTAKE_BASE_RIGHT_ID = 14;
+    public static final int INTAKE_FLYWHEEL_ID = 15;
     public static final int INTAKE_ENCODER_ID = 2;
 }

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index d72340fe71d388f0651a5df0ecad4ce026928615..47363ddeecd4cc856db990c2ecc2aa90f75fbbf1 100644 (file)
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -31,7 +31,8 @@ import frc.robot.constants.IdConstants;
 
 public class Intake extends SubsystemBase implements IntakeIO{
     private TalonFX flyWheelMotor = new TalonFX(IdConstants.INTAKE_FLYWHEEL_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
-    private TalonFX baseMotor = new TalonFX(IdConstants.INTAKE_BASE_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private TalonFX baseMotorLeft = new TalonFX(IdConstants.INTAKE_BASE_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private TalonFX baseMotorRight = new TalonFX(IdConstants.INTAKE_BASE_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
 
     double basePower;
     double flyWheelPower;
@@ -59,7 +60,7 @@ public class Intake extends SubsystemBase implements IntakeIO{
 
     public Intake() {
         updateInputs();
-        encoderSim = baseMotor.getSimState();
+        encoderSim = baseMotorLeft.getSimState();
         
         intakeSim = new SingleJointedArmSim(
             baseIntakeMotorSim,
@@ -71,12 +72,14 @@ public class Intake extends SubsystemBase implements IntakeIO{
             Units.degreesToRadians(0),
             Units.degreesToRadians(360),
             true,
-            Units.degreesToRadians(IntakeConstants.START_ANGLE)
+            Units.degreesToRadians(IntakeConstants.STOW_ANGLE)
         );
 
         double absoluteAngleDegrees =  getAbsoluteEncoderAngle() - IntakeConstants.ABSOLUTE_OFFSET_ANGLE;
-        baseMotor.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO));
-        baseMotor.setNeutralMode(NeutralModeValue.Coast);
+        baseMotorLeft.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO));
+        baseMotorLeft.setNeutralMode(NeutralModeValue.Coast);
+        baseMotorRight.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO));
+        baseMotorRight.setNeutralMode(NeutralModeValue.Coast);
 
         TalonFXConfiguration config = new TalonFXConfiguration();
         config.Slot0.kS = 0.1; // Static friction compensation (should be >0 if friction exists)
@@ -93,14 +96,19 @@ public class Intake extends SubsystemBase implements IntakeIO{
 
         config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
         
-        baseMotor.getConfigurator().apply(config);
+        baseMotorLeft.getConfigurator().apply(config);
+
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+        baseMotorRight.getConfigurator().apply(config);
 
          CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
 
         limitConfig.StatorCurrentLimit = 30; // 120
         limitConfig.StatorCurrentLimitEnable = true;
 
-        baseMotor.getConfigurator().apply(limitConfig);
+        baseMotorLeft.getConfigurator().apply(limitConfig);
+        baseMotorRight.getConfigurator().apply(limitConfig);
 
         flyWheelMotor.getConfigurator().apply(
             new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
@@ -115,7 +123,8 @@ public class Intake extends SubsystemBase implements IntakeIO{
 
     public void setSetpoint(double setpoint) {
         double clampedSetpoint = MathUtil.clamp(setpoint, 90.0, 180.0);
-        baseMotor.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0)));
+        baseMotorLeft.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0)));
+        baseMotorRight.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0)));
     }
 
     @AutoLogOutput
@@ -133,7 +142,7 @@ public class Intake extends SubsystemBase implements IntakeIO{
         // if(RobotBase.isSimulation()){
         //     return position;
         // }
-        return inputs.measuredAngle;
+        return inputs.measuredAngleLeft;
     }
 
     public double getFlyWheelVelocity() {
@@ -154,12 +163,12 @@ public class Intake extends SubsystemBase implements IntakeIO{
     }
 
     public double getAppliedVoltage() {
-        return baseMotor.getMotorVoltage().getValueAsDouble();
+        return baseMotorLeft.getMotorVoltage().getValueAsDouble();
     }
 
     @Override
     public void simulationPeriodic() {
-        double voltsMotor = baseMotor.getMotorVoltage().getValueAsDouble();
+        double voltsMotor = baseMotorLeft.getMotorVoltage().getValueAsDouble();
         intakeSim.setInputVoltage(voltsMotor);
 
         intakeSim.update(Constants.LOOP_TIME);
@@ -194,8 +203,9 @@ public class Intake extends SubsystemBase implements IntakeIO{
 
     @Override
     public void updateInputs() {
-        inputs.measuredAngle = Units.rotationsToDegrees(baseMotor.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO);
-        inputs.currentAmps = baseMotor.getStatorCurrent().getValueAsDouble();
+        inputs.measuredAngleLeft = Units.rotationsToDegrees(baseMotorLeft.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO);
+        inputs.measuredAngleRight = Units.rotationsToDegrees(baseMotorLeft.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO);
+        inputs.currentAmps = baseMotorLeft.getStatorCurrent().getValueAsDouble();
         inputs.flyWheelVelocity = Units.rotationsPerMinuteToRadiansPerSecond(flyWheelMotor.getVelocity().getValueAsDouble() * 60);
 
         Logger.processInputs("Intake", inputs);

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 7df91add402f9642068253d577f96e7e9dc0ba18..ed9a578d62d07f95946e2c3e9ece650004d1bf19 100644 (file)
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -1,7 +1,7 @@
 package frc.robot.subsystems.intake;
 
 public class IntakeConstants {
-    public static final double PIVOT_GEAR_RATIO = 1/(18.0*18.0*10.0/54.0/60.0/38.0);  //38
+    public static final double PIVOT_GEAR_RATIO = 3.0;  //38
 
     public static final double MASS = 1.5753263; // kilograms: mass of arm specifically
     public static final double CENTER_OF_MASS_LENGTH = 0.199608903192622; // meters
@@ -11,14 +11,14 @@ public class IntakeConstants {
     public static final double MAX_VELOCITY = 15.0; // rad/s
     public static final double MAX_ACCELERATION = 100.0; // rad/s^2
 
-    public static final double START_ANGLE = 101.7539148;
+    // TODO: Find these
+    public static final double STOW_ANGLE = 101.7539148;
     public static final double INTAKE_ANGLE = 142.48;
-    public static final double STOW_ANGLE = 107.0;
 
     //TODO: find this
     public static final double FLYWHEEL_SPEED = 1.0;
 
     //TODO: find this
-    public static final double ABSOLUTE_OFFSET_ANGLE = (139.1748046875 - START_ANGLE + 30);
+    public static final double ABSOLUTE_OFFSET_ANGLE = 0.0;
 
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index 01eb981fbae8bc514ead7844af851859f8f8f511..9d0c9a67077f1ad18243d2bd9cda39b53be4a66c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -5,7 +5,8 @@ import org.littletonrobotics.junction.AutoLog;
 public interface IntakeIO {
     @AutoLog
     public static class IntakeIOInputs{
-        public double measuredAngle = IntakeConstants.START_ANGLE;
+        public double measuredAngleLeft = IntakeConstants.STOW_ANGLE;
+        public double measuredAngleRight = IntakeConstants.STOW_ANGLE;
         public double currentAmps = 0.0;
         public double flyWheelVelocity = 0.0;
     }