src/robot/subsystems: Added invert motor direction flags

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 LAUNCHER_MOTOR_ID = 2;
-    public static final int TURRET_ID = 1;
+    public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
-    public static final int LAUNCHER_ID = 2;
+
-    public static final int HOOD_CHANNEL = 3;
+    public static final int TURRET_MOTOR_ID = 1;
-    public static final int HOOD_ENCODER_ID = 4;
+    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 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;
   }
 }

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);}

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()

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);
+    turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
-    turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_ID);
+    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