From f3c7a35d0e51d908ea6ceb4ce687c714b28afb71 Mon Sep 17 00:00:00 2001 From: Ray Healy Date: Wed, 11 Mar 2026 19:56:48 -0400 Subject: [PATCH] src/robot/subsystems: Added invert motor direction flags --- src/main/java/frc/robot/Constants.java | 30 +++++++++++++------ .../frc/robot/subsystems/HopperSubsystem.java | 7 ++++- .../frc/robot/subsystems/IntakeSubsystem.java | 10 +++---- .../frc/robot/subsystems/TurretSubsystem.java | 28 ++++++++++------- 4 files changed, 50 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2174bde..eeab7a5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,8 @@ import edu.wpi.first.units.Unit; import edu.wpi.first.units.measure.Angle; import java.security.PublicKey; +import com.ctre.phoenix6.signals.InvertedValue; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -67,23 +69,30 @@ public final class Constants { public static class IntakeConstants { public static final int DEPLOY_MOTOR_ID = 12; + public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this + public static final int INTAKE_MOTOR_ID = 1; + public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this } public static class ShooterConstants { - public static final int LIMIT_CHANNEL = 0; - public static final int TURRET_ID = 1; - public static final int LAUNCHER_ID = 2; - public static final int HOOD_CHANNEL = 3; - public static final int HOOD_ENCODER_ID = 4; + public static final int LAUNCHER_MOTOR_ID = 2; + public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this + + public static final int TURRET_MOTOR_ID = 1; + public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final int TURRET_LIMIT_CHANNEL = 0; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); - public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); - public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); - public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); public static final double HOMING_SPEED = .35; public static final double TURRET_GEAR_RATIO = 1; + public static final int HOOD_SERVO_CHANNEL = 3; + public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this + public static final int HOOD_ENCODER_ID = 4; + public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); + public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); + public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0); public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(20); public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(40); @@ -92,6 +101,9 @@ public final class Constants { public static class HopperConstants { public static final int KICKER_MOTOR_ID = 0; - public static final int SPINDEXER_MOTOR_ID = 1; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + + public static final int SPINDEXER_MOTOR_ID = 1; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 3da74ad..31a1578 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -6,10 +6,12 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; + import com.ctre.phoenix6.hardware.TalonFX; + import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import static org.fairportrobotics.frc.robolib.motors.Utils.*; import frc.robot.Constants; -import frc.robot.commands.ManualHopperCommand; public class HopperSubsystem extends TestableSubsystem { @@ -18,7 +20,10 @@ public class HopperSubsystem extends TestableSubsystem { /** Creates a new HopperSubsystem. */ public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); + SetMotorDirection(spindexerMotor, Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION); + kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); + SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION); } public void feed() {kickerMotor.set(0.5);} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7123b1b..d0e5eb0 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -9,16 +9,13 @@ import org.fairportrobotics.frc.posty.test.PostTest; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.Constants; + import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import static org.fairportrobotics.frc.robolib.motors.Utils.*; -import java.time.chrono.ThaiBuddhistChronology; - -import edu.wpi.first.hal.FRCNetComm.tInstances; -import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; @@ -32,9 +29,12 @@ public class IntakeSubsystem extends TestableSubsystem { public IntakeSubsystem() { intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); + SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); + deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); } public void neutral() diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index e4e4057..1cb6f68 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -5,21 +5,20 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; -import org.fairportrobotics.frc.posty.test.PostTest; -import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; -import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import frc.robot.Constants; +import static org.fairportrobotics.frc.robolib.motors.Utils.*; + import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj2.command.Command; public class TurretSubsystem extends TestableSubsystem { public enum TurretState { @@ -40,12 +39,16 @@ public class TurretSubsystem extends TestableSubsystem { private Angle targElev; public TurretSubsystem() { - turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.LIMIT_CHANNEL); - turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_ID); + turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL); + turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID); + SetMotorDirection(turretMotor, Constants.ShooterConstants.TURRET_MOTOR_DIRECTION); - launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_ID, MotorType.kBrushless); + launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); + SparkMaxConfig config = new SparkMaxConfig() + .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO_2); + config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); - hood = new Servo(Constants.ShooterConstants.HOOD_CHANNEL); + hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID); currentState = TurretState.INIT; limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; @@ -126,16 +129,21 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.set(0); } + int k = 1; + if(Constants.ShooterConstants.HOOD_SERVO_INVERTED) { + k=-1; + } + Angle curElev = hoodEncoder.getAbsolutePosition().getValue(); if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees)) { if(targElev.gt(curElev)) { - hood.set(50); + hood.set(k * 0.5); } else { - hood.set(-50); + hood.set(k * -0.5); } } else