Dev

src/main/deploy/pathplanner/settings.json

@@ -18,7 +18,7 @@
   "driveGearing": 6.12,
   "maxDriveSpeed": 4.4,
   "driveMotorType": "krakenX60FOC",
-  "driveCurrentLimit": 100.0,
+  "driveCurrentLimit": 120.0,
   "wheelCOF": 1.6,
   "flModuleX": 0.172,
   "flModuleY": 0.273,

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -24,10 +24,7 @@
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.armelevator.ArmElevator;
 import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
-import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.climber.Climber;
-import frc.trigon.robot.subsystems.climber.ClimberCommands;
-import frc.trigon.robot.subsystems.climber.ClimberConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffector;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
@@ -38,6 +35,7 @@
 import frc.trigon.robot.subsystems.transporter.Transporter;
 import frc.trigon.robot.subsystems.transporter.TransporterCommands;
 import frc.trigon.robot.subsystems.transporter.TransporterConstants;
+import lib.commands.WheelRadiusCharacterizationCommand;
 import lib.utilities.flippable.Flippable;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -85,8 +83,8 @@ private void configureBindings() {
     private void bindDefaultCommands() {
         SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
         ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand());
-        CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST));
-        END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST));
+//        CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST));
+        END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getDefaultCommand());
         INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST));
         TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST));
     }

src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java

@@ -90,7 +90,7 @@ public static Command getDriveToReefAndScoreCommand() {
 
     public static Command getCollectCoralCommand(boolean isRight) {
         return new ParallelCommandGroup(
-                CoralCollectionCommands.getIntakeSequenceCommand(),
+                CoralCollectionCommands.getIntakeCoralCommand(),
                 ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL),
                 getDriveToCoralCommand(isRight)
         )

src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java

@@ -19,7 +19,8 @@
 public class CoralCollectionCommands {
     public static Command getCoralCollectionCommand() {
         return new SequentialCommandGroup(
-                getIntakeSequenceCommand(),
+                getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral),
+                getCollectionConfirmationCommand(),
                 new InstantCommand(
                         () -> {
                             if (!AlgaeManipulationCommands.isHoldingAlgae())
@@ -33,7 +34,7 @@ public static Command getLoadCoralCommand() {
         return new ParallelCommandGroup(
                 ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL),
                 EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL)
-        ).until(RobotContainer.END_EFFECTOR::hasGamePiece);
+        ).until(RobotContainer.END_EFFECTOR::hasGamePiece).unless(RobotContainer.END_EFFECTOR::hasGamePiece);
     }
 
     public static Command getUnloadCoralCommand() {
@@ -43,23 +44,13 @@ public static Command getUnloadCoralCommand() {
         ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece());
     }
 
-    static Command getIntakeSequenceCommand() {
-        return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral);
-    }
-
-    private static Command getInitiateCollectionCommand() {
+    public static Command getIntakeCoralCommand() {
         return new ParallelCommandGroup(
                 IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT),
-                TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.COLLECT)
+                TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT)
         );
     }
 
-    private static Command getAlignCoralCommand() {
-        return new SequentialCommandGroup(
-                TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.ALIGN_CORAL).until(RobotContainer.TRANSPORTER::hasCoral),
-                TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.HOLD_CORAL)
-        );
-    }
 
     private static Command getCollectionConfirmationCommand() {
         return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER));

src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java

@@ -18,7 +18,7 @@
 import lib.utilities.flippable.FlippableTranslation2d;
 
 public class CoralPlacingCommands {
-    public static boolean SHOULD_SCORE_AUTONOMOUSLY = true;
+    public static boolean SHOULD_SCORE_AUTONOMOUSLY = false;
     static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;
 
     public static Command getScoreInReefCommand(boolean shouldScoreRight) {
@@ -44,7 +44,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) {
 
     private static Command getScoreCommand(boolean shouldScoreRight) {
         return new SequentialCommandGroup(
-                getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER),
+                getPrepareArmElevatorIfWontHitReef(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER),
                 new ParallelCommandGroup(
                         GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore),
                         EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL)

src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java

@@ -8,8 +8,8 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.RobotContainer;
-import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
 import lib.hardware.RobotHardwareStats;
+import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -27,7 +27,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs
     private static final List<MotorSubsystem> REGISTERED_SUBSYSTEMS = new ArrayList<>();
     private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled);
     private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8);
