working branch

src/main/java/frc/trigon/robot/subsystems/collector/Collector.java

@@ -0,0 +1,60 @@
+package frc.trigon.robot.subsystems.collector;
+
+
+import com.ctre.phoenixpro.hardware.TalonFX;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Collector extends SubsystemBase {
+
+    private final static Collector INSTANCE = new Collector();
+
+    public static Collector getInstance() {
+        return INSTANCE;
+    }
+
+    private final TalonFX motor = CollectorConstants.MOTOR;
+
+    private Collector() {
+    }
+
+    public CommandBase getCollectCommand() {
+        return new StartEndCommand(
+                this::collect,
+                this::stop,
+                this
+        );
+    }
+
+    public CommandBase getEjectCommand() {
+        return new StartEndCommand(
+                this::eject,
+                this::stop,
+                this
+        );
+    }
+    public CommandBase getCollectWaitEjectWait() {
+        return new SequentialCommandGroup(
+                getCollectCommand().withTimeout(3),
+                getEjectCommand().withTimeout(3));
+
+    }
+
+    public CommandBase getCollectThenEjectCommand() {
+        return getCollectCommand().andThen(getEjectCommand());
+    }
+
+    private void eject() {
+        motor.setVoltage(CollectorConstants.EJECT_VOLTAGE);
+    }
+
+    private void collect() {
+        motor.setVoltage(CollectorConstants.COLLECT_VOLTAGE);
+    }
+
+    private void stop() {
+        motor.stopMotor();
+    }
+}

src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java

@@ -0,0 +1,28 @@
+package frc.trigon.robot.subsystems.collector;
+
+import com.ctre.phoenixpro.configs.TalonFXConfiguration;
+import com.ctre.phoenixpro.hardware.TalonFX;
+import com.ctre.phoenixpro.signals.InvertedValue;
+import com.ctre.phoenixpro.signals.NeutralModeValue;
+
+public class CollectorConstants {
+    private static final int MOTOR_ID = 0;
+    private static final InvertedValue INVERTED = InvertedValue.Clockwise_Positive;
+    private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake;
+    public static final double
+            EJECT_VOLTAGE = -6,
+            COLLECT_VOLTAGE = 6;
+    static final TalonFX MOTOR = new TalonFX(MOTOR_ID);
+
+    static {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.Audio.BeepOnBoot = false;
+        config.MotorOutput.Inverted = INVERTED;
+        config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE;
+        MOTOR.getConfigurator().apply(config);
+    }
+
+
+
+
+}

src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDrive.java

