Dev

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

@@ -24,7 +24,6 @@
 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;

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/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

@@ -19,6 +19,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;
@@ -45,14 +46,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 +65,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 +106,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 +133,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 +152,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() {
@@ -187,7 +195,7 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i
 
     void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) {
         if (isStateReversed) {
-            setTargetArmAngle(subtractFrom360Degrees(targetState.targetAngle), targetState.ignoreConstraints);
+            setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints);
             return;
         }
         setTargetArmAngle(targetState.targetAngle, targetState.ignoreConstraints);
@@ -199,7 +207,8 @@ 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) {
@@ -215,12 +224,12 @@ private Rotation2d calculateMinimumArmSafeAngle() {
         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,14 +242,14 @@ 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);
     }