diff --git a/2025-training b/2025-training new file mode 160000 index 0000000..bc380f5 --- /dev/null +++ b/2025-training @@ -0,0 +1 @@ +Subproject commit bc380f511945ab0b6dbb442b2c6e5e035519b7a1 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..cda4276 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,15 @@ package frc.robot; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.Constants.OperatorConstants; +import frc.robot.subsystems.Piper; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -18,6 +24,10 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private Piper m_piper = Piper.getInstance(); + + private final CommandXboxController m_driverController = + new CommandXboxController(OperatorConstants.kDriverControllerPort); /** * This function is run when the robot is first started up and should be used for any @@ -28,6 +38,8 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + // DogLog.setOptions(new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true)); + } /** @@ -81,7 +93,10 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + m_driverController.rightBumper().onTrue(new InstantCommand(() -> m_piper.increaseFF())); + m_driverController.leftBumper().onTrue(new InstantCommand(() -> m_piper.decreaseFF())); + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..975491d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,8 +6,9 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.commands.Intake; +import frc.robot.commands.Shooter; +import frc.robot.subsystems.Piper; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -20,15 +21,19 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + private final Piper piper = Piper.getInstance(); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings + m_driverController.rightTrigger().whileTrue(new Intake(piper)); + m_driverController.leftTrigger().whileTrue(new Shooter(piper)); configureBindings(); } @@ -43,12 +48,12 @@ public RobotContainer() { */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + // new Trigger(m_exampleSubsystem::exampleCondition) + // .onTrue(new ExampleCommand(m_exampleSubsystem)); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // // cancelling on release. + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); } /** @@ -57,7 +62,8 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + return null; // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + // return Autos.exampleAuto(m_exampleSubsystem); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 107aad7..b32acdb 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,15 +4,11 @@ package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public final class Autos { /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } private Autos() { throw new UnsupportedOperationException("This is a utility class!"); diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java new file mode 100644 index 0000000..c612e6f --- /dev/null +++ b/src/main/java/frc/robot/commands/Intake.java @@ -0,0 +1,52 @@ +// 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.commands; +import frc.robot.subsystems.Piper; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class Intake extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Piper m_piper; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public Intake(Piper piper) { + m_piper = piper; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(piper); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double speed = 90; + //m_piper.spinShooter(speed); + m_piper.spinIntake(speed); + m_piper.spinPreShooter(speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_piper.stopIntake(); + m_piper.stopPreShooter(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_piper.isNotesThere(); + } +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/PiperCommand.java similarity index 84% rename from src/main/java/frc/robot/commands/ExampleCommand.java rename to src/main/java/frc/robot/commands/PiperCommand.java index 7481d3c..9a62e34 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/PiperCommand.java @@ -4,20 +4,20 @@ package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.Piper; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { +public class PiperCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; + private final Piper m_subsystem; /** * Creates a new ExampleCommand. * * @param subsystem The subsystem used by this command. */ - public ExampleCommand(ExampleSubsystem subsystem) { + public PiperCommand(Piper subsystem) { m_subsystem = subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(subsystem); diff --git a/src/main/java/frc/robot/commands/Shooter.java b/src/main/java/frc/robot/commands/Shooter.java new file mode 100644 index 0000000..668a8d6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter.java @@ -0,0 +1,51 @@ +// 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.commands; +import frc.robot.subsystems.Piper; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class Shooter extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Piper m_piper; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public Shooter(Piper piper) { + m_piper = piper; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(piper); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double speed = 50; + m_piper.spinShooter(speed); + m_piper.spinPreShooter(speed/2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_piper.stopShooter(); + m_piper.stopPreShooter(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return !(m_piper.isNotesThere()); + } +} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 6b375da..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,47 +0,0 @@ -// 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.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/Piper.java b/src/main/java/frc/robot/subsystems/Piper.java new file mode 100644 index 0000000..00be46e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Piper.java @@ -0,0 +1,172 @@ +// 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.subsystems; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; + +import dev.doglog.DogLog; + +import com.ctre.phoenix6.controls.VelocityVoltage; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Piper extends SubsystemBase { + private TalonFX shooter1, shooter2; + private TalonFX preshooterMotor; + private TalonFX intakeMotor; + private DigitalInput noteSwitch; + private static Piper piperInstance; + double FFConst; + + public Piper() { + // Shooter + shooter1 = new TalonFX(35); + shooter2 = new TalonFX(34); + shooter1.setInverted(true); + shooter2.setInverted(false); + + Slot0Configs s0c = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKG(0).withKV(0).withKA(0); + CurrentLimitsConfigs clc = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(20); + + shooter1.getConfigurator().apply(s0c); + shooter2.getConfigurator().apply(s0c); + shooter1.getConfigurator().apply(clc); + shooter2.getConfigurator().apply(clc); + + + // PreShooter + preshooterMotor = new TalonFX(32); + + preshooterMotor.getConfigurator().apply(clc); + + + intakeMotor = new TalonFX(33); + + intakeMotor.getConfigurator().apply(clc); + + //note detector + + noteSwitch = new DigitalInput(1); + + } + public boolean isNotesThere() { + return !(noteSwitch.get()); + } + + public static Piper piperInstance() { + if (piperInstance == null) { + piperInstance = new Piper(); + } + return piperInstance; + } + + //Shooter Functions + public void spinShooter(double speed) { + + VelocityVoltage VV = new VelocityVoltage(speed); + + shooter1.setControl(VV); + shooter2.setControl(VV); + } + + public void stopShooter() { + VelocityVoltage VV = new VelocityVoltage(0); + shooter1.setControl(VV); + shooter2.setControl(VV); + } + + // PreShooter Functions + public void spinPreShooter(double speed) { + + VelocityVoltage VV = new VelocityVoltage(speed * 1); +; + preshooterMotor.setControl(VV); + } + + public void stopPreShooter() { + preshooterMotor.stopMotor(); + } + + //Intake Functions + public void spinIntake(double speed) { + VelocityVoltage VV = new VelocityVoltage(speed).withFeedForward(FFConst); +; + intakeMotor.setControl(VV); + + } + + public void stopIntake() { + intakeMotor.stopMotor(); + } + + + + /** + * Example command factory method. + * + * @return a command + */ + public Command exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); + } + + /** + * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + public boolean exampleCondition() { + // Query some boolean state, such as a digital sensor. + return false; + } + + @Override + public void periodic() { + DogLog.log("IntakeMotor Speed", intakeMotor.get()); + SmartDashboard.putNumber("IntakeMotor Speed", intakeMotor.get()); + DogLog.log("ShooterMotor Speed", shooter1.get()); + SmartDashboard.putNumber("ShooterMotor Speed", shooter1.get()); + DogLog.log("IsNotePresent", !(noteSwitch.get()) ? 1:0); + SmartDashboard.putNumber("isnotepresent", !(noteSwitch.get()) ? 1:0); + DogLog.log("FF Constant Value", FFConst); + SmartDashboard.putNumber("FF Constant Value", FFConst); + } + + public void increaseFF() { + FFConst += 0.00001; + } + + public void decreaseFF() { + FFConst -= 0.00001; + } + + //private int getSpeed(int Speed) { + // return Speed; + //} + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } + public static Piper getInstance() { + if(piperInstance == null){ + piperInstance = new Piper(); + } + + return piperInstance; + } +} diff --git a/src/main/java/frc/robot/subsystems/RunMotorSubsystem.java b/src/main/java/frc/robot/subsystems/RunMotorSubsystem.java new file mode 100644 index 0000000..0717f99 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RunMotorSubsystem.java @@ -0,0 +1,48 @@ +// 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.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class RunMotorSubsystem extends SubsystemBase { + + //The motor is declared here! + private TalonFX motor; + private static RunMotorSubsystem instance; + + + private RunMotorSubsystem() { + //TODO: Initalize the motor on port 0 + motor = new TalonFX(0); + } + // This is a singleton pattern. Ensures only one instance of `RunMotorSubsystem` exists! + // See if you can understand how it works! + public static RunMotorSubsystem getInstance(){ + if(instance == null){ + instance = new RunMotorSubsystem(); + } + return instance; + } + + public void runMotor(double speed) { + //TODO: set motor speed and log speed + motor.set(speed); + SmartDashboard.putNumber("motor speed", speed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/util/LoggedTalonFX.java b/src/main/java/frc/robot/util/LoggedTalonFX.java new file mode 100644 index 0000000..92fa083 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTalonFX.java @@ -0,0 +1,81 @@ + +package frc.robot.util; + +import java.util.ArrayList; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import dev.doglog.DogLog; + +public class LoggedTalonFX extends TalonFX{ + + private static ArrayList motors = new ArrayList<>(); + private String name; + private String temperature,closedLoopError,closedLoopReference,position,velocity,acceleration,supplycurrent,statorcurrent,torquecurrent,motorvoltage,supplyvoltage; + + public LoggedTalonFX(String deviceName,int deviceId, String canbus) { + super(deviceId, canbus); + init(); + name = deviceName; + } + + public LoggedTalonFX(String deviceName,int deviceId) { + super(deviceId); + init(); + name = deviceName; + } + + public LoggedTalonFX(int deviceId, String canbus) { + super(deviceId, canbus); + init(); + name = "motor "+deviceId; + } + + public LoggedTalonFX(int deviceId) { + super(deviceId); + init(); + name = "motor "+deviceId; + } + + public void init(){ + motors.add(this); + this.temperature=name + "/temperature(degC)"; + this.closedLoopError = name + "/closedLoopError"; + this.closedLoopReference = name + "/closedLoopReference"; + this.position=name + "/position(rotations)"; + this.velocity=name + "/velocity(rps)"; + this.acceleration=name + "/acceleration(rps2)"; + this.supplycurrent=name + "/current/supply(A)"; + this.statorcurrent=name + "/current/stator(A)"; + this.torquecurrent=name + "/current/torque(A)"; + this.motorvoltage=name + "/voltage/motor(V)"; + this.supplyvoltage=name + "/voltage/supply(V)"; + } + + public static void peroidic(){ + for(LoggedTalonFX l: motors){ + l.periodic(); + } + } + + public void periodic(){ + DogLog.log(temperature,this.getDeviceTemp().getValue()); + DogLog.log(closedLoopError,this.getClosedLoopError().getValue()); + DogLog.log(closedLoopReference,this.getClosedLoopReference().getValue()); + + DogLog.log(position,this.getPosition().getValue()); + DogLog.log(velocity,this.getVelocity().getValue()); + DogLog.log(acceleration,this.getAcceleration().getValue()); + + //Current + DogLog.log(supplycurrent,this.getSupplyCurrent().getValue()); + DogLog.log(statorcurrent,this.getStatorCurrent().getValue()); + DogLog.log(torquecurrent,this.getTorqueCurrent().getValue()); + + //Voltage + DogLog.log(motorvoltage,this.getMotorVoltage().getValue()); + DogLog.log(supplyvoltage,this.getSupplyVoltage().getValue()); + } + +} diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 0000000..750a428 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2024.5.8" + } + ], + "fileName": "DogLog.json", + "frcYear": "2024", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2024.5.8", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file