-    private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation());
+    private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", true);//TODO: AHAHAHAHAHHA
 
     static {
         DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true));

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
 import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import lib.hardware.phoenix6.cancoder.CANcoderEncoder;
@@ -19,6 +18,7 @@
 import org.littletonrobotics.junction.Logger;
 
 public class ArmElevator extends MotorSubsystem {
+    private static final boolean SHOULD_CALIBRATE_ELEVATOR = true;
     private final TalonFXMotor
             armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR,
             elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR;
@@ -30,7 +30,7 @@ public class ArmElevator extends MotorSubsystem {
             ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION,
             ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_JERK
     ).withEnableFOC(ArmElevatorConstants.FOC_ENABLED);
-    private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage(
+    private final DynamicMotionMagicVoltage elevatorPositionRequest = new DynamicMotionMagicVoltage(
             0,
             ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY,
             ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION,
@@ -45,14 +45,15 @@ public ArmElevator() {
 
     @Override
     public SysIdRoutine.Config getSysIDConfig() {
-        return ArmElevatorConstants.ARM_SYSID_CONFIG;
-        //return ArmElevatorConstants.ELEVATOR_SYSID_CONFIG;
+        return SHOULD_CALIBRATE_ELEVATOR ? ArmElevatorConstants.ELEVATOR_SYSID_CONFIG : ArmElevatorConstants.ARM_SYSID_CONFIG;
     }
 
     @Override
     public void setBrake(boolean brake) {
         armMasterMotor.setBrake(brake);
+        ArmElevatorConstants.ARM_FOLLOWER_MOTOR.setBrake(brake);
         elevatorMasterMotor.setBrake(brake);
+        ArmElevatorConstants.ELEVATOR_FOLLOWER_MOTOR.setBrake(brake);
     }
 
     @Override
@@ -63,21 +64,24 @@ public void stop() {
 
     @Override
     public void updateLog(SysIdRoutineLog log) {
+        if (SHOULD_CALIBRATE_ELEVATOR) {
+            log.motor("Elevator")
+                    .linearPosition(Units.Meters.of(getElevatorPositionRotations()))
+                    .linearVelocity(Units.MetersPerSecond.of(elevatorMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ELEVATOR_GEAR_RATIO))
+                    .voltage(Units.Volts.of(elevatorMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
+            return;
+        }
         log.motor("Arm")
-                .angularPosition(Units.Rotations.of(getCurrentArmAngle().getRotations()))
-                .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.VELOCITY)))
+                .angularPosition(Units.Rotations.of(armMasterMotor.getSignal(TalonFXSignal.POSITION)))
+                .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ARM_GEAR_RATIO))
                 .voltage(Units.Volts.of(armMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
-//        log.motor("Elevator")
-//                .linearPosition(Units.Meters.of(getPositionRotations()))
-//                .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY)))
-//                .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
     }
 
     @Override
     public void updateMechanism() {
         ArmElevatorConstants.ARM_MECHANISM.update(
-                Rotation2d.fromRotations(getCurrentArmAngle().getRotations() + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET),
-                Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET)
+                Rotation2d.fromRotations(getCurrentArmAngle().getRotations()),
+                Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)
         );
         ArmElevatorConstants.ELEVATOR_MECHANISM.update(
                 getCurrentElevatorPositionMeters(),
@@ -101,8 +105,11 @@ public void updatePeriodically() {
 
     @Override
     public void sysIDDrive(double targetVoltage) {
+        if (SHOULD_CALIBRATE_ELEVATOR) {
+            elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
+            return;
+        }
         armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
-//        elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
     }
 
     public Pose3d calculateGamePieceCollectionPose() {
@@ -125,7 +132,7 @@ public boolean atState(ArmElevatorConstants.ArmElevatorState targetState) {
     public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) {
         if (targetState == null)
             return false;
-        return armAtAngle(isStateReversed ? subtractFrom360Degrees(targetState.targetAngle) : targetState.targetAngle)
+        return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle)
                 && elevatorAtPosition(targetState.targetPositionMeters);
     }
 
@@ -144,7 +151,7 @@ public boolean elevatorAtPosition(double positionMeters) {
     }
 
     public Rotation2d getCurrentArmAngle() {
-        return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
+        return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET);
     }
 
     public double getCurrentElevatorPositionMeters() {
@@ -186,8 +193,9 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i
     }
 
     void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) {
+        scaleArmPositionRequestSpeed(targetState.speedScalar);
         if (isStateReversed) {
-            setTargetArmAngle(subtractFrom360Degrees(targetState.targetAngle), targetState.ignoreConstraints);
+            setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints);
             return;
         }
         setTargetArmAngle(targetState.targetAngle, targetState.ignoreConstraints);
@@ -199,28 +207,29 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) {
     }
 
     void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) {
-        armMasterMotor.setControl(armPositionRequest.withPosition(ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations())));
+        final double targetPosition = ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations());
+        armMasterMotor.setControl(armPositionRequest.withPosition(targetPosition - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET));
     }
 
     void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignoreConstraints) {
         setTargetElevatorPositionRotations(metersToRotations(targetPositionMeters), ignoreConstraints);
     }
 
     void setTargetElevatorPositionRotations(double targetPositionRotations, boolean ignoreConstraints) {
-        elevatorMasterMotor.setControl(positionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations())));
+        elevatorMasterMotor.setControl(elevatorPositionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations())));
     }
 
     private Rotation2d calculateMinimumArmSafeAngle() {
         final double heightFromSafeZone = getElevatorHeightFromMinimumSafeZone();
         final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmElevatorConstants.ARM_LENGTH_METERS, 0, 1);
         final double acos = Math.acos(cosOfMinimumSafeAngle);
         return Double.isNaN(acos)
-                ? Rotation2d.fromRadians(0)
-                : Rotation2d.fromRadians(acos);
+                ? Rotation2d.fromDegrees(-90)
+                : Rotation2d.fromRadians(acos).minus(Rotation2d.kCCW_90deg);
     }
 
     private double calculateMinimumSafeElevatorHeightRotations() {
-        final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getCos();
+        final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().plus(Rotation2d.kCCW_90deg).getCos();
         final double elevatorHeightFromArm = armCos * ArmElevatorConstants.ARM_LENGTH_METERS;
         final double minimumElevatorSafeZone = ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
         final double minimumSafeHeightMeters = (RobotContainer.ARM_ELEVATOR.isArmAboveSafeAngle()
@@ -233,22 +242,28 @@ private double getElevatorHeightFromMinimumSafeZone() {
         return getCurrentElevatorPositionMeters() - ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
     }
 
-    private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) {
-        return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees());
+    private static Rotation2d subtractFrom180Degrees(Rotation2d angleToSubtract) {
+        return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() - angleToSubtract.getDegrees());
     }
 
     private Pose3d calculateVisualizationPose() {
         final Transform3d armTransform = new Transform3d(
                 new Translation3d(0, 0, getCurrentElevatorPositionMeters()),
-                new Rotation3d(0, getCurrentArmAngle().getRadians(), 0)
+                new Rotation3d(0, getCurrentArmAngle().getRadians() + Math.PI / 2, 0)
         );
         return ArmElevatorConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform);
     }
 
+    private void scaleArmPositionRequestSpeed(double speedScalar) {
+        armPositionRequest.Velocity = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_VELOCITY * speedScalar;
+        armPositionRequest.Acceleration = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION * speedScalar;
+        armPositionRequest.Jerk = armPositionRequest.Acceleration * 10;
+    }
+
     private void scaleElevatorPositionRequestSpeed(double speedScalar) {
-        positionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar;
-        positionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar;
-        positionRequest.Jerk = positionRequest.Acceleration * 10;
+        elevatorPositionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar;
+        elevatorPositionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar;
+        elevatorPositionRequest.Jerk = elevatorPositionRequest.Acceleration * 10;
     }
 
     private Pose3d getFirstStageComponentPose() {