diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aa489da..c94b579 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,9 @@ public class Robot extends TimedRobot @Override - public void robotInit() {} + public void robotInit() { + robotContainer = new RobotContainer(); + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4013790..c558423 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,28 +1,22 @@ -// Copyright (c) FIRST and other WPILib contributors. - -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Shooter.Shooter; import frc.robot.subsystems.Transporter.Transporter; - public class RobotContainer { + public static final Shooter SHOOTER = new Shooter(); public static final Transporter TRANSPORTER = new Transporter(); public RobotContainer() { configureBindings(); } - private void configureBindings() { } - public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java new file mode 100644 index 0000000..a095573 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.Shooter; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + private final TalonSRX + rightMotor = ShooterConstants.RIGHT_MOTOR, + leftMotor = ShooterConstants.LEFT_MOTOR; + private ShooterConstants.ShooterState currentState; + + void stopMotors() { + setMotorOutput(0); + } + + void setTargetState(ShooterConstants.ShooterState targetState) { + currentState = targetState; + setMotorOutput(targetState.voltagePercentage); + } + + void setMotorOutput(double targetVoltagePercentage) { + rightMotor.set(ControlMode.PercentOutput, targetVoltagePercentage); + leftMotor.set(ControlMode.PercentOutput, targetVoltagePercentage); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterCommands.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterCommands.java new file mode 100644 index 0000000..cb116eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterCommands.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.RobotContainer; + +public class ShooterCommands { + public static Command getSetTargetStateCommand(ShooterConstants.ShooterState targetState) { + return new StartEndCommand( + () -> RobotContainer.SHOOTER.setTargetState(targetState), + RobotContainer.SHOOTER::stopMotors, + RobotContainer.SHOOTER + ); + } + + private static Command getSetMotorOutputCommand(double targetVoltagePercentage) { + return new StartEndCommand( + () -> RobotContainer.SHOOTER.setMotorOutput(targetVoltagePercentage), + RobotContainer.SHOOTER::stopMotors, + RobotContainer.SHOOTER + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java new file mode 100644 index 0000000..c9c0b00 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.Shooter; + +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +public class ShooterConstants { + private static final int + RIGHT_MOTOR_ID = 6, + LEFT_MOTOR_ID = 7; + static final WPI_TalonSRX + RIGHT_MOTOR = new WPI_TalonSRX(RIGHT_MOTOR_ID), + LEFT_MOTOR = new WPI_TalonSRX(LEFT_MOTOR_ID); + + private static final InvertType RIGHT_MOTOR_INVERTED_VALUE = InvertType.None; + private static final InvertType LEFT_MOTOR_INVERTED_VALUE = InvertType.InvertMotorOutput; + private static final NeutralMode NEUTRAL_MODE = NeutralMode.Coast; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + + static { + configureRightMotor(); + configureLeftMotor(); + } + + private static final void configureRightMotor() { + RIGHT_MOTOR.configFactoryDefault(); + + RIGHT_MOTOR.setInverted(RIGHT_MOTOR_INVERTED_VALUE); + RIGHT_MOTOR.setNeutralMode(NEUTRAL_MODE); + + RIGHT_MOTOR.enableVoltageCompensation(true); + RIGHT_MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static final void configureLeftMotor() { + LEFT_MOTOR.configFactoryDefault(); + + LEFT_MOTOR.setInverted(LEFT_MOTOR_INVERTED_VALUE); + LEFT_MOTOR.setNeutralMode(NEUTRAL_MODE); + + LEFT_MOTOR.enableVoltageCompensation(true); + LEFT_MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_SATURATION); + } + + public enum ShooterState { + SHOOT(1), + EJECT(0.3), + STOP(0); + + final double voltagePercentage; + + ShooterState(double voltagePercentage) { + this.voltagePercentage = voltagePercentage; + } + } +} \ No newline at end of file