diff --git a/src/main/java/com/team841/calliope/Robot.java b/src/main/java/com/team841/calliope/Robot.java index eee8899..2bdc0dd 100644 --- a/src/main/java/com/team841/calliope/Robot.java +++ b/src/main/java/com/team841/calliope/Robot.java @@ -17,105 +17,112 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - @Override - public void robotInit() { - - Logger.recordMetadata("ProjectName", "Calliope-software"); - Logger.recordMetadata("TuningMode", Boolean.toString(RC.robotType == RC.RunType.DEV)); - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer = RobotContainer.getInstance(); + + @Override + public void robotInit() { + + Logger.recordMetadata("ProjectName", "Calliope-software"); + Logger.recordMetadata("TuningMode", Boolean.toString(RC.robotType == RC.RunType.DEV)); + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + switch (RC.robotType) { + case COMP, DEV: + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + new PowerDistribution(1, PowerDistribution.ModuleType.kRev); // Enables power distribution logging + break; + case SIM: + Logger.addDataReceiver(new NT4Publisher()); // Save outputs to a new log + break; + case REPLAY: + setUseTiming(false); + String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"), 0.01)); + break; + } + + Logger.start(); } - switch (RC.robotType){ - case COMP: - Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(1, PowerDistribution.ModuleType.kRev); // Enables power distribution logging - break; - case SIM: - Logger.addDataReceiver(new NT4Publisher()); // Save outputs to a new log - break; - case REPLAY: - setUseTiming(false); - String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"), 0.01)); - break; + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); } - Logger.start(); - - m_robotContainer = new RobotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() {} + @Override + public void disabledInit() { + } - @Override - public void disabledPeriodic() {} + @Override + public void disabledPeriodic() { + } - @Override - public void disabledExit() {} + @Override + public void disabledExit() { + } - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } } - } - @Override - public void autonomousPeriodic() {} + @Override + public void autonomousPeriodic() { + } - @Override - public void autonomousExit() {} + @Override + public void autonomousExit() { + } - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + @Override + public void teleopInit() { + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } } - } - @Override - public void teleopPeriodic() {} + @Override + public void teleopPeriodic() { + } - @Override - public void teleopExit() {} + @Override + public void teleopExit() { + } - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } - @Override - public void testPeriodic() {} + @Override + public void testPeriodic() { + } - @Override - public void testExit() {} + @Override + public void testExit() { + } } diff --git a/src/main/java/com/team841/calliope/RobotContainer.java b/src/main/java/com/team841/calliope/RobotContainer.java index 539847f..c7e8dfc 100644 --- a/src/main/java/com/team841/calliope/RobotContainer.java +++ b/src/main/java/com/team841/calliope/RobotContainer.java @@ -1,20 +1,254 @@ -// 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 com.team841.calliope; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.team841.calliope.constants.RC; +import com.team841.calliope.constants.Swerve; +import com.team841.calliope.drive.Drivetrain; +import com.team841.calliope.superstructure.hanger.Hanger; +import com.team841.calliope.superstructure.hanger.HangerIO; +import com.team841.calliope.superstructure.hanger.HangerIOTalonFX; +import com.team841.calliope.superstructure.indexer.Indexer; +import com.team841.calliope.superstructure.indexer.IndexerIO; +import com.team841.calliope.superstructure.indexer.IndexerIOTalonFX; +import com.team841.calliope.superstructure.intake.Intake; +import com.team841.calliope.superstructure.intake.IntakeCommand; +import com.team841.calliope.superstructure.intake.IntakeIO; +import com.team841.calliope.superstructure.intake.IntakeIOTalonFX; +import com.team841.calliope.superstructure.lights.LED; +import com.team841.calliope.superstructure.lights.LEDIO; +import com.team841.calliope.superstructure.lights.LEDIOSpark; +import com.team841.calliope.superstructure.shooter.Shooter; +import com.team841.calliope.superstructure.shooter.ShooterIO; +import com.team841.calliope.superstructure.shooter.ShooterIOTalonFX; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { - public RobotContainer() { - configureBindings(); - } - private void configureBindings() {} + public final Drivetrain drivetrain = Swerve.DriveTrain; + + public final ShooterIO shooterIO; + public final Shooter shooter; + + public final IntakeIO intakeIO; + public final Intake intake; + + public final IndexerIO indexerIO; + public final Indexer indexer; + + public final HangerIO hangerIO; + public final Hanger hanger; + + public final LEDIO ledIO; + public final LED led; + + public final CommandXboxController soloStick = new CommandXboxController(RC.Controllers.soloStick); + + public final CommandPS5Controller duoStickDrive = new CommandPS5Controller(RC.Controllers.duoStickDrive); + public final CommandXboxController duoStickCoDrive = new CommandXboxController(RC.Controllers.duoStickCoDrive); + + private final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(Swerve.MaxSpeed * 0.1) + .withRotationalDeadband(Swerve.MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric + + // driving in open loop + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry telemetry = new Telemetry(Swerve.MaxSpeed); + + private static RobotContainer instance; + + public static RobotContainer getInstance() { + if (instance == null) { + instance = new RobotContainer(); + } + return instance; + } + + public RobotContainer() { + switch (RC.robotType) { + default -> { + this.shooterIO = new ShooterIOTalonFX(); + this.shooter = new Shooter(this.shooterIO); + + this.intakeIO = new IntakeIOTalonFX(); + this.intake = new Intake(this.intakeIO); + + this.indexerIO = new IndexerIOTalonFX(); + this.indexer = new Indexer(this.indexerIO); + + this.hangerIO = new HangerIOTalonFX(); + this.hanger = new Hanger(this.hangerIO); + + this.ledIO = new LEDIOSpark(); + this.led = new LED(this.ledIO); + } + } + + configureSoloStick(); + configureDuoStick(); + } + + private void configureSoloStick() { + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> + drive + .withVelocityX(-soloStick.getLeftY() * Swerve.MaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -soloStick.getLeftX() * Swerve.MaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -soloStick.getRightX() + * Swerve + .MaxAngularRate))); // Drive counterclockwise with negative X (left) + + // reset the field-centric heading on left bumper press + soloStick.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); + + if (Utils.isSimulation()) { + drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); + } else { + drivetrain.registerTelemetry(telemetry::telemeterize); + } + + Command c_command = new IntakeCommand(intake, indexer); + soloStick.leftBumper().whileTrue(c_command); + soloStick + .leftTrigger() + .onTrue(new InstantCommand(shooter::spinUp)) + .onFalse(new InstantCommand(shooter::stopShooter)); + soloStick + .rightTrigger() + .onTrue( + new ConditionalCommand( + new InstantCommand(indexer::Pass), + new InstantCommand(indexer::stopIndexer), + () -> shooter.isShooting())) + .onFalse(new InstantCommand(indexer::stopIndexer)); + soloStick + .x() + .onTrue( + new SequentialCommandGroup( + new InstantCommand(indexer::stopIndexer), + new InstantCommand(shooter::stopShooter))); + soloStick.rightStick().whileTrue(new InstantCommand(hanger::ExtendHanger)).onFalse(new InstantCommand(hanger::StopHanger)); + soloStick.leftStick().whileTrue(new InstantCommand(hanger::RetractHanger)).onFalse(new InstantCommand(hanger::StopHanger)); + soloStick.povCenter().whileTrue(new InstantCommand(hanger::StopHanger)); + soloStick.povLeft().whileTrue(new InstantCommand(hanger::toggleHanger)); + soloStick + .y() + .onTrue(new InstantCommand(shooter::ampShot)) + .onFalse(new InstantCommand(shooter::stopShooter)); + soloStick + .b() + .onTrue( + new ParallelCommandGroup( + new InstantCommand(intake::outTake), new InstantCommand(indexer::reverseIndexer))) + .onFalse( + new SequentialCommandGroup( + new InstantCommand(indexer::stopIndexer), new InstantCommand(intake::stopIntake))); + + soloStick + .rightBumper() + .whileTrue(new InstantCommand(shooter::flyShot)) + .onFalse(new InstantCommand(shooter::stopShooter)); + } + + private void configureDuoStick() { + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> + drive + .withVelocityX(-duoStickDrive.getLeftY() * Swerve.MaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -duoStickDrive.getLeftX() * Swerve.MaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -duoStickDrive.getRightX() + * Swerve + .MaxAngularRate))); // Drive counterclockwise with negative X (left) + + + duoStickDrive.cross().whileTrue(drivetrain.applyRequest(() -> brake)); + duoStickDrive + .circle() + .whileTrue( + drivetrain.applyRequest( + () -> + point.withModuleDirection( + new Rotation2d(-duoStickDrive.getLeftY(), -duoStickDrive.getLeftX())))); + + // reset the field-centric heading on left bumper press + duoStickDrive.touchpad().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); + + //duoStickDrive.R2().whileTrue(autoAim); + + //duoStickDrive.triangle().whileTrue(BackOffTrap); + + if (Utils.isSimulation()) { + drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); + } else { + drivetrain.registerTelemetry(telemetry::telemeterize); + } + + Command c_command = new IntakeCommand(intake, indexer); + duoStickCoDrive.leftBumper().whileTrue(c_command); + duoStickCoDrive + .leftTrigger() + .onTrue(new InstantCommand(shooter::spinUp)) + .onFalse(new InstantCommand(shooter::stopShooter)); + duoStickCoDrive + .rightTrigger() + .onTrue( + new ConditionalCommand( + new InstantCommand(indexer::Pass), + new InstantCommand(indexer::stopIndexer), + () -> shooter.isShooting())) + .onFalse(new InstantCommand(indexer::stopIndexer)); + duoStickCoDrive + .rightBumper() + .onTrue( + new SequentialCommandGroup( + new InstantCommand(indexer::stopIndexer), + new InstantCommand(shooter::stopShooter))); + duoStickCoDrive.povUp().whileTrue(new InstantCommand(hanger::ExtendHanger)); + duoStickCoDrive.povDown().whileTrue(new InstantCommand(hanger::RetractHanger)); + duoStickCoDrive.povCenter().whileTrue(new InstantCommand(hanger::StopHanger)); + duoStickCoDrive.povLeft().whileTrue(new InstantCommand(hanger::toggleHanger)); + duoStickCoDrive + .x() + .onTrue(new InstantCommand(shooter::ampShot)) + .onFalse(new InstantCommand(shooter::stopShooter)); + duoStickCoDrive + .b() + .onTrue( + new ParallelCommandGroup( + new InstantCommand(intake::outTake), new InstantCommand(indexer::reverseIndexer))) + .onFalse( + new SequentialCommandGroup( + new InstantCommand(indexer::stopIndexer), new InstantCommand(intake::stopIntake))); + + duoStickCoDrive + .y() + .whileTrue(new InstantCommand(shooter::flyShot)) + .onFalse(new InstantCommand(shooter::stopShooter)); + duoStickCoDrive + .a() + .onTrue(new InstantCommand(shooter::trapShot)) + .onFalse(new InstantCommand(shooter::stopShooter)); + } - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } } diff --git a/src/main/java/com/team841/calliope/Telemetry.java b/src/main/java/com/team841/calliope/Telemetry.java new file mode 100644 index 0000000..1e54d4d --- /dev/null +++ b/src/main/java/com/team841/calliope/Telemetry.java @@ -0,0 +1,118 @@ +package com.team841.calliope; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import org.littletonrobotics.junction.Logger; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Robot speeds for general checking */ + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + Logger.recordOutput("Telemetry/Speed", velocities.getNorm()); + velocityX.set(velocities.getX()); + Logger.recordOutput("Telemetry/VelocityX", velocities.getX()); + velocityY.set(velocities.getY()); + Logger.recordOutput("Telemetry/VelocityY", velocities.getY()); + odomPeriod.set(state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + Logger.recordOutput("Telemetry/Module" + i, m_moduleMechanisms[i]); + } + } +} diff --git a/src/main/java/com/team841/calliope/constants/Manifest.java b/src/main/java/com/team841/calliope/constants/Manifest.java deleted file mode 100644 index f427eb2..0000000 --- a/src/main/java/com/team841/calliope/constants/Manifest.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.team841.calliope.constants; - -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - -public class Manifest { - public class SubsystemManifest { - - } - - public class JoystickManifest { - - } -} diff --git a/src/main/java/com/team841/calliope/constants/RC.java b/src/main/java/com/team841/calliope/constants/RC.java index 526ac19..e857770 100644 --- a/src/main/java/com/team841/calliope/constants/RC.java +++ b/src/main/java/com/team841/calliope/constants/RC.java @@ -18,11 +18,11 @@ public class RC { public static final RunType robotType = RunType.SIM; public static class Controllers { - public static final int driverPortLeft = 0; // controller USB port 0 - public static final int driverPortRight = 1; // controller USB port 1 - public static final int codriverPort = 2; // controller USB port 2 + public static final int soloStick = 3; + public static final int duoStickDrive = 0; + public static final int duoStickCoDrive = 1; - public static final int SysIDCommandPort = 4; // for sysID + public static final int SysIDCommandPort = 5; // for sysID } public static final class SC_CAN_ID { diff --git a/src/main/java/com/team841/calliope/constants/Swerve.java b/src/main/java/com/team841/calliope/constants/Swerve.java index 4fea89c..281ff3e 100644 --- a/src/main/java/com/team841/calliope/constants/Swerve.java +++ b/src/main/java/com/team841/calliope/constants/Swerve.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.team841.calliope.drive.Drivetrain; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -158,7 +159,7 @@ public class Swerve { public static double MaxAngularRate = 4 * Math.PI; // 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity public static double MaxSpeed = kSpeedAt12VoltsMps; - // protected static final Drivetrain DriveTrain = new Drivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + public static final Drivetrain DriveTrain = new Drivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); public static final ProfiledPIDController TurnController = new ProfiledPIDController(7, 0.0, 0.0, new TrapezoidProfile.Constraints(0, 0)); diff --git a/src/main/java/com/team841/calliope/superstructure/indexer/IndexerIOTalonFX.java b/src/main/java/com/team841/calliope/superstructure/indexer/IndexerIOTalonFX.java index 7c9c630..ed34bd4 100644 --- a/src/main/java/com/team841/calliope/superstructure/indexer/IndexerIOTalonFX.java +++ b/src/main/java/com/team841/calliope/superstructure/indexer/IndexerIOTalonFX.java @@ -26,10 +26,12 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.velocity = this.indexerTalon.getVelocity().getValue(); } + @Override public void setDutyCycle(double velocity) { this.indexerTalon.setControl(output.withOutput(velocity)); } + @Override public void stopIndexer() { this.indexerTalon.stopMotor(); } diff --git a/src/main/java/com/team841/calliope/superstructure/intake/Intake.java b/src/main/java/com/team841/calliope/superstructure/intake/Intake.java index e2a2bfc..d4387d3 100644 --- a/src/main/java/com/team841/calliope/superstructure/intake/Intake.java +++ b/src/main/java/com/team841/calliope/superstructure/intake/Intake.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { @@ -17,6 +18,7 @@ public Intake(IntakeIO io) { @Override public void periodic() { io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); } public void setIntakeBrakes(boolean on) { diff --git a/src/main/java/com/team841/calliope/superstructure/intake/IntakeIOTalonFX.java b/src/main/java/com/team841/calliope/superstructure/intake/IntakeIOTalonFX.java index f9ec1d5..dfdabe3 100644 --- a/src/main/java/com/team841/calliope/superstructure/intake/IntakeIOTalonFX.java +++ b/src/main/java/com/team841/calliope/superstructure/intake/IntakeIOTalonFX.java @@ -5,7 +5,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.team841.calliope.constants.RC; -public class IntakeIOTalonFX { +public class IntakeIOTalonFX implements IntakeIO { private final TalonFX intakeMotor = new TalonFX(RC.SC_CAN_ID.kIntake, "rio"); @@ -15,11 +15,13 @@ public IntakeIOTalonFX(){ setBrakes(brakesOn); } + @Override public void updateInputs(IntakeIO.IntakeIOInputs inputs) { inputs.brakesOn = brakesOn; inputs.velocity = this.intakeMotor.getVelocity().getValue(); } + @Override public void setBrakes(boolean on) { intakeMotor .getConfigurator() @@ -28,10 +30,12 @@ public void setBrakes(boolean on) { .withNeutralMode(on ? NeutralModeValue.Brake : NeutralModeValue.Coast)); } + @Override public void setVelocity(double velocity) { intakeMotor.set(velocity); } + @Override public void stopIntake() { intakeMotor.stopMotor(); } diff --git a/src/main/java/com/team841/calliope/superstructure/lights/LED.java b/src/main/java/com/team841/calliope/superstructure/lights/LED.java new file mode 100644 index 0000000..ed60d28 --- /dev/null +++ b/src/main/java/com/team841/calliope/superstructure/lights/LED.java @@ -0,0 +1,45 @@ +package com.team841.calliope.superstructure.lights; + +import com.team841.calliope.RobotContainer; +import com.team841.calliope.superstructure.indexer.Indexer; +import com.team841.calliope.superstructure.intake.Intake; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class LED extends SubsystemBase { + + private final LEDIO io; + private final LedIOInputsAutoLogged inputs = new LedIOInputsAutoLogged(); + + private Indexer indexer = RobotContainer.getInstance().indexer; + + private Intake intake = RobotContainer.getInstance().intake; + + private int count = 0; + + /** Creates a new LED. */ + public LED(LEDIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("LED", inputs); + + // if (intake.) + if (indexer.getRightIndexerSensor() && indexer.getLeftIndexerSensor()) { + io.setColor("Green"); + if (count == 0) count += 1; + } + + if (count > 0) { + count += 1; + } + + if (count > 200) { + io.setColor("Violet"); + count = 0; + } + } +} diff --git a/src/main/java/com/team841/calliope/superstructure/lights/LEDIO.java b/src/main/java/com/team841/calliope/superstructure/lights/LEDIO.java new file mode 100644 index 0000000..20b623a --- /dev/null +++ b/src/main/java/com/team841/calliope/superstructure/lights/LEDIO.java @@ -0,0 +1,14 @@ +package com.team841.calliope.superstructure.lights; + +import org.littletonrobotics.junction.AutoLog; + +public interface LEDIO { + @AutoLog + public class LedIOInputs{ + public double sparkOutput; + } + + public default void updateInputs(LedIOInputs inputs) { } + + public default void setColor(String color) { } +} diff --git a/src/main/java/com/team841/calliope/superstructure/lights/LEDIOSpark.java b/src/main/java/com/team841/calliope/superstructure/lights/LEDIOSpark.java new file mode 100644 index 0000000..2965569 --- /dev/null +++ b/src/main/java/com/team841/calliope/superstructure/lights/LEDIOSpark.java @@ -0,0 +1,26 @@ +package com.team841.calliope.superstructure.lights; + +import com.team841.calliope.constants.SC; +import edu.wpi.first.wpilibj.motorcontrol.Spark; + +public class LEDIOSpark implements LEDIO{ + private final Spark LED = new Spark(SC.Intake.kBlinkingID); + + public LEDIOSpark() { + + } + + @Override + public void updateInputs(LedIOInputs inputs) { + inputs.sparkOutput = LED.get(); + } + + @Override + public void setColor(String color){ + switch (color) { + case "Violet" -> LED.set(0.91); + case "Green" -> LED.set(.77); + case "Orange" -> LED.set(.65); + } + } +} diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java b/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java index c789e86..388eaeb 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/Shooter.java @@ -2,49 +2,39 @@ import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.team841.betaSwerve2024.Constants.ConstantsIO; -import com.team841.betaSwerve2024.Constants.SC; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { + private final ShooterIO io; + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - public Shooter() { - - // bottomShooter.setControl(new Follower(topShooter.getDeviceID(), false)); + public Shooter(ShooterIO io) { + this.io = io; } - private void setVelocity(double velocity) { - topShooter.setControl(new MotionMagicVelocityVoltage(velocity)); - } - - protected void Shoot() { - setVelocity(60); + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); } public void spinUp() { - topShooter.setControl( - new MotionMagicVelocityVoltage(95).withFeedForward(6).withAcceleration(200).withSlot(0)); - bottomShooter.setControl( - new MotionMagicVelocityVoltage(95).withFeedForward(6).withAcceleration(200).withSlot(0)); + io.setMotionMagicVelocityVoltageOutput(95); } public void ampShot() { - topShooter.setControl( - new MotionMagicVelocityVoltage(1.5).withFeedForward(6).withAcceleration(200).withSlot(0)); - bottomShooter.setControl( - new MotionMagicVelocityVoltage(12.5).withFeedForward(6).withAcceleration(200).withSlot(0)); + io.setMotionMagicVelocityVoltageOutput(1.5, 12.5); } public Command idleBack() { return new RunCommand( () -> { - topShooter.set(-0.05); - bottomShooter.set(-0.015); + io.setMotionMagicVelocityVoltageOutput(-0.05, -0.015); }, this); } @@ -52,55 +42,30 @@ public Command idleBack() { public void trapShot() { double top = 9.9375; double bottom = 82.8125; - topShooter.setControl( - new MotionMagicVelocityVoltage(top).withFeedForward(6).withAcceleration(200).withSlot(0)); - bottomShooter.setControl( - new MotionMagicVelocityVoltage(bottom) - .withFeedForward(6) - .withAcceleration(200) - .withSlot(0)); + io.setMotionMagicVelocityVoltageOutput(top, bottom); } public void flyShot() { - topShooter.setControl( - new MotionMagicVelocityVoltage(70).withFeedForward(6).withAcceleration(200).withSlot(0)); - bottomShooter.setControl( - new MotionMagicVelocityVoltage(70).withFeedForward(6).withAcceleration(200).withSlot(0)); + io.setMotionMagicVelocityVoltageOutput(70); } public void disruptshot() { - topShooter.setControl( - new MotionMagicVelocityVoltage(12.5).withFeedForward(6).withAcceleration(200).withSlot(0)); - bottomShooter.setControl( - new MotionMagicVelocityVoltage(12.5).withFeedForward(6).withAcceleration(200).withSlot(0)); - } - - protected double getMotorVoltage() { - return topShooter.getMotorVoltage().getValue(); + io.setMotionMagicVelocityVoltageOutput(12.5); } public boolean isShooting() { - return this.topShooter.getVelocity().getValue() > 0; + return this.inputs.topVelocity > 0; } public boolean isHighShot() { - return this.topShooter.getVelocity().getValue() > 75; + return this.inputs.topVelocity > 75; } - /* public void stopShooter() { - topShooter.stopMotor(); - bottomShooter.stopMotor(); - } */ - public void stopShooter() { - bottomShooter.set(-0.05); - topShooter.set(-0.05); + io.setMotionMagicVelocityVoltageOutput(-0.05); } public Command runShooter(double velocity) { - return new InstantCommand(() -> this.setVelocity(velocity)); + return new InstantCommand(() -> io.setMotionMagicVelocityVoltageOutput(velocity)); } - - @Override - public void periodic() {} } diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java index 8fa333b..f3ee7ed 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIO.java @@ -5,13 +5,18 @@ public interface ShooterIO { @AutoLog public class ShooterIOInputs{ - public double velocity; - public double motionMagicVoltageOutput; + public double topVelocity; + public double bottomVelocity; + public double topMotionMagicVoltageVelocity; + public double bottomMotionMagicVoltageVelocity; + } public default void updateInputs(ShooterIOInputs inputs) { } - public default void setMotionMagicVoltageOutput(double voltageOutput) { } + public default void setMotionMagicVelocityVoltageOutput(double velocity) { } + + public default void setMotionMagicVelocityVoltageOutput(double topVelocity, double bottomVelocity) { } - public default void stopShooter() { } + public default void stop() { } } diff --git a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java index ae6ca4a..119d26f 100644 --- a/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java +++ b/src/main/java/com/team841/calliope/superstructure/shooter/ShooterIOTalonFX.java @@ -1,5 +1,6 @@ package com.team841.calliope.superstructure.shooter; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.team841.calliope.constants.RC; import com.team841.calliope.constants.SC; @@ -8,16 +9,37 @@ public class ShooterIOTalonFX implements ShooterIO{ private final TalonFX bottomShooter = new TalonFX(RC.SC_CAN_ID.bottomShooter, "rio"); private final TalonFX topShooter = new TalonFX(RC.SC_CAN_ID.topShooter, "rio"); + private final MotionMagicVelocityVoltage topControl = new MotionMagicVelocityVoltage(0).withFeedForward(6).withAcceleration(200).withSlot(0); + private final MotionMagicVelocityVoltage bottomControl = new MotionMagicVelocityVoltage(0).withFeedForward(6).withAcceleration(200).withSlot(0); + public ShooterIOTalonFX() { bottomShooter.getConfigurator().apply(SC.Shooter.k_BottomConfiguration); topShooter.getConfigurator().apply(SC.Shooter.k_TopConfiguration); } + @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.velocity = bottomShooter + inputs.topVelocity = topShooter.getVelocity().getValue(); + inputs.bottomVelocity = bottomShooter.getVelocity().getValue(); + inputs.bottomMotionMagicVoltageVelocity = bottomControl.Velocity; + inputs.topMotionMagicVoltageVelocity = topControl.Velocity; } - public default void setMotionMagicVoltageOutput(double voltageOutput) { } + @Override + public void setMotionMagicVelocityVoltageOutput(double velocity) { + bottomShooter.setControl(bottomControl.withVelocity(velocity)); + topShooter.setControl(topControl.withVelocity(velocity)); + } - public default void stopShooter() { } + @Override + public void setMotionMagicVelocityVoltageOutput(double topVelocity, double bottomVelocity) { + bottomShooter.setControl(bottomControl.withVelocity(topVelocity)); + topShooter.setControl(topControl.withVelocity(bottomVelocity)); + } + + @Override + public void stop() { + bottomShooter.stopMotor(); + topShooter.stopMotor(); + } }