From b9eaaee18c2bd202d4c8fda4d1e146fe27656735 Mon Sep 17 00:00:00 2001 From: ofirKAMInetsky <144820835+ofirKAMInetsky@users.noreply.github.com> Date: Tue, 12 Sep 2023 19:32:32 +0300 Subject: [PATCH] initial commit --- .../robot/subsystems/collector/Collector.java | 60 ++++++++++ .../collector/CollectorConstants.java | 28 +++++ .../differentialDrive/DifferentialDrive.java | 103 ++++++++++++++++++ .../DifferentialDriveConstants.java | 42 +++++++ .../trigon/robot/subsystems/steer/Steer.java | 83 ++++++++++++++ .../subsystems/steer/SteerConstants.java | 32 ++++++ .../robot/subsystems/turret/Turret.java | 61 +++++++++++ .../subsystems/turret/TurretConstants.java | 28 +++++ 8 files changed, 437 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/collector/Collector.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDrive.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDriveConstants.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/steer/Steer.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/steer/SteerConstants.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/turret/Turret.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java b/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java new file mode 100644 index 0000000..c624f31 --- /dev/null +++ b/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(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java new file mode 100644 index 0000000..9a31986 --- /dev/null +++ b/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); + } + + + + +} diff --git a/src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDrive.java b/src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDrive.java new file mode 100644 index 0000000..2acb184 --- /dev/null +++ b/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 powerSupplier, Supplier 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 leftPowerSupplier, Supplier 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 powerSupplier, Supplier rotationSupplier, Supplier 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(); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDriveConstants.java b/src/main/java/frc/trigon/robot/subsystems/differentialDrive/DifferentialDriveConstants.java new file mode 100644 index 0000000..90515f4 --- /dev/null +++ b/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); + } + + + +} diff --git a/src/main/java/frc/trigon/robot/subsystems/steer/Steer.java b/src/main/java/frc/trigon/robot/subsystems/steer/Steer.java new file mode 100644 index 0000000..2aed15e --- /dev/null +++ b/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 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(); + } +} + diff --git a/src/main/java/frc/trigon/robot/subsystems/steer/SteerConstants.java b/src/main/java/frc/trigon/robot/subsystems/steer/SteerConstants.java new file mode 100644 index 0000000..b227e55 --- /dev/null +++ b/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); + + } + + +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java new file mode 100644 index 0000000..8e08812 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java @@ -0,0 +1,61 @@ +package frc.trigon.robot.subsystems.turret; + + +import com.ctre.phoenixpro.controls.PositionVoltage; +import com.ctre.phoenixpro.controls.VoltageOut; +import com.ctre.phoenixpro.hardware.TalonFX; +import edu.wpi.first.math.controller.PIDController; +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 Turret extends SubsystemBase { + private final static Turret INSTANCE = new Turret(); + private final TalonFX motor = TurretConstants.MOTOR; + private final PIDController pidController = TurretConstants.PID_CONTROLLER; + + private Turret() { + + } + + public static Turret getInstance() { + return INSTANCE; + } + + /** + * Creates a command that aligns to the reflector. + * + * @param reflectorPositionSupplier the reflector position Supplier + * @param hasTargetSupplier a supplier that indicates whether a target is present + * @return return the command + */ + public CommandBase getAlignToReflectorCommand(Supplier reflectorPositionSupplier, Supplier hasTargetSupplier) { + return new FunctionalCommand( + () -> { + }, + () -> alignToReflector(reflectorPositionSupplier.get(), hasTargetSupplier.get()), + (interrupted) -> stop(), + () -> false, + this + ); + + } + + private void alignToReflector(double reflectorPosition, boolean hasTarget) { + if (!hasTarget) { + PositionVoltage request = new PositionVoltage(5); + motor.setControl(request); + return; + } + VoltageOut request = new VoltageOut(pidController.calculate(reflectorPosition)); + motor.setControl(request); + } + + private void stop() { + motor.stopMotor(); + } + +} + diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java new file mode 100644 index 0000000..cdf24a2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java @@ -0,0 +1,28 @@ +package frc.trigon.robot.subsystems.turret; + +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.math.controller.PIDController; + +public class TurretConstants { + private static 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); + private static final double + P = 8.4, + I = 0, + D = 0; + static final PIDController PID_CONTROLLER = new PIDController(P,I,D); + static { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; + config.MotorOutput.Inverted = INVERTED_VALUE; + MOTOR.getConfigurator().apply(config); + PID_CONTROLLER.setSetpoint(0); + + } +}