@@ -0,0 +1,103 @@
+package frc.trigon.robot.subsystems.differentialDrive;
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import java.util.function.Supplier;
+
+
+public class DifferentialDrive extends SubsystemBase {
+    private final static DifferentialDrive INSTANCE = new DifferentialDrive();
+    private final MotorControllerGroup leftMotors = DifferentialDriveConstants.LEFT_MOTORS;
+    private final MotorControllerGroup rightMotors = DifferentialDriveConstants.RIGHT_MOTORS;
+    private final edu.wpi.first.wpilibj.drive.DifferentialDrive motors = new edu.wpi.first.wpilibj.drive.DifferentialDrive(leftMotors, rightMotors);
+
+    public static DifferentialDrive getInstance() {
+        return INSTANCE;
+    }
+
+    private DifferentialDrive() {
+    }
+
+    /**
+     * Creates a command that drives the differential drive in arcade mode.
+     *
+     * @param powerSupplier    the forward power supplier
+     * @param rotationSupplier the rotation power supplier
+     * @return the command
+     */
+    public CommandBase getArcadeDriveCommand(Supplier<Double> powerSupplier, Supplier<Double> rotationSupplier) {
+        return new FunctionalCommand(
+                () -> {
+                },
+                () -> arcadeDrive(powerSupplier.get(), rotationSupplier.get()),
+                (interrupted) -> stop(),
+                () -> false,
+                this
+        );
+    }
+
+    /**
+     * Creates the command that drives the differential drive in tank mode.
+     *
+     * @param leftPowerSupplier  the left power supplier
+     * @param rightPowerSupplier the right power supplier
+     * @return return the command
+     */
+    public CommandBase getTankDriveCommand(Supplier<Double> leftPowerSupplier, Supplier<Double> rightPowerSupplier) {
+        return new FunctionalCommand(
+                () -> {
+                },
+                () -> tankDrive(leftPowerSupplier.get(), rightPowerSupplier.get()),
+                (interrupted) -> stop(),
+                () -> false,
+                this
+        );
+    }
+
+    /**
+     * Creates a command that drives the differential drive in curvature mode.
+     *
+     * @param powerSupplier            the forward power supplier
+     * @param rotationSupplier         the rotation power supplier
+     * @param allowTurnInPlaceSupplier a supplier for turning in place
+     * @return the command
+     */
+    public CommandBase getCurvatureDriveCommand(Supplier<Double> powerSupplier, Supplier<Double> rotationSupplier, Supplier<Boolean> allowTurnInPlaceSupplier) {
+        return new FunctionalCommand(
+                () -> {
+                },
+                () -> curvatureDrive(powerSupplier.get(), rotationSupplier.get(), allowTurnInPlaceSupplier.get()),
+                (interrupted) -> stop(),
+                () -> false,
+                this
+        );
+    }
+
+    private void curvatureDrive(double power, double rotation, boolean allowTurnInPlace) {
+        motors.curvatureDrive(power, rotation, allowTurnInPlace);
+    }
+
+    private void arcadeDrive(double power, double rotation) {
+        motors.arcadeDrive(power, rotation);
+    }
+
+    private void tankDrive(double leftPower, double rightPower) {
+        motors.tankDrive(leftPower, rightPower);
+    }
+
+    private void stopLeftMotors() {
+        leftMotors.stopMotor();
+    }
+
+    private void stopRightMotors() {
+        rightMotors.stopMotor();
+    }
+
+    private void stop() {
+        stopLeftMotors();
+        stopRightMotors();
+    }
+}

src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDriveConstants.java

@@ -0,0 +1,42 @@
+package frc.trigon.robot.subsystems.differentialDrive;
+
+import com.ctre.phoenixpro.configs.TalonFXConfiguration;
+import com.ctre.phoenixpro.hardware.TalonFX;
+import com.ctre.phoenixpro.signals.InvertedValue;
+import com.ctre.phoenixpro.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+
+public class DifferentialDriveConstants {
+    private static final int
+            FRONT_LEFT_MOTOR_ID = 0,
+            FRONT_RIGHT_MOTOR_ID = 1,
+            BACK_LEFT_MOTOR_ID = 2,
+            BACK_RIGHT_MOTOR_ID = 3;
+
+    private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive;
+    private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake;
+
+    static final TalonFX
+            FRONT_LEFT_MOTOR = new TalonFX(FRONT_LEFT_MOTOR_ID),
+            FRONT_RIGHT_MOTOR = new TalonFX(FRONT_RIGHT_MOTOR_ID),
+            BACK_LEFT_MOTOR = new TalonFX(BACK_LEFT_MOTOR_ID),
+            BACK_RIGHT_MOTOR = new TalonFX(BACK_RIGHT_MOTOR_ID);
+
+    static MotorControllerGroup LEFT_MOTORS = new MotorControllerGroup(FRONT_LEFT_MOTOR, BACK_LEFT_MOTOR);
+    static MotorControllerGroup RIGHT_MOTORS = new MotorControllerGroup(FRONT_RIGHT_MOTOR, BACK_RIGHT_MOTOR);
+
+
+    static {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.Audio.BeepOnBoot = false;
+        config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE;
+        config.MotorOutput.Inverted = INVERTED_VALUE;
+        BACK_RIGHT_MOTOR.getConfigurator().apply(config);
+        BACK_LEFT_MOTOR.getConfigurator().apply(config);
+        FRONT_RIGHT_MOTOR.getConfigurator().apply(config);
+        FRONT_LEFT_MOTOR.getConfigurator().apply(config);
+    }
+
+
+
+}

src/main/java/frc/trigon/robot/subsystems/steer/Steer.java

@@ -0,0 +1,83 @@
+package frc.trigon.robot.subsystems.steer;
+
+
+import com.ctre.phoenixpro.controls.PositionVoltage;
+import com.ctre.phoenixpro.hardware.TalonFX;
+import edu.wpi.first.wpilibj2.command.*;
+
+import java.util.Date;
+import java.util.function.Supplier;
+
+public class
+Steer extends SubsystemBase {
+
+    private final static Steer INSTANCE = new Steer();
+
+    public static Steer getInstance() {
+        return INSTANCE;
+    }
+
+    private final TalonFX motor = SteerConstants.MOTOR;
+
+    private Steer() {
+    }
+
+    /**
+     * Creates a command to set a target angle in degrees using a given angle supplier.
+     *
+     * @param angleDegrees A Supplier that provides the target angle in degrees
+     * @return return the command
+     */
+    public CommandBase getSetTargetAngleCommand(Supplier<Double> angleDegrees) {
+        return new FunctionalCommand(
+                () -> {
+                },
+                () -> setTargetAngle(angleDegrees.get()),
+                (interrupted) -> stop(),
+                () -> false,
+                this
+        );
+    }
+
+    /**
+     * Creates a command to set a target angle in degrees directly.
+     *
+     * @param angleDegrees The target angle in degrees
+     * @return return the command
+     */
+    public CommandBase getSetTargetAngleCommand(double angleDegrees) {
+        return new FunctionalCommand(
+                () -> {
+                },
+                () -> setTargetAngle(angleDegrees),
+                (interrupted) -> stop(),
+                () -> false,
+                this
+        );
+    }
+
+    /**
+     * Get a command to set a sequence of target angles.
+     *
+     * @return return the command
+     */
+    public CommandBase getTargetAngleCommand() {
+        return new SequentialCommandGroup(
+                getSetTargetAngleCommand(90).withTimeout(3),
+                getSetTargetAngleCommand(180).withTimeout(3),
+                getSetTargetAngleCommand(0));
+
+    }
+
+    private void setTargetAngle(double angleDegrees) {
+        double systemRevolution = angleDegrees / 360;
+        double motorRevolution = systemRevolution * SteerConstants.GEAR_RATIO;
+        PositionVoltage request = new PositionVoltage(motorRevolution);
+        motor.setControl(request);
+    }
+
+    private void stop() {
+        motor.stopMotor();
+    }
+}
+

src/main/java/frc/trigon/robot/subsystems/steer/SteerConstants.java

@@ -0,0 +1,32 @@
+package frc.trigon.robot.subsystems.steer;
+
+import com.ctre.phoenixpro.configs.TalonFXConfiguration;
+import com.ctre.phoenixpro.hardware.TalonFX;
+import com.ctre.phoenixpro.signals.InvertedValue;
+import com.ctre.phoenixpro.signals.NeutralModeValue;
+
+public class SteerConstants {
+    private static final int MOTOR_ID = 0;
+    private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive;
+    private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake;
+    static final TalonFX MOTOR = new TalonFX(MOTOR_ID);
+    static final double GEAR_RATIO = 12.8;
+    private static final double
+            P = 8.4,
+            I = 0,
+            D = 0;
+
+    static {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.Audio.BeepOnBoot = false;
+        config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE;
+        config.MotorOutput.Inverted = INVERTED_VALUE;
+        config.Slot0.kP = P;
+        config.Slot0.kI = I;
+        config.Slot0.kD = D;
+        MOTOR.getConfigurator().apply(config);
+
+    }
+
+
+}