Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'shoot-button' into Leds_idk

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-11 16:34:12 -07:00
committed by GitHub
parent 0cc2d423fe f2415195ce
commit de0876293b
35 changed files with 1597 additions and 370 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+42 -19
View File
@@ -9,19 +9,15 @@ package frc4388.robot;
import java.io.File;
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -32,25 +28,28 @@ import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.structs.LEDPatterns;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.alignment.RotTo45;
import frc4388.robot.commands.MoveUntilSuply;
// import frc4388.robot.commands.alignment.DriveToReef;
// import frc4388.robot.commands.wait.waitElevatorRefrence;
// import frc4388.robot.commands.wait.waitEndefectorRefrence;
// import frc4388.robot.commands.wait.waitFeedCoral;
import frc4388.robot.commands.wait.waitSupplier;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.LEDConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import com.pathplanner.lib.commands.PathPlannerAuto;
import frc4388.robot.constants.FieldConstants;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.shooter.Shooter;
import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.FieldPositions;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.controller.DeadbandedXboxController;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.structs.LEDPatterns;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -93,6 +92,22 @@ public class RobotContainer {
private SendableChooser<String> autoChooser;
private Command autoCommand;
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new WaitCommand(5),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
// );
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
// );
// private Command RobotIntake = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
// );
public RobotContainer() {
@@ -103,6 +118,7 @@ public class RobotContainer {
// }, false);
DeferredBlock.addBlock(() -> { // Called on every robot enable
TimesNegativeOne.update();
FieldPositions.update();
}, true);
@@ -160,6 +176,13 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotLED.setTo5V()));
// new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
// .onTrue(RobotShoot)
// .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter));
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
// .onTrue(RobotIntake)
// .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter));
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
@@ -212,7 +235,7 @@ public class RobotContainer {
dir = new File("/home/lvuser/deploy/pathplanner/autos/");
} else {
// dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos\\");
}
String[] autos = dir.list();
@@ -238,7 +261,7 @@ public class RobotContainer {
// }
// System.out.println("Robot Auto Changed " + filename);
});
// SmartDashboard.putData(autoChooser);
SmartDashboard.putData(autoChooser);
}
src/main/java/frc4388/robot/RobotMap.java
+33 -26
View File
@@ -9,26 +9,24 @@ package frc4388.robot;
import com.ctre.phoenix6.hardware.TalonFX;
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.constants.Constants;
//import frc4388.robot.constants.Constants.ElevatorConstants;
import frc4388.robot.constants.Constants.SimConstants;
import frc4388.robot.constants.Constants.VisionConstants;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.robot.subsystems.intake.IntakeIO;
import frc4388.robot.subsystems.intake.IntakeReal;
import frc4388.robot.subsystems.shooter.ShooterConstants;
import frc4388.robot.subsystems.shooter.ShooterIO;
import frc4388.robot.subsystems.shooter.ShooterReal;
// import frc4388.robot.subsystems.elevator.ElevatorIO;
// import frc4388.robot.subsystems.elevator.ElevatorReal;
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
import frc4388.robot.subsystems.swerve.SwerveIO;
import frc4388.robot.subsystems.swerve.SwerveReal;
import frc4388.robot.subsystems.vision.VisionIO;
import frc4388.robot.subsystems.vision.VisionReal;
import frc4388.utility.status.FaultCANCoder;
import frc4388.utility.status.FaultPhotonCamera;
import frc4388.utility.status.FaultPidgeon2;
import frc4388.utility.status.FaultTalonFX;
@@ -55,18 +53,19 @@ public class RobotMap {
/* Swreve Drive Subsystem */
// public final SwerveIO swerveDrivetrain;
// /* Elevator Subsystem */
// public final ElevatorIO elevatorIO;
// /* Shooter and Intake Subsystem */
public final ShooterIO shooterIO;
public final IntakeIO intakeIO;
public RobotMap(SimConstants.Mode mode) {
switch (mode) {
case REAL:
// // Configure cameras
// Configure cameras
// PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
// PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
// leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
// rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
// leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ;
// rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS);
// FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
// FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
@@ -84,25 +83,34 @@ public class RobotMap {
// swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
// Configure elevator
// TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
// TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
// Configure Shooter 22,23,24
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS);
TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS);
TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS);
//Configure Intake 20,21
TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS);
TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS);
// DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
// DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
// DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
// elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
// shooterIO = new ShooterIO() {};
shooterIO = new ShooterReal(shooter1, shooter2, indexer);
intakeIO = new IntakeReal(arm, roller);
// Fault
// FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
// // FaultTalonFX.addDevice(elevator, "Elevator");
// // FaultTalonFX.addDevice(endeffector, "Endeffector");
FaultTalonFX.addDevice(shooter1, "Shooter1");
FaultTalonFX.addDevice(shooter2, "Shooter2");
FaultTalonFX.addDevice(indexer, "Indexer");
FaultTalonFX.addDevice(arm, "Arm");
FaultTalonFX.addDevice(roller, "Roller");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
@@ -123,10 +131,9 @@ public class RobotMap {
default:
// leftCamera = new VisionIO() {};
// rightCamera = new VisionIO() {};
// reefLidar = new LidarIO() {};
// reverseLidar = new LidarIO() {};
// swerveDrivetrain = new SwerveIO() {};
// elevatorIO = new ElevatorIO() {};
swerveDrivetrain = new SwerveIO() {};
intakeIO = new IntakeIO() {};
shooterIO = new ShooterIO() {};
break;
}
}
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt
-20
View File
@@ -1,20 +0,0 @@
AUTO file format
HEADER static size 0x5
0x00 BYTE NUM AXES: defualts to 6
0x01 BYTE NUM POV: defualts to 1
0x02 BYTE NUM CONTROLLERS: defualts to 2
0x03 SHORT FRAMES: any value greator or equal than one.
FRAME PER CONTROLLER: defualt size 0x34
0x00 DOUBLE AXES[NUM AXES]
0x30 SHORT BUTTONS
0x32 SHORT POVs[NUM POV]
FRAME: size varrys
FRAME PER CONTROLLER[NUM CONTROLLERS]
INT UNIXTIMESTAMP
FILE:
HEADER
FRAME[FRAMES]
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
-108
View File
@@ -1,108 +0,0 @@
package frc4388.robot.commands.Autos;
// package frc4388.robot.commands.Autos;
// import java.io.File;
// import java.util.ArrayList;
// import java.util.HashMap;
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.wpilibj2.command.InstantCommand;
// import frc4388.robot.commands.Swerve.JoystickPlayback;
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.utility.controller.VirtualController;
// public class neoPlaybackChooser {
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
// private final SwerveDrive m_swerve;
// private final VirtualController[] m_controllers;
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// // private SendableChooser<Command> m_playback = null;
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
// // private final File m_dir = new File("/home/lvuser/autos/");
// // private int m_cmdNum = 0;
// // commands
// private Command m_noAuto = new InstantCommand();
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
// m_teamChosser.addOption("Red", "red");
// m_teamChosser.setDefaultOption("Blue", "blue");
// m_teamChosser.addOption("Nuetral", "nuetral");
// m_possitionChosser.addOption("AMP", "amp");
// m_possitionChosser.setDefaultOption("Center", "center");
// m_possitionChosser.addOption("Source", "source");
// m_swerve = swerve;
// m_controllers = controllers;
// }
// public neoPlaybackChooser addOption(String name, String option) {
// m_autoNameChosser.addOption(name, option);
// return this;
// }
// // public PlaybackChooser buildDisplay() {
// // for (int i = 0; i < 10; i++) {
// // appendCommand();
// // }
// // m_playback = m_choosers.get(0);
// // nextChooser();
// // // ! This does not work, why?
// // Shuffleboard.getTab("Auto Chooser")
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// // .withPosition(4, 0);
// // return this;
// // }
// // This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// m_widgets.add(widget);
// }
// // public void nextChooser() {
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// // String[] dirs = m_dir.list();
// // if(dirs != null){ // Fix funny error
// // for (String auto : dirs) {
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// // }
// // }
// // for (var cmd_name : m_commandPool.keySet()) {
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// // }
// // }
// public String autoName() {
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
// }
// public Command getCommand() {
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
// }
// }
src/main/java/frc4388/robot/constants/BuildConstants.java
+8 -8
View File
@@ -5,15 +5,15 @@ package frc4388.robot.constants;
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 9;
public static final String GIT_SHA = "fe412d09810b860f3ac2a65296613feac250a4e9";
public static final String GIT_DATE = "2026-01-19 13:57:43 MST";
public static final String GIT_BRANCH = "master";
public static final String BUILD_DATE = "2026-01-27 19:38:56 MST";
public static final long BUILD_UNIX_TIME = 1769567936908L;
public static final int DIRTY = 1;
public static final int GIT_REVISION = 40;
public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190";
public static final String GIT_DATE = "2026-02-10 19:42:47 MST";
public static final String GIT_BRANCH = "shoot-button";
public static final String BUILD_DATE = "2026-02-11 12:41:33 MST";
public static final long BUILD_UNIX_TIME = 1770838893524L;
public static final int DIRTY = 0;
private BuildConstants(){}
}
src/main/java/frc4388/robot/constants/Constants.java
+17 -22
View File
@@ -7,7 +7,7 @@
package frc4388.robot.constants;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.CANBus;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
@@ -21,7 +21,6 @@ import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.utility.compute.Trim;
import frc4388.utility.status.CanDevice;
import frc4388.utility.structs.Gains;
import frc4388.utility.structs.LEDPatterns;
@@ -34,7 +33,8 @@ import frc4388.utility.structs.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final String CANBUS_NAME = "rio";
public static final CANBus RIO_CANBUS = CANBus.roboRIO();
public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain");
// public static final class LiDARConstants {
// public static final int REEF_LIDAR_DIO_CHANNEL = 7;
@@ -91,32 +91,27 @@ public final class Constants {
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
}
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
// Test april tag field layout
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
// Arrays.asList(new AprilTag[] {
// new AprilTag(1, new Pose3d(
// new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
// )),
// }), 100, 100);
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 9;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED_ORANGE;
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
// LED color for when the intake is out
public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
// LED color for when the intake is out, but the driver conditions are bad
public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE;
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
// LED color for when the flywheel speed isn't in tolarance
public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD;
// LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad
public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE;
// Operator ready to shoot
public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE;
// Operator ready to shoot, but the driver conditions are bad
public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
}
public static final class OIConstants {
src/main/java/frc4388/robot/constants/FieldConstants.java
+27
View File
@@ -0,0 +1,27 @@
package frc4388.robot.constants;
import java.util.Arrays;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
public final class FieldConstants {
// public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
public static final Translation2d BLUE_HUB_POS = new Translation2d();
public static final Translation2d RED_HUB_POS = new Translation2d();
// Test april tag field layout
public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
Arrays.asList(new AprilTag[] {
new AprilTag(0, new Pose3d(
new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
)),
}), 100, 100);
}
src/main/java/frc4388/robot/subsystems/LED.java
+4
View File
@@ -44,6 +44,10 @@ public class LED extends SubsystemBase implements Queryable {
this.mode = pattern;
}
public String getMode(){
return mode.name();
}
@Override
public void periodic() {
update();
src/main/java/frc4388/robot/subsystems/TestRobot.java
+245
View File
@@ -0,0 +1,245 @@
package frc4388.robot.subsystems;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble;
public class TestRobot extends SubsystemBase {
// TalonFX m_intakeMotor;
// TalonFX m_armMotor;
// TalonFX m_storageMotor;
TalonFX m_outerShooter;
TalonFX m_innerShooter;
ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0);
ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0);
ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0);
ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0);
ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0);
ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0);
VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0);
VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0);
public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second
public enum RobotStare {
IntakeDown,
Idle,
Shoot
}
public RobotStare robotStare = RobotStare.Idle;
public TestRobot(
// TalonFX intakeMotor,
// TalonFX armMotor,
// TalonFX storageMotor,
TalonFX outerShooter,
TalonFX innerShooter
) {
// m_intakeMotor = intakeMotor;
// m_armMotor = armMotor;
// m_storageMotor = storageMotor;
m_outerShooter = outerShooter;
m_innerShooter = innerShooter;
// m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
// m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
// m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
outerVelocity.Slot = 0;
innerVelocity.Slot = 0;
}
// public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true)
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
// public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static Slot0Configs SHOOTER_PID;
static {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = 0.12;
// SHOOTER_PID.kP = 0.11;
// SHOOTER_PID.kI = 0.48;
// SHOOTER_PID.kD = 0.01;
SHOOTER_PID.kP = 0.3;
SHOOTER_PID.kI = 0.0;
SHOOTER_PID.kD = 0.0;
}
ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12);
ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11);
ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48);
ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01);
@Override
public void periodic() {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = kV.get();
SHOOTER_PID.kP = kP.get();
SHOOTER_PID.kI = kI.get();
SHOOTER_PID.kD = kD.get();
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond));
SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond));
switch (robotStare) {
case Idle:
// Move the arm to the up position
PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get());
// m_armMotor.setControl(armPosition);
// // Stop the intake motor
// m_intakeMotor.set(0);
// // Stop the storage motor
// m_storageMotor.set(0);
// Stop the outer shooter motor
m_outerShooter.set(0);
// Stop the inner shooter motor
m_innerShooter.set(0);
break;
case IntakeDown:
// Move the arm to the down sposition
PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get());
// m_armMotor.setControl(armPosition1);
// Move balls into the robot because the arm is down
VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get());
// m_intakeMotor.setControl(intakeVelocity);
// Move balls into the main box
VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get());
// m_storageMotor.setControl(storageVelocity);
// Stop the outer shooter motor
m_outerShooter.set(0);
// Stop the inner shooter motor
m_innerShooter.set(0);
break;
case Shoot:
// Move the arm to the up position
PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get());
// m_armMotor.setControl(armPosition2);
// // Stop the intake motor
// m_intakeMotor.set(0);
// // Move the balls into the
VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get());
// m_storageMotor.setControl(storageVelocity1);
// outerVelocity.
// m_outerShooter.setControl(outerVelocity);
double outerRPM = outerRate.get();
m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
// m_innerShooter.setControl(innerVelocity);
// m_innerShooter.set(innerRate.get());
double innerRPM = innerRate.get();
m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
// SmartDashboard.putNumber("Test", outerRate.get());
break;
default:
break;
}
}
}
src/main/java/frc4388/robot/subsystems/climber/Climber.java
+60
View File
@@ -0,0 +1,60 @@
package frc4388.robot.subsystems.climber;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Climber extends SubsystemBase {
ClimberIO io;
ClimberStateAutoLogged state = new ClimberStateAutoLogged();
// Supplier<Pose2d> m_swervePoseSupplier;
public Climber(
ClimberIO io
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
// public enum ClimberMode {
// Up,
// Down,
// }
// public void setMode(IntakeMode mode) {
// switch (mode) {
// case Up:
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
// io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
// break;
// case Down:
// io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
// io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
// break;
// }
// }
@Override
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Climber", state);
io.updateInputs(state);
}
}
src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java
+74
View File
@@ -0,0 +1,74 @@
package frc4388.robot.subsystems.climber;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.Slot1Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Distance;
import frc4388.utility.status.CanDevice;
public class ClimberConstants {
// Motor conversions
public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.;
//IDs
// public static final CanDevice CLIMBER_ID = new CanDevice("SHOOTER 1", 20);
// Limits
// 0 is the forward angle on the robot.
// negative is left, positive is right
// public static final Angle L1 = Degrees.of(-180);
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0);
public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0);
public static final Slot0Configs CLIMBER_PID = new Slot0Configs()
.withKP(2.0)
.withKI(0.0)
.withKD(10.0);
// 0 is paralell to the ground, 90 is directly up
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// Motor configs
public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
}
// public static final class IDs {
// public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
// }
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java
+43
View File
@@ -0,0 +1,43 @@
package frc4388.robot.subsystems.climber;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Inches;
import org.littletonrobotics.junction.AutoLog;
import edu.wpi.first.units.CurrentUnit;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.LinearVelocity;
public interface ClimberIO {
@AutoLog
public class ClimberState {
Distance climberDistance = Inches.of(0);
Distance climberTargetDistance = Inches.of(0);
Current climberMotorCurrent = Amps.of(0);
// Distance shooterPitch = Rotations.of(0);
// Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0);
// AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
// AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
// Current rollerMotorCurrent = Amps.of(0);
// LinearVelocity feederVelocity = InchesPerSecond.of(0);
// LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
// Current feederMotorCurrent = Amps.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setClimberDistance(ClimberState state, Distance distance) {}
public default void updateInputs(ClimberState state) {}
}
src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java
+78
View File
@@ -0,0 +1,78 @@
package frc4388.robot.subsystems.climber;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.utility.configurable.ConfigurableDouble;
public class ClimberReal implements ClimberIO {
TalonFX m_climberMotor;
public ClimberReal(
TalonFX climberMotor
) {
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_climberMotor = climberMotor;
// Apply the configs
m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID);
m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG);
}
private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){
if(distance.gt(climberLimitUpper)) {
return climberLimitUpper;
}else if(distance.lt(climberLimitLower)) {
return climberLimitLower;
}else{
return distance;
}
}
@Override
public void setClimberDistance(ClimberState state, Distance distance) {
state.climberTargetDistance = distance;
// Assume that the angle is always accurate, because I think we will use a shaft encoder
// Assume that 0 degrees = forwards. Might need an offset here
Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER);
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance);
m_climberMotor.setControl(posRequest);
}
@Override
public void updateInputs(ClimberState state) {
double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations);
double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO;
state.climberDistance = Inches.of(linearInches);
state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue();
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
// state.armAngle = m_armMotor.getPosition().getValue();
// state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
}
}
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+84
View File
@@ -0,0 +1,84 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Rotation;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.status.FaultReporter;
public class Intake extends SubsystemBase {
public IntakeIO io;
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
Supplier<Pose2d> m_swervePoseSupplier;
public Intake(
IntakeIO io
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
public enum IntakeMode {
Extended,
Retracted,
}
private IntakeMode mode = IntakeMode.Extended;
public void setMode(IntakeMode mode) {
this.mode = mode;
}
public IntakeMode getMode() {
return mode;
}
// public enum FieldZone {
// // The robot should aim at the hub
// InShootZone,
// // The robot should aim towards the wall
// AimAtWall,
// }
// // Calculate what should be done based off of the position of the robot
// // TODO: Implement field zones
// public FieldZone getTarget(Pose2d position) {
// return FieldZone.InShootZone;
// }
@Override
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Intake", state);
io.updateInputs(state);
switch (mode) {
case Extended:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
break;
case Retracted:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(0));
break;
}
}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+95
View File
@@ -0,0 +1,95 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.Slot1Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.CanDevice;
public class IntakeConstants {
// Motor conversions
public static final double ARM_MOTOR_GEAR_RATIO = 100;
public static final double ROLLER_MOTOR_GEAR_RATIO = 3;
//IDs
public static final CanDevice ARM_ID = new CanDevice("ARM", 20);
public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21);
// Limits
// 0 is the forward angle on the robot.
// negative is left, positive is right
//when testing the negative output of 10% made the robot put the intake up
// public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3);
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0);
public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
// public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
// public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
public static final Slot0Configs ARM_PID = new Slot0Configs()
.withKP(0.2)
.withKI(0.0)
.withKD(0.0);
public static final Slot0Configs ROLLER_PID = new Slot0Configs()
.withKP(0.2)
.withKI(0.0)
.withKD(0.0);
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2);
public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);
public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2);
public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0);
public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0);
// 0 is paralell to the ground, 90 is directly up
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// Motor configs
public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
}
src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java
+43
View File
@@ -0,0 +1,43 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.AutoLog;
import edu.wpi.first.units.CurrentUnit;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.LinearVelocity;
public interface IntakeIO {
@AutoLog
public class IntakeState {
Angle armAngle = Rotations.of(0);
Angle armTargetAngle = Rotations.of(0);
Current armMotorCurrent = Amps.of(0);
// Angle shooterPitch = Rotations.of(0);
// Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0);
AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
Current rollerMotorCurrent = Amps.of(0);
// LinearVelocity feederVelocity = InchesPerSecond.of(0);
// LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
// Current feederMotorCurrent = Amps.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setArmAngle(IntakeState state, Angle angle) {}
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
public default void updateInputs(IntakeState state) {}
public default void updateGains() {}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+112
View File
@@ -0,0 +1,112 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble;
public class IntakeReal implements IntakeIO {
TalonFX m_armMotor;
TalonFX m_rollerMotor;
PositionDutyCycle armPosition = new PositionDutyCycle(0);
VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0);
public IntakeReal(
TalonFX armMotor,
TalonFX rollerMotor
) {
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_armMotor = armMotor;
m_rollerMotor = rollerMotor;
// Apply the configs
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
armPosition.Slot = 0;
rollerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
if(x.gt(max)) {
return max;
}else if(x.lt(min)) {
return min;
}else{
return x;
}
}
@Override
public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
state.rollerTargetVelocity = angularVelocity;
if(angularVelocity.baseUnitMagnitude() == 0) {
m_rollerMotor.set(0);
return;
}
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
// m_rollerMotor.set(motorSpeed);
// VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed));
}
@Override
public void setArmAngle(IntakeState state, Angle angle) {
state.armTargetAngle = angle;
// Assume that the angle is always accurate, because I think we will use a shaft encoder
// Assume that 0 degrees = forwards. Might need an offset here
// angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED);
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
m_armMotor.setControl(armPosition.withPosition(motorAngle));
}
@Override
public void updateInputs(IntakeState state) {
state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO);
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
}
@Override
public void updateGains() {
IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get();
IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get();
IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get();
m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get();
IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get();
IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get();
m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
}
}
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+130 -20
View File
@@ -1,28 +1,46 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Rotation;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.function.Supplier;
import java.text.FieldPosition;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.status.FaultReporter;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.FieldPositions;
public class Shooter extends SubsystemBase {
ShooterIO io;
public ShooterIO io;
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
Supplier<Pose2d> m_swervePoseSupplier;
SwerveDrive m_SwerveDrive;
Intake m_Intake;
LED m_robotLED;
// Supplier<Pose2d> m_swervePoseSupplier;
public Shooter(
ShooterIO io,
Supplier<Pose2d> swervePoseSupplier
SwerveDrive swerveDrive,
Intake intake,
LED robotLED
) {
this.io = io;
this.m_swervePoseSupplier = swervePoseSupplier;
this.m_SwerveDrive = swerveDrive;
this.m_Intake = intake;
this.m_robotLED = robotLED;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
public enum FieldZone {
@@ -33,26 +51,118 @@ public class Shooter extends SubsystemBase {
}
// Calculate what should be done based off of the position of the robot
// TODO: Implement field zones
public FieldZone getTarget(Pose2d position) {
return FieldZone.InShootZone;
public enum ShooterMode {
// Shooter is actively shooting
Shooting,
// Shooter is going to fire soon
Ready,
// Not ready to shoot
NotReady,
}
private ShooterMode mode = ShooterMode.NotReady;
public void setShooterReady() {
if(this.mode == ShooterMode.NotReady) {
this.mode = ShooterMode.Ready;
}
}
public void setShooterNotReady() {
this.mode = ShooterMode.NotReady;
}
@AutoLogOutput
public ShooterMode getMode() {
return mode;
}
@Override
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Shooter", state);
Pose2d pose = m_swervePoseSupplier.get();
Angle robotRot = pose.getRotation().getMeasure();
io.updateInputs(state);
if(this.mode != ShooterMode.NotReady) {
ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm();
// TODO: get if the robot is within the angle of the hub
boolean driverError =
XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0);
switch (bitmask) {
case 0b000: // No Errors
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b001: // No op err, yes driver err
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
break;
case 0b010: // Bad flywheel, no driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
case 0b011: // Bad flywheel, yes driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
break;
case 0b100: // Bad intake, no driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
break;
case 0b101: // Bad intake, yes driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
break;
case 0b110: // Bad intake and shooter (intake is more important), no driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
break;
case 0b111: // Bad intake and shooter (intake is more important), yes driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
break;
}
// We set the shooter mode to ready if there are no errors
mode = (
bitmask == 0 ?
ShooterMode.Ready :
ShooterMode.NotReady
);
}
switch (mode) {
case Shooting:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get()));
break;
case Ready:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
break;
case NotReady:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
break;
}
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+70 -14
View File
@@ -1,21 +1,74 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.CanDevice;
public class ShooterConstants {
public class ShooterConstants {
// Motor conversions
// public static final double ANGLE_MOTOR_GEAR_RATIO = 1.;
public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
public static final double FLYWHEEL_GEAR_RATIO = 1.;
public static final double FEEDER_INCHES_PER_ROT = 1.;
public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
public static final double INDEXER_GEAR_RATIO = 1.;
// public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30);
// public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15);
// public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
// public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10);
// public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
// Tolerances
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3);
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1);
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.2);
public static Slot0Configs INDEXER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.2);
public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0);
public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0);
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2);
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0);
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
// Limits
@@ -25,8 +78,8 @@ public class ShooterConstants {
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
// 0 is paralell to the ground, 90 is directly up
public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// Motor configs
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -40,21 +93,23 @@ public class ShooterConstants {
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
public static final class IDs {
public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
}
public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration()
public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
@@ -64,7 +119,8 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration()
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+18 -8
View File
@@ -24,19 +24,29 @@ public interface ShooterIO {
// Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0);
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
Current flywheelMotorCurrent = Amps.of(0);
AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0);
AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0);
LinearVelocity feederVelocity = InchesPerSecond.of(0);
LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
Current feederMotorCurrent = Amps.of(0);
AngularVelocity motor1Velocity = RotationsPerSecond.of(0);
AngularVelocity motor2Velocity = RotationsPerSecond.of(0);
AngularVelocity indexerVelocity = RotationsPerSecond.of(0);
Current motor1Current = Amps.of(0);
Current motor2Current = Amps.of(0);
Current indexerCurrent = Amps.of(0);
// LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void updateInputs(ShooterState state) {}
public default void updateGains() {}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+77 -80
View File
@@ -1,111 +1,108 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.*;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.utility.configurable.ConfigurableDouble;
public class ShooterReal implements ShooterIO {
// TalonFX m_angleMotor;
// TalonFX m_pitchMotor;
TalonFX m_flywheelMotor;
TalonFX m_feederMotor;
TalonFX m_shooter1Motor;
TalonFX m_shooter2Motor;
TalonFX m_indexerMotor;
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
public ShooterReal(
// TalonFX angleMotor,
// TalonFX pitchMotor,
TalonFX flywheelMotor,
TalonFX feederMotor
TalonFX shooter1Motor,
TalonFX shooter2Motor,
TalonFX indexerMotor
) {
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_flywheelMotor = flywheelMotor;
m_feederMotor = feederMotor;
m_shooter1Motor = shooter1Motor;
m_shooter2Motor = shooter2Motor;
m_indexerMotor = indexerMotor;
// Apply the configs
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
shooter1Velocity.Slot = 0;
shooter2Velocity.Slot = 0;
m_indexerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
if(x.gt(max)) {
return max;
}else if(x.lt(min)) {
return min;
}else{
return x;
}
}
// // TODO: Test
// @Override
// public void setShooterAngle(ShooterState state, Angle angle) {
// state.shooterTargetAngle = angle;
// // Assume that the angle is always accurate, because I think we will use a shaft encoder
// // Assume that 0 degrees = forwards. Might need an offset here
// Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_angleMotor.setControl(posRequest);
// }
// TODO: Test
// @Override
// public void setShooterPitch(ShooterState state, Angle angle) {
// state.shooterTargetPitch = angle;
// // TODO: Test
// // This assumes that the 0 is paralell to the ground. Might need an offset here
// Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_pitchMotor.setControl(posRequest);
// }
@Override
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
state.flywheelTargetVelocity = angularVelocity;
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO;
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velocity);
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
state.motor1TargetVelocity = target;
state.motor2TargetVelocity = target;
if(target.baseUnitMagnitude() == 0) {
m_shooter1Motor.set(0);
m_shooter2Motor.set(0);
return;
}
AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO);
m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps));
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
}
@Override
public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {
state.feederTargetVelocity = linearVelocity;
// (IN / SEC) * (ROT / IN) = (ROT / SEC)
double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT;
VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velRequest);
public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
state.indexerTargetVelocity = target;
if(target.baseUnitMagnitude() == 0) {
m_indexerMotor.set(0);
return;
}
AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO);
m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps));
}
@Override
public void updateInputs(ShooterState state) {
// state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO);
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO);
state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
// state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue();
state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
}
@Override
public void updateGains() {
// TEMPORARY PIDs
ShooterConstants.SHOOTER_PID.kP = ShooterConstants.shooter_kP.get();
ShooterConstants.SHOOTER_PID.kI = ShooterConstants.shooter_kI.get();
ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get();
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get();
ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get();
ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get();
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
}
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+74 -15
View File
@@ -10,6 +10,12 @@ import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -19,15 +25,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.status.Status;
import frc4388.utility.status.FaultReporter;
import frc4388.utility.status.Queryable;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.util.PathPlannerLogging;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.RobotConfig;
import frc4388.utility.status.Status;
public class SwerveDrive extends SubsystemBase implements Queryable {
// private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
@@ -138,6 +138,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
initalPose2d = pose;
io.resetPose(pose);
}
// MIRA public void setOdoPose(Pose2d pose) {
// if (pose == null) return;
// io.tareEverything();
// initalPose2d = pose;
// io.resetPose(pose);
// robotKnowsWhereItIs = true;
// rotTarget = pose.getRotation().getDegrees();
// }
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
// Translation2d rightStick){
@@ -251,13 +260,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
io.setControl(ctrl);
}
// Drive with a specific velocity and heading
public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) {
if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going:
stopModules(); // stop the swerve
// if (leftStick.getNorm() < 0.05) // if no imput
// return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
@@ -268,13 +275,65 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
SwerveDriveConstants.PIDConstants.AIM_kD.get()
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kP,
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kI,
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kD
);
io.setControl(ctrl);
// SmartDashboard.putBoolean("drift correction", true);
}
public void driveIntake(Translation2d leftStick, boolean invertRotation){
// if (invert){
// Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY());
// driveFieldAngle(stick, heading);
// } else{
// driveFieldAngle(leftStick, heading);
// }
double speed = leftStick.getNorm();
if(speed < 0.3) {
driveWithInput(leftStick, new Translation2d(), true);
} else {
Rotation2d heading = new Rotation2d(leftStick.getX(), -leftStick.getY());//.r otateBy(Rotation2d.fromDegrees(90));
// if (invertRotation){
heading = heading.rotateBy(Rotation2d.fromDegrees(270));
// }
// Only drive forward in robot direction (no strafe)
// Translation2d forwardOnly = new Translation2d(speed, 0.0);
driveFieldAngle(leftStick, heading);
}
}
// Drive with the robot facing towards a specific position
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
// Get the current speed of the robot
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
// Calculate a point to aim ahead of the actual position.
Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos);
// Calculate the angle between the current position and the lead position
Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle();
driveFieldAngle(leftStick, ang);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+18 -5
View File
@@ -22,6 +22,8 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.Angle;
import frc4388.robot.constants.Constants;
import frc4388.utility.configurable.ConfigurableDouble;
//import edu.wpi.first.units.measure.measure.Distance;
import frc4388.utility.status.CanDevice;
import frc4388.utility.structs.Gains;
@@ -37,6 +39,9 @@ public final class SwerveDriveConstants {
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
// TODO: Replace with a constant
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
@@ -75,7 +80,7 @@ public final class SwerveDriveConstants {
private static final class ModuleSpecificConstants { //2025
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -83,7 +88,7 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -91,7 +96,7 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -99,7 +104,7 @@ public final class SwerveDriveConstants {
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
@@ -156,6 +161,14 @@ public final class SwerveDriveConstants {
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
// TODO: Replace this with a static constant
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 15);
public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
// public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
}
public static final class Configurations {
@@ -196,7 +209,7 @@ public final class SwerveDriveConstants {
}
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withPigeon2Id(IDs.DRIVE_PIGEON.id);
.withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName());
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
src/main/java/frc4388/robot/subsystems/vision/Vision.java
+15 -6
View File
@@ -2,15 +2,11 @@ package frc4388.robot.subsystems.vision;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -18,20 +14,22 @@ import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
import frc4388.utility.status.FaultReporter;
import frc4388.utility.status.Queryable;
import frc4388.utility.status.Status;
import frc4388.utility.structs.LEDPatterns;
public class Vision extends SubsystemBase implements Queryable {
public class Vision extends SubsystemBase implements Queryable{
VisionIO[] io;
VisionStateAutoLogged[] state;
public Pose2d lastVisionPose = new Pose2d();
public Pose2d lastPhysOdomPose = new Pose2d();
public LED m_robotLED;
public Vision(VisionIO... devices) {
FaultReporter.register(this);
io = devices;
state = new VisionStateAutoLogged[io.length];
m_robotLED = new LED();
for(int i = 0; i < io.length; i++) {
state[i] = new VisionStateAutoLogged();
}
@@ -43,6 +41,11 @@ public class Vision extends SubsystemBase implements Queryable {
io[i].updateInputs(state[i]);
Logger.processInputs("Vision/Camera" + i , state[i]);
}
Logger.recordOutput("Vision/isTagDectected", isTag());
if (isTag()){
m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK);
}
}
public List<PoseObservation> getPosesToAdd(){
@@ -92,4 +95,10 @@ public class Vision extends SubsystemBase implements Queryable {
// throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
}
// Simple LED helper class for compilation and basic usage; replace with real implementation if available.
private static class LED {
public void setMode(LEDPatterns mode) {
// no-op stub for compilation; integrate with hardware driver as needed
}
}
}
src/main/java/frc4388/robot/subsystems/vision/VisionReal.java
+3 -3
View File
@@ -8,7 +8,7 @@ import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonPipelineResult;
import frc4388.robot.constants.Constants.FieldConstants;
import frc4388.robot.constants.FieldConstants;
import frc4388.robot.constants.Constants.VisionConstants;
public class VisionReal implements VisionIO {
@@ -45,12 +45,12 @@ public class VisionReal implements VisionIO {
var result = results.get(results.size()-1);
state.isTagDetected = state.isTagDetected | result.hasTargets();
// If there are no tags
if(!result.hasTargets())
return;
state.isTagDetected = state.isTagDetected | result.hasTargets();
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
// If the tag was failed to be processed