From bd43699921d8559e790eeb55120ed0eb15eaa6b8 Mon Sep 17 00:00:00 2001 From: shrubman8787 <138845668+shrubman8787@users.noreply.github.com> Date: Tue, 11 Feb 2025 17:20:23 +0800 Subject: [PATCH 1/3] added state machine --- src/main/java/frc/robot/Robot.java | 29 +- src/main/java/frc/robot/RobotContainer.java | 40 +++ .../frc/robot/commands/TeleopSuperStruct.java | 43 +++ .../java/frc/robot/states/StateMachine.java | 29 ++ .../frc/robot/states/SuperStructState.java | 15 + .../frc/robot/subsystems/AlgaeIntake.java | 174 ++++++++++++ .../{ElevatorSubsystem.java => Elevator.java} | 128 +++++++-- .../java/frc/robot/subsystems/Grabber.java | 267 +++++------------- .../java/frc/robot/subsystems/Intake.java | 162 +++++++++++ .../frc/robot/subsystems/SuperStruct.java | 229 +++++++++++++++ 10 files changed, 890 insertions(+), 226 deletions(-) create mode 100644 src/main/java/frc/robot/RobotContainer.java create mode 100644 src/main/java/frc/robot/commands/TeleopSuperStruct.java create mode 100644 src/main/java/frc/robot/states/StateMachine.java create mode 100644 src/main/java/frc/robot/states/SuperStructState.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaeIntake.java rename src/main/java/frc/robot/subsystems/{ElevatorSubsystem.java => Elevator.java} (70%) create mode 100644 src/main/java/frc/robot/subsystems/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/SuperStruct.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 97207ec..bcc721c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,10 +15,15 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.SuperStruct; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.TeleopSuperStruct; import java.util.Map; @@ -34,6 +39,10 @@ public class Robot extends TimedRobot { private final SendableChooser m_chooser = new SendableChooser<>(); private final Grabber m_grabber = new Grabber(); + private final Intake m_intake = new Intake(); + private final Elevator m_elevator = new Elevator(); + private final SuperStruct m_superStruct = SuperStruct.getInstance(); + private Command m_teleopSuperStruct; /** * This function is run when the robot is first started up and should be used for any @@ -48,7 +57,10 @@ public Robot() { } - + @Override + public void robotInit() { + m_teleopSuperStruct = new TeleopSuperStruct(m_superStruct); + } /** * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics @@ -59,7 +71,10 @@ public Robot() { */ @Override public void robotPeriodic() { - m_grabber.periodic(); // Subsystem handles everything now! + m_grabber.periodic(); + m_intake.periodic(); + m_elevator.periodic(); + m_superStruct.periodic(); } /** @@ -95,7 +110,15 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + public void teleopInit() { + m_teleopSuperStruct.schedule(); + } + + /** This function is called once when teleop is disabled. */ + @Override + public void teleopExit() { + m_teleopSuperStruct.cancel(); + } /** This function is called periodically during operator control. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..46fd5b5 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,40 @@ +package frc.robot; + + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Grabber; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.SuperStruct; +public class RobotContainer { + + private CommandXboxController m_Controller; + private Joystick joystick; + + private final Elevator m_elevator; + private final Grabber m_grabber; + private final Intake m_intake; + private final SuperStruct m_SuperStruct; + + public RobotContainer() { + m_elevator = Elevator.getInstance(); + m_grabber = Grabber.getInstance(); + m_intake = Intake.getInstance(); + m_SuperStruct = SuperStruct.getInstance(); + + configureButtonBindings(); + } + + private void configureButtonBindings() { + } + + public Command getAutonomousCommand() { + return null; + } + + +} diff --git a/src/main/java/frc/robot/commands/TeleopSuperStruct.java b/src/main/java/frc/robot/commands/TeleopSuperStruct.java new file mode 100644 index 0000000..b9969e6 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopSuperStruct.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.states.SuperStructState; +import frc.robot.subsystems.SuperStruct; + +public class TeleopSuperStruct extends Command { + private final SuperStruct m_superStruct; + + + public TeleopSuperStruct(SuperStruct superStruct) { + m_superStruct = superStruct; + addRequirements(superStruct); + } + + // Called when the command is initially scheduled + @Override + public void initialize() { + // Set to default state when starting teleop + m_superStruct.setState(SuperStructState.DEFAULT); + } + + // Called every time the scheduler runs while the command is scheduled + @Override + public void execute() { + + m_superStruct.handleControllerInput(); + } + + // Called once the command ends or is interrupted + @Override + public void end(boolean interrupted) { + // Return to safe default state when ending + m_superStruct.setState(SuperStructState.DEFAULT); + } + + // Returns true when the command should end + @Override + public boolean isFinished() { + + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/states/StateMachine.java b/src/main/java/frc/robot/states/StateMachine.java new file mode 100644 index 0000000..63eb1fd --- /dev/null +++ b/src/main/java/frc/robot/states/StateMachine.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.states; + +/** Add your docs here. */ +public class StateMachine { + private static StateMachine instance = null; + + private SuperStructState commandedState = SuperStructState.TRAVEL; + + public StateMachine() {} + + public static synchronized StateMachine getInstance() { + if (instance == null) { + instance = new StateMachine(); + } + return instance; + } + + public void setCommandedState(SuperStructState targetState) { + commandedState = targetState; + } + + public SuperStructState getCommandedState() { + return commandedState; + } +} diff --git a/src/main/java/frc/robot/states/SuperStructState.java b/src/main/java/frc/robot/states/SuperStructState.java new file mode 100644 index 0000000..dec5794 --- /dev/null +++ b/src/main/java/frc/robot/states/SuperStructState.java @@ -0,0 +1,15 @@ +package frc.robot.states; + +public enum SuperStructState { + L1, // + L2, // + L3, // + L4, // + TRAVEL, + CS, // CORAL STATION + PLACEMENT, + DEFAULT, + ALGAE_STOWAGE, + ALGAE_INTAKE, + ALGAE_PLACEMENT +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java new file mode 100644 index 0000000..58a4ec2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -0,0 +1,174 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class AlgaeIntake extends SubsystemBase { + private final SparkMax leftAngle; + private final SparkMax rightAngle; + private final CANcoder angleEncoder; + private final SparkMax intakeMotor; + private final PIDController anglePIDController; + private static final double ANGLE_kP = 0.01; // Adjust these PID values! uwu + private static final double ANGLE_kI = 0.0; + private static final double ANGLE_kD = 0.0; + private static final double ANGLE_TOLERANCE = 2.0; // Degrees + + private static AlgaeIntake mInstance = null; + + public static AlgaeIntake getInstance() { + if (mInstance == null) { + mInstance = new AlgaeIntake(); + } + return mInstance; + } + + /** Creates a new AlgaeIntake. */ + public AlgaeIntake() { + leftAngle = new SparkMax(10, MotorType.kBrushless); + rightAngle = new SparkMax(11, MotorType.kBrushless); + angleEncoder = new CANcoder(12, "GTX7130"); + intakeMotor = new SparkMax(13, MotorType.kBrushless); + + configureNEO(leftAngle, false, true); + configureSlaveNEO(rightAngle, false, true); + configureNEO(intakeMotor, false, false); + configureCANcoder(); + + // Initialize angle PID controller + anglePIDController = new PIDController(ANGLE_kP, ANGLE_kI, ANGLE_kD); + anglePIDController.setTolerance(ANGLE_TOLERANCE); + } + + public void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { + SoftLimitConfig softLimitConfig = new SoftLimitConfig(); + softLimitConfig + .forwardSoftLimit(0.0) + .forwardSoftLimitEnabled(softlimit) + .reverseSoftLimit(0) + .reverseSoftLimitEnabled(softlimit); + + SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig + .smartCurrentLimit(40) + .idleMode(IdleMode.kBrake) + .voltageCompensation(12) + .inverted(inverted) + .apply(softLimitConfig); + + motor.setCANTimeout(250); + motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void configureSlaveNEO(SparkMax motor, boolean inverted, boolean softlimit) { + SoftLimitConfig softLimitConfig = new SoftLimitConfig(); + softLimitConfig + .forwardSoftLimit(0.0) //45:1 gear reduction + .forwardSoftLimitEnabled(softlimit) + .reverseSoftLimit(0) + .reverseSoftLimitEnabled(softlimit); + + SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig + .smartCurrentLimit(40) + .idleMode(IdleMode.kBrake) + .voltageCompensation(12) + .inverted(inverted) + .apply(softLimitConfig) + .follow(10); + + motor.setCANTimeout(250); + motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + private void configureCANcoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + // Configure for absolute position mode + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + + // Apply configuration + angleEncoder.getConfigurator().apply(config); + + } + + /** + * Sets the angle of the intake using CANcoder feedback (✿◠‿◠) + * @param targetAngleDegrees The desired angle in degrees + */ + public void setAngle(double targetAngleDegrees) { + // Update PID setpoint + anglePIDController.setSetpoint(targetAngleDegrees); + + // Get current angle from CANcoder + double currentAngle = angleEncoder.getAbsolutePosition().getValueAsDouble(); + + // Calculate PID output + double pidOutput = anglePIDController.calculate(currentAngle); + + // Limit output for safety ʕ•ᴥ•ʔ + pidOutput = MathUtil.clamp(pidOutput, -0.4, 0.4); + + // Set motor outputs (both left and right angle motors) + leftAngle.set(pidOutput); + + } + + /** + * Checks if intake is at the target angle (◕ᴗ◕✿) + * @return true if at target angle, false otherwise + */ + public boolean isAtAngle() { + return anglePIDController.atSetpoint(); + } + + /** + * Gets the current angle from CANcoder uwu + * @return current angle in degrees + */ + public double getCurrentAngle() { + return angleEncoder.getAbsolutePosition().getValueAsDouble() * 360.0; + } + + public void intake() { + intakeMotor.set(0.2); + } + + public void outgae() { //get it? + intakeMotor.set(-0.2); + } + + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + // Add angle info to dashboard + SmartDashboard.putNumber("Algae Current Angle", getCurrentAngle()); + SmartDashboard.putNumber("Algae Target Angle", anglePIDController.getSetpoint()); + SmartDashboard.putBoolean("Algae At Target Angle", isAtAngle()); + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/Elevator.java similarity index 70% rename from src/main/java/frc/robot/subsystems/ElevatorSubsystem.java rename to src/main/java/frc/robot/subsystems/Elevator.java index dc3e583..661282c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -15,7 +15,11 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; + import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -23,15 +27,26 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.math.controller.PIDController; +import java.util.Map; +import frc.robot.states.SuperStructState; -public class ElevatorSubsystem extends SubsystemBase { +public class Elevator extends SubsystemBase { private final SparkMax leftMotor; private final SparkMax rightMotor; - // private final CANcoder encoder; + private final CANcoder Cancoder; private static final double kG = 0.05; // Gravity feed forward constant - adjust this! private static final double kDownSpeedMultiplier = 0.5; // Reduces down speed - adjust this! + private static Elevator mInstance = null; + + public static Elevator getInstance() { + if (mInstance == null) { + mInstance = new Elevator(); + } + return mInstance; + } + // Shuffleboard entries private final ShuffleboardTab elevatorTab = Shuffleboard.getTab("Elevator"); private final GenericEntry upButton = elevatorTab.add("Elevator Up", false) @@ -58,18 +73,21 @@ public class ElevatorSubsystem extends SubsystemBase { private final GenericEntry maxLeftRotationsEntry; private final GenericEntry maxRightRotationsEntry; - - private final PIDController pidController = new PIDController(0.07, 0.035, 0.007); // Adjust P, I, D values! private boolean positionLocked = false; private double targetPosition = 0.0; private final GenericEntry lockButton; + + private final CANcoder elevatorCoder; + private static final double CANCODER_OFFSET = 0.0; // Adjust this value! uwu + private final GenericEntry absolutePositionEntry; + /** Creates a new ElevatorSubsystem. */ - public ElevatorSubsystem() { + public Elevator() { leftMotor = new SparkMax(4, MotorType.kBrushless); // Update ID as needed rightMotor = new SparkMax(5, MotorType.kBrushless); // Update ID as needed - // encoder = new CANcoder(3); // Update CANcoder ID as needed! + Cancoder = new CANcoder(3); // Update CANcoder ID as needed! configureNEO(leftMotor, false); // Invert both motors to make default direction clockwise configureNEO(rightMotor, false); // Both motors still turn the same way @@ -109,8 +127,15 @@ public ElevatorSubsystem() { .withSize(1, 1) .getEntry(); - - // or + // Initialize CANcoder (ID 3 from your existing code) + elevatorCoder = new CANcoder(3); + configureCANcoder(); + + // Add absolute position display to dashboard + absolutePositionEntry = elevatorTab.add("Elevator Absolute Position", 0.0) + .withPosition(0, 7) + .withSize(2, 1) + .getEntry(); } private void configureNEO(SparkMax motor, boolean inverted) { @@ -142,6 +167,62 @@ private void configureNEO(SparkMax motor, boolean inverted) { motor.getEncoder().setPosition(0.0); // Reset encoder to zero } + /** + * Configure the CANcoder for absolute position reading (◕ᴗ◕✿) + */ + private void configureCANcoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + // Configure for absolute position mode + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + + // Apply configuration + elevatorCoder.getConfigurator().apply(config); + + // Wait for config to apply + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + + /** + * Get the absolute position from CANcoder! (✿◠‿◠) + */ + public double getPosition() { + return Cancoder.getPosition().getValueAsDouble(); + } + + public void setPosition(double targetPosition) { + // Update PID setpoint + pidController.setSetpoint(targetPosition); + + // Calculate PID output + double currentPosition = leftMotor.getEncoder().getPosition(); + double pidOutput = pidController.calculate(currentPosition); + + // Add gravity compensation + double gravityCompensation = (pidOutput >= 0) ? kG : 0.0; + + // Set motor output + setElevatorSpeed(pidOutput + gravityCompensation); +} + +public boolean isAtPosition() { + double currentPosition = leftMotor.getEncoder().getPosition(); + return Math.abs(currentPosition - pidController.getSetpoint()) < 0.1; +} + /** + * Reset the NEO encoders based on CANcoder position ʕ•ᴥ•ʔ + */ + public void resetToAbsolute() { + double absolutePosition = getPosition(); + leftMotor.getEncoder().setPosition(absolutePosition); + rightMotor.getEncoder().setPosition(absolutePosition); + } + /** * Sets the elevator speed. Positive values move up, negative values move down. * Includes gravity compensation when moving up and speed reduction when moving down! ^w^ @@ -190,16 +271,11 @@ public void unlockPosition() { positionLocked = false; } + + @Override public void periodic() { - // Check for negative position and reset if needed - if (leftMotor.getEncoder().getPosition() < 0) { - leftMotor.getEncoder().setPosition(0.0); - } - if (rightMotor.getEncoder().getPosition() < 0) { - rightMotor.getEncoder().setPosition(0.0); - } - + // Handle position locking if (lockButton.getBoolean(false)) { if (!positionLocked) { @@ -223,13 +299,6 @@ public void periodic() { } } - // Update values instead of creating new widgets - speedEntry.setDouble(leftMotor.get()); - // positionEntry.setDouble(encoder.getPosition().getValueAsDouble()); //cancoder - positionEntry.setDouble(leftMotor.getEncoder().getPosition()); // encoder - leftRotationsEntry.setDouble(leftMotor.getEncoder().getPosition()); - rightRotationsEntry.setDouble(rightMotor.getEncoder().getPosition()); - // Track maximum rotations double leftRotations = Math.abs(leftMotor.getEncoder().getPosition()); double rightRotations = Math.abs(rightMotor.getEncoder().getPosition()); @@ -240,5 +309,18 @@ public void periodic() { // Update max rotation displays maxLeftRotationsEntry.setDouble(maxLeftRotations); maxRightRotationsEntry.setDouble(maxRightRotations); + + // State machine control (✿◠‿◠) + double currentHeight = leftMotor.getEncoder().getPosition(); + double pidOutput = pidController.calculate(currentHeight); + + // Add gravity compensation! ˎ₍•ʚ•₎ˏ + double gravityCompensation = kG; + + setElevatorSpeed(pidOutput + gravityCompensation); + + + // Update absolute position display + absolutePositionEntry.setDouble(getPosition()); } } diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index 1756ae2..802b5cd 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -15,230 +15,96 @@ import java.util.Map; import edu.wpi.first.math.controller.PIDController; + public class Grabber extends SubsystemBase { - private final SparkMax up; - private final SparkMax down; private final SparkMax angle; - // private final CANCoder angleEncoder; private static final double DEFAULT_KG = 0.003; - private int startupCounter = 0; - private int stallCounter = 0; - // Shuffleboard entries + private static Grabber mInstance = null; + + public static Grabber getInstance() { + if (mInstance == null) { + mInstance = new Grabber(); + } + return mInstance; + } + + // Shuffleboard entries for angle control only private final ShuffleboardTab grabberTab = Shuffleboard.getTab("Grabber"); - // private final GenericEntry upButton, downButton, upSpeed, downSpeed; - // private final GenericEntry forwardButton, backwardButton, flatButton; - // private final GenericEntry angleDisplay, kGTuner; - // private final GenericEntry bothInButton, bothOutButton; - // private final GenericEntry upRPMStatus, downRPMStatus; - - // private final ShuffleboardTab motorTab = Shuffleboard.getTab("Motor Controls"); - private final GenericEntry upButton = grabberTab.add("Up Motor", false) - .withWidget("Toggle Button") - .withPosition(0, 0) - .getEntry(); - private final GenericEntry downButton = grabberTab.add("Down Motor", false) - .withWidget("Toggle Button") - .withPosition(0, 1) - .getEntry(); - private final GenericEntry upSpeed = grabberTab.add("Up Motor Speed", 0.5) - .withWidget("Number Slider") - .withProperties(Map.of("min", -1.0, "max", 1.0)) - .withPosition(1, 0) - .getEntry(); - private final GenericEntry downSpeed = grabberTab.add("Down Motor Speed", 0.5) - .withWidget("Number Slider") - .withProperties(Map.of("min", -1.0, "max", 1.0)) - .withPosition(1, 1) - .getEntry(); - - // Add preset angle buttons to Shuffleboard - private final GenericEntry forwardButton = grabberTab.add("Turn 45 Forward", false) - .withWidget("Toggle Button") - .withPosition(0, 4) - .withSize(1, 1) - .getEntry(); - private final GenericEntry backwardButton = grabberTab.add("Turn 45 Back", false) - .withWidget("Toggle Button") - .withPosition(1, 4) - .withSize(1, 1) - .getEntry(); - - private final GenericEntry flatButton = grabberTab.add("Flat", false) - .withWidget("Toggle Button") - .withPosition(2, 4) - .withSize(1, 1) - .getEntry(); - - // Add angle display widget - private final GenericEntry angleDisplay = grabberTab.add("Current Angle", 0.0) - .withWidget("Text View") // Shows number clearly UwU - .withPosition(2, 4) - .withSize(1, 1) - .getEntry(); - - // Add kG tuning slider ✨ - private final GenericEntry kGTuner = grabberTab.add("Gravity Compensation", DEFAULT_KG) - .withWidget("Number Slider") - .withProperties(Map.of("min", 0.0, "max", 0.2)) // Reasonable range - .withPosition(3, 4) - .withSize(1, 1) - .getEntry(); - - // Add buttons for synchronized movement ✨ - private final GenericEntry bothInButton = grabberTab.add("Both Motors In", false) - .withWidget("Toggle Button") - .withPosition(0, 5) - .withSize(1, 1) - .getEntry(); - private final GenericEntry bothOutButton = grabberTab.add("Both Motors Out", false) - .withWidget("Toggle Button") - .withPosition(1, 5) - .withSize(1, 1) - .getEntry(); - - // Add RPM monitoring widgets ✨ - private final GenericEntry upRPMStatus = grabberTab.add("Up Motor RPM OK", true) - .withWidget("Boolean Box") // Shows green/red status - .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) - .withPosition(0, 6) - .withSize(1, 1) - .getEntry(); - - private final GenericEntry downRPMStatus = grabberTab.add("Down Motor RPM OK", true) - .withWidget("Boolean Box") - .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) - .withPosition(1, 6) - .withSize(1, 1) - .getEntry(); - - private final PIDController pidController = new PIDController( - 0.07, // kP - 0.035, // kI - helps eliminate steady-state error UwU - 0.007 // kD - reduces overshoot and oscillation ✨ - ); + private final GenericEntry angleDisplay; + private final GenericEntry kGTuner; + private final GenericEntry stateDisplay; + private final PIDController pidController; public Grabber() { - up = new SparkMax(2, MotorType.kBrushless); - down = new SparkMax(3, MotorType.kBrushless); angle = new SparkMax(1, MotorType.kBrushless); - // angleEncoder = angle.getEncoder(); - - configureNEO550(up); - configureNEO550(down); - configureNEO(angle); - + configureNEO(angle, false, true); angle.getEncoder().setPosition(0.0); - + // Initialize Shuffleboard entries + angleDisplay = grabberTab.add("Current Angle", 0.0) + .withWidget("Text View") + .withPosition(2, 4) + .getEntry(); + + kGTuner = grabberTab.add("Gravity Compensation", DEFAULT_KG) + .withWidget("Number Slider") + .withProperties(Map.of("min", 0.0, "max", 0.2)) + .withPosition(3, 4) + .getEntry(); + + stateDisplay = grabberTab.add("Current State", "DEFAULT") + .withWidget("Text View") + .withPosition(2, 0) + .getEntry(); + + pidController = new PIDController(0.07, 0.035, 0.007); } - @Override - public void periodic() { - double kG = kGTuner.getDouble(DEFAULT_KG); - - if (forwardButton.getBoolean(false)) { - angle.set(0.07 + kG); // Now positive is forward - if (angle.getEncoder().getPosition() >= 2.76) { // For 53° at 18.75:1 - angle.set(kG); + /** + * Sets the grabber to a specific angle (✿◠‿◠) + * @param targetAngleDegrees The desired angle in degrees + */ + public void setAngle(double targetAngleDegrees) { + // Convert degrees to motor rotations (18.75:1 gear ratio) + double targetRotations = targetAngleDegrees * (18.75 / 360.0); - } - - } else if (backwardButton.getBoolean(false)) { - angle.set(-0.025); // Now negative is backward - if (angle.getEncoder().getPosition() <= 0) { - angle.set(0); - } - } else if (flatButton.getBoolean(false)) { - angle.set(0.07 + kG); // Move forward - if (angle.getEncoder().getPosition() >= 1.71875) { // 33° position - angle.set(kG); - } - } else { - angle.set(kG * 0.5); // Flipped kG direction too! + // Update PID setpoint + pidController.setSetpoint(targetRotations); } - // Get button states and speeds fow motow contwol OwO - boolean upButtonState = upButton.getBoolean(false); - boolean downButtonState = downButton.getBoolean(false); - - double upSpeedValue = upSpeed.getDouble(0.5); - double downSpeedValue = downSpeed.getDouble(0.5); - - up.set(upButtonState ? upSpeedValue : 0); - down.set(downButtonState ? downSpeedValue : 0); - - // Update angle display with new ratio - double currentAngleDegrees = angle.getEncoder().getPosition() * (360.0 / 18.75); // Convert from rotations - angleDisplay.setDouble(currentAngleDegrees); - - // Handle synchronized motor control - if (bothInButton.getBoolean(false)) { - up.set(0.3); // Up motor forward - down.set(0.15); // Down motor reverse - } else if (bothOutButton.getBoolean(false)) { - double upRPM = Math.abs(up.getEncoder().getVelocity()); - double downRPM = Math.abs(down.getEncoder().getVelocity()); - - if (startupCounter < 10) { // Startup delay - up.set(-0.4); - down.set(-0.4); - startupCounter++; - } else if (upRPM < 50 || downRPM < 50) { - if (stallCounter < 50) { // Wait ~0.5 seconds (25 * 20ms) before stopping - stallCounter++; - up.set(-0.4); - down.set(-0.4); - } else { - up.set(0); - down.set(0); - bothOutButton.setBoolean(false); - startupCounter = 0; - stallCounter = 0; - } - } else { - stallCounter = 0; // Reset stall counter if RPM is good - up.set(-0.4); - down.set(-0.4); - } - } else { - startupCounter = 0; // Reset both counters when button released - stallCounter = 0; + /** + * Checks if the grabber is at the target angle (◕ᴗ◕✿) + * @return true if at target angle, false otherwise + */ + public boolean isAtTargetAngle() { + double currentAngle = angle.getEncoder().getPosition(); + return Math.abs(currentAngle - pidController.getSetpoint()) < 0.1; } - // Check RPM and update status - double upRPM = Math.abs(up.getEncoder().getVelocity()); - double downRPM = Math.abs(down.getEncoder().getVelocity()); - - SmartDashboard.putNumber("rpm", up.getEncoder().getVelocity()); - // ... (rest of your control logic) - } + @Override + public void periodic() { + double currentAngle = angle.getEncoder().getPosition(); + double kG = kGTuner.getDouble(DEFAULT_KG); - // Copy your configuration methods - private void configureNEO550(SparkMax motor) { - SparkMaxConfig neo550Config = new SparkMaxConfig(); - - neo550Config - .smartCurrentLimit(40) // Protect our smol motors! >w< - .idleMode(IdleMode.kCoast) // Better control! - .voltageCompensation(12.0) // Stable performance! UwU - .openLoopRampRate(0.1); // Smooth acceleration! - - // Apply our configuration with proper timeout - motor.setCANTimeout(250); - motor.configure(neo550Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // Use PID to maintain target angle + angle.set(pidController.calculate(currentAngle) + kG); + + // Update displays + double currentAngleDegrees = angle.getEncoder().getPosition() * (360.0 / 18.75); + angleDisplay.setDouble(currentAngleDegrees); } - private void configureNEO(SparkMax motor) { + private void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { SparkMaxConfig neoConfig = new SparkMaxConfig(); - + // Create soft limit config SoftLimitConfig softLimitConfig = new SoftLimitConfig(); softLimitConfig .forwardSoftLimit(2.76) // +53 degrees with 18.75:1 - .forwardSoftLimitEnabled(true) + .forwardSoftLimitEnabled(softlimit) .reverseSoftLimit(0) // Keep starting point - .reverseSoftLimitEnabled(true); + .reverseSoftLimitEnabled(softlimit); neoConfig .smartCurrentLimit(40) @@ -246,8 +112,9 @@ private void configureNEO(SparkMax motor) { .voltageCompensation(12.0) .openLoopRampRate(0.1) .apply(softLimitConfig) - .inverted(false); // Flips motor direction - + .inverted(inverted); // Flips motor direction + + motor.setCANTimeout(250); motor.configure(neoConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..5658a32 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,162 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.wpilibj.shuffleboard.*; +import edu.wpi.first.networktables.GenericEntry; +import java.util.Map; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Intake extends SubsystemBase { + private final SparkMax up; + private final SparkMax down; + private int startupCounter = 0; + private int stallCounter = 0; + + private static Intake mInstance = null; + + public static Intake getInstance() { + if (mInstance == null) { + mInstance = new Intake(); + } + return mInstance; + } + + // Shuffleboard entries + private final ShuffleboardTab intakeTab = Shuffleboard.getTab("Intake"); + private final GenericEntry upButton = intakeTab.add("Up Motor", false) + .withWidget("Toggle Button") + .withPosition(0, 0) + .getEntry(); + private final GenericEntry downButton = intakeTab.add("Down Motor", false) + .withWidget("Toggle Button") + .withPosition(0, 1) + .getEntry(); + private final GenericEntry upSpeed = intakeTab.add("Up Motor Speed", 0.5) + .withWidget("Number Slider") + .withProperties(Map.of("min", -1.0, "max", 1.0)) + .withPosition(1, 0) + .getEntry(); + private final GenericEntry downSpeed = intakeTab.add("Down Motor Speed", 0.5) + .withWidget("Number Slider") + .withProperties(Map.of("min", -1.0, "max", 1.0)) + .withPosition(1, 1) + .getEntry(); + private final GenericEntry bothInButton = intakeTab.add("Both Motors In", false) + .withWidget("Toggle Button") + .withPosition(0, 5) + .getEntry(); + private final GenericEntry bothOutButton = intakeTab.add("Both Motors Out", false) + .withWidget("Toggle Button") + .withPosition(1, 5) + .getEntry(); + private final GenericEntry upRPMStatus = intakeTab.add("Up Motor RPM OK", true) + .withWidget("Boolean Box") + .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) + .withPosition(0, 6) + .getEntry(); + private final GenericEntry downRPMStatus = intakeTab.add("Down Motor RPM OK", true) + .withWidget("Boolean Box") + .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) + .withPosition(1, 6) + .getEntry(); + + public Intake() { + up = new SparkMax(2, MotorType.kBrushless); + down = new SparkMax(3, MotorType.kBrushless); + + configureNEO550(up); + configureNEO550(down); + } + + + + private void configureNEO550(SparkMax motor) { + SparkMaxConfig neo550Config = new SparkMaxConfig(); + neo550Config + .smartCurrentLimit(40) + .idleMode(IdleMode.kCoast) + .voltageCompensation(12.0) + .openLoopRampRate(0.1); + + motor.setCANTimeout(250); + motor.configure(neo550Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void periodic() { + + + // Manual control through Shuffleboard + boolean upButtonState = upButton.getBoolean(false); + boolean downButtonState = downButton.getBoolean(false); + double upSpeedValue = upSpeed.getDouble(0.5); + double downSpeedValue = downSpeed.getDouble(0.5); + + if (upButtonState) up.set(upSpeedValue); + if (downButtonState) down.set(downSpeedValue); + + // Handle synchronized movement + if (bothOutButton.getBoolean(false)) { + up.set(0.3); + down.set(0.15); + } else if (bothInButton.getBoolean(false)) { + intake(); + } + + // Update RPM status + updateRPMStatus(); + } + + public void setSpeed(double speed) { + up.set(speed); + down.set(speed); + } + + public void intake() { + double upRPM = Math.abs(up.getEncoder().getVelocity()); + double downRPM = Math.abs(down.getEncoder().getVelocity()); + + if (startupCounter < 10) { + up.set(-0.4); + down.set(-0.4); + startupCounter++; + } else if (upRPM < 50 || downRPM < 50) { + if (stallCounter < 50) { + stallCounter++; + up.set(-0.4); + down.set(-0.4); + } else { + up.set(0); + down.set(0); + bothOutButton.setBoolean(false); + startupCounter = 0; + stallCounter = 0; + } + } else { + stallCounter = 0; + up.set(-0.4); + down.set(-0.4); + } + } + + private void updateRPMStatus() { + double upRPM = Math.abs(up.getEncoder().getVelocity()); + double downRPM = Math.abs(down.getEncoder().getVelocity()); + + upRPMStatus.setBoolean(upRPM >= 50); + downRPMStatus.setBoolean(downRPM >= 50); + + SmartDashboard.putNumber("Up Motor RPM", upRPM); + SmartDashboard.putNumber("Down Motor RPM", downRPM); + } +} diff --git a/src/main/java/frc/robot/subsystems/SuperStruct.java b/src/main/java/frc/robot/subsystems/SuperStruct.java new file mode 100644 index 0000000..f6d1ed9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SuperStruct.java @@ -0,0 +1,229 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.states.StateMachine; +import frc.robot.states.SuperStructState; +import edu.wpi.first.wpilibj.XboxController; + +public class SuperStruct extends SubsystemBase { + Elevator mElevator; + Grabber mGrabber; + Intake mIntake; + AlgaeIntake mAlgaeIntake; + StateMachine mStateMachine; + SuperStructState mCommandedState; + private final XboxController controller; + private static final int CONTROLLER_PORT = 0; // Adjust port as needed! uwu + + private static SuperStruct mInstance = null; + + public static SuperStruct getInstance() { + if (mInstance == null) { + mInstance = new SuperStruct(); + } + return mInstance; + } + /** Creates a new StateMachine. */ + public SuperStruct() { + mElevator = Elevator.getInstance(); + mGrabber = Grabber.getInstance(); + mIntake = Intake.getInstance(); + mAlgaeIntake = AlgaeIntake.getInstance(); + mStateMachine = StateMachine.getInstance(); + mCommandedState = SuperStructState.DEFAULT; + controller = new XboxController(CONTROLLER_PORT); + } + + public void L1() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L2() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L3() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L4() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void TRAVEL() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0.01); + } + + public void CS() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.intake(); + } + + public void PLACEMENT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0.3); + } + + public void DEFAULT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0); + mAlgaeIntake.setIntakeSpeed(0); + mAlgaeIntake.setAngle(0); + } + + public void ALGAE_STOWAGE() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.setAngle(0); + } + + public void ALGAE_INTAKE() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.intake(); + } + + public void ALGAE_PLACEMENT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.setAngle(0); + } + + public void updateState() { + switch (mCommandedState) { + case L1: + L1(); + break; + case L2: + L2(); + break; + case L3: + L3(); + break; + case L4: + L4(); + break; + case TRAVEL: + TRAVEL(); + break; + case CS: + CS(); + break; + case PLACEMENT: + PLACEMENT(); + break; + case DEFAULT: + DEFAULT(); + break; + case ALGAE_STOWAGE: + ALGAE_STOWAGE(); + break; + case ALGAE_INTAKE: + ALGAE_INTAKE(); + break; + case ALGAE_PLACEMENT: + ALGAE_PLACEMENT(); + break; + } + } + + public void setState(SuperStructState state) { + mStateMachine.setCommandedState(state); + } + + /** + * Handle Xbox controller inputs for state control (✿◠‿◠) + */ + public void handleControllerInput() { + // A button - L1 + if (controller.getAButton()) { + setState(SuperStructState.L1); + } + // B button - L2 + else if (controller.getBButton()) { + setState(SuperStructState.L2); + } + // Y button - L3 + else if (controller.getYButton()) { + setState(SuperStructState.L3); + } + // X button - L4 + else if (controller.getXButton()) { + setState(SuperStructState.L4); + } + // Right bumper - TRAVEL + else if (controller.getRightBumperButton()) { + setState(SuperStructState.TRAVEL); + } + // Left bumper - CS (Charging Station) + else if (controller.getLeftBumperButton()) { + setState(SuperStructState.CS); + } + // Start button - PLACEMENT + else if (controller.getStartButton()) { + setState(SuperStructState.PLACEMENT); + } + // Back button - DEFAULT + else if (controller.getBackButton()) { + setState(SuperStructState.DEFAULT); + } + // POV Up - ALGAE_STOWAGE + else if (controller.getPOV() == 0) { + setState(SuperStructState.ALGAE_STOWAGE); + } + // POV Right - ALGAE_INTAKE + else if (controller.getPOV() == 90) { + setState(SuperStructState.ALGAE_INTAKE); + } + // POV Down - ALGAE_PLACEMENT + else if (controller.getPOV() == 180) { + setState(SuperStructState.ALGAE_PLACEMENT); + } + } + + @Override + public void periodic() { + // Handle controller input first + handleControllerInput(); + + // This method will be called once per scheduler run + mCommandedState = mStateMachine.getCommandedState(); + updateState(); + + SmartDashboard.putString("Commanded State", mCommandedState.toString()); + // Add controller info to dashboard + SmartDashboard.putString("Last Button Pressed", getLastButtonPressed()); + } + + /** + * Helper to get last button pressed for dashboard uwu + */ + private String getLastButtonPressed() { + if (controller.getAButton()) return "A - L1"; + if (controller.getBButton()) return "B - L2"; + if (controller.getYButton()) return "Y - L3"; + if (controller.getXButton()) return "X - L4"; + if (controller.getRightBumperButton()) return "RB - TRAVEL"; + if (controller.getLeftBumperButton()) return "LB - CS"; + if (controller.getStartButton()) return "Start - PLACEMENT"; + if (controller.getBackButton()) return "Back - DEFAULT"; + if (controller.getPOV() == 0) return "POV Up - ALGAE_STOWAGE"; + if (controller.getPOV() == 90) return "POV Right - ALGAE_INTAKE"; + if (controller.getPOV() == 180) return "POV Down - ALGAE_PLACEMENT"; + return "None"; + } +} From 649faff38ddbadffab6d6e88e59bbf041cc47530 Mon Sep 17 00:00:00 2001 From: shrubman8787 <138845668+shrubman8787@users.noreply.github.com> Date: Tue, 11 Feb 2025 17:41:21 +0800 Subject: [PATCH 2/3] added swerve code --- src/main/java/frc/robot/Constants.java | 52 +++ src/main/java/frc/robot/RobotContainer.java | 83 +++- src/main/java/frc/robot/Vision.java | 173 ++++++++ .../frc/robot/commands/DriveCommands.java | 68 ++++ .../frc/robot/subsystems/drive/Drive.java | 382 ++++++++++++++++++ .../frc/robot/subsystems/drive/GyroIO.java | 30 ++ .../robot/subsystems/drive/GyroIOPigeon2.java | 38 ++ .../frc/robot/subsystems/drive/Module.java | 177 ++++++++ .../frc/robot/subsystems/drive/ModuleIO.java | 47 +++ .../frc/robot/subsystems/drive/Swerve.java | 232 +++++++++++ vendordeps/AdvantageKit.json | 35 ++ vendordeps/ChoreoLib-2025.0.3.json | 44 ++ vendordeps/PathplannerLib-2025.2.3.json | 38 ++ vendordeps/photonlib.json | 71 ++++ 14 files changed, 1469 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/Vision.java create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Swerve.java create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/ChoreoLib-2025.0.3.json create mode 100644 vendordeps/PathplannerLib-2025.2.3.json create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..a7bc38a --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,52 @@ + + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final Mode currentMode = Mode.REAL; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public static class Vision { + public static final String kCameraName = "WEB_CAM"; + // Cam mounted facing forward, half a meter forward of center, half a meter up from center. + public static final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); + + // The layout of the AprilTags on the field + public static final AprilTagFieldLayout kTagLayout = + AprilTagFields.k2025Reefscape.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); +} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 46fd5b5..087080e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,15 +1,26 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.DriveCommands; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; import frc.robot.subsystems.Intake; import frc.robot.subsystems.SuperStruct; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIO; +import frc.robot.subsystems.drive.Swerve; public class RobotContainer { private CommandXboxController m_Controller; @@ -20,20 +31,90 @@ public class RobotContainer { private final Intake m_intake; private final SuperStruct m_SuperStruct; + + + private final Drive drive; + private final Vision vision = new Vision(); + + private final CommandXboxController controller = new CommandXboxController(0); + + + private final SendableChooser autoChooser; + public RobotContainer() { m_elevator = Elevator.getInstance(); m_grabber = Grabber.getInstance(); m_intake = Intake.getInstance(); m_SuperStruct = SuperStruct.getInstance(); + + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + + drive = + new Drive( + new GyroIOPigeon2(), + new Swerve(0), + new Swerve(1), + new Swerve(2), + new Swerve(3), + vision); + + break; + + // case SIM: + // // Sim robot, instantiate physics sim IO implementations + // drive = + // new Drive( + // new GyroIO() {}, + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim(), + // new ModuleIOSim()); + // break; + + default: + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + vision); + break; + } + + // Set up auto routines + + autoChooser = AutoBuilder.buildAutoChooser(); + configureButtonBindings(); } private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); } public Command getAutonomousCommand() { - return null; + return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java new file mode 100644 index 0000000..608afed --- /dev/null +++ b/src/main/java/frc/robot/Vision.java @@ -0,0 +1,173 @@ +package frc.robot; + +import static frc.robot.Constants.Vision.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class Vision { + private final PhotonCamera camera; + private final PhotonPoseEstimator photonEstimator; + private Matrix curStdDevs; + + // Simulation + private PhotonCameraSim cameraSim; + private VisionSystemSim visionSim; + + public Vision() { + camera = new PhotonCamera(kCameraName); + + photonEstimator = + new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + // ----- Simulation + if (Robot.isSimulation()) { + // Create the vision system simulation which handles cameras and targets on the field. + visionSim = new VisionSystemSim("main"); + // Add all the AprilTags inside the tag layout as visible targets to this simulated field. + visionSim.addAprilTags(kTagLayout); + // Create simulated camera properties. These can be set to mimic your actual camera. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(15); + cameraProp.setAvgLatencyMs(50); + cameraProp.setLatencyStdDevMs(15); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(camera, cameraProp); + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(cameraSim, kRobotToCam); + + cameraSim.enableDrawWireframe(true); + } + } + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + + + } + return visionEst; + } + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + // ----- Simulation + + public void simulationPeriodic(Pose2d robotSimPose) { + visionSim.update(robotSimPose); + } + + /** Reset pose history of the robot in the vision system simulation. */ + public void resetSimPose(Pose2d pose) { + if (Robot.isSimulation()) visionSim.resetRobotPose(pose); + } + + /** A Field2d for visualizing our robot and objects on the field. */ + public Field2d getSimDebugField() { + if (!Robot.isSimulation()) return null; + return visionSim.getDebugField(); + } + + /** + * Gets the latest estimated pose from vision! OwO + * @return The estimated Pose2d, or null if no valid vision data + */ + public Pose2d getLatestPose() { + Optional visionEst = getEstimatedGlobalPose(); + + // Check if we have a valid vision estimate >w< + if (visionEst.isPresent()) { + return visionEst.get().estimatedPose.toPose2d(); + } + + return null; // No valid pose found ʕ•ᴥ•ʔ + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..d424998 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,68 @@ + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; +import java.util.function.DoubleSupplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + + private DriveCommands() {} + + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * 0.3, + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * 0.3, + omega * drive.getMaxAngularSpeedRadPerSec() * 0.3, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } + + +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 0000000..a119847 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,382 @@ +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; + +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.pathfinding.Pathfinding; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PathPlannerLogging; +// import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Vision; +import frc.robot.subsystems.drive.GyroIO.GyroIOInputs; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.AutoLogOutputManager; +import org.littletonrobotics.junction.Logger; +import org.photonvision.estimation.VisionEstimation; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + + +public class Drive extends SubsystemBase { + private static final double MAX_LINEAR_SPEED = 4; + private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + private static final double DRIVE_BASE_RADIUS = + Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + private Field2d odoField2d = new Field2d(); + + private final Pose2d photonPose2d = new Pose2d(); + + private GyroIO gyroIO; + private final GyroIOInputs gyroInputs = new GyroIOInputs(); + + private Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId = new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(12); + } + }, + null, + this)); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + private final Vision vision; + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO, + Vision vision) { + this.gyroIO = gyroIO; + this.vision = vision; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Configure AutoBuilder for PathPlanner + // AutoBuilder.configure( + // this::getPose, + // this::setPose, + // () -> kinematics.toChassisSpeeds(getModuleStates()), + // this::runVelocity, + // new HolonomicPathFollowerConfig( + // MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + // () -> + // DriverStation.getAlliance().isPresent() + // && DriverStation.getAlliance().get() == Alliance.Red, + // this); + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + e.printStackTrace(); + } + + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> runVelocity(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + );}; + + + // Pathfinding.setPathfinder(new LocalADStarAK()); + // PathPlannerLogging.setLogActivePathCallback( + // (activePath) -> { + // Logger.recordOutput( + // "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + // }); + // PathPlannerLogging.setLogTargetPoseCallback( + // (targetPose) -> { + // Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + // }); + + // Configure SysId + // Sysid = new SysIdRoutine( + // new SysIdRoutine.Config( + // null, + // null, + // null, + // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> { + // for (int i = 0; i < 4; i++) { + // modules[i].runCharacterization(12); + // } + // }, + // null, + // this)); + + + public void periodic() { + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = getModulePositions(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.yawPosition; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply odometry update + + poseEstimator.update(rawGyroRotation, modulePositions); + odoField2d.setRobotPose(poseEstimator.getEstimatedPosition()); + SmartDashboard.putData("nig", odoField2d); + + // Update pose estimator with vision data! >w< + Pose2d visionPose = vision.getLatestPose(); + if (visionPose != null) { + // Add vision measurement with a timestamp UwU + poseEstimator.addVisionMeasurement( + visionPose, + Timer.getFPGATimestamp() + ); + } + + // Add these cute wittle dashboard outputs >w< + Pose2d currentPose = getPose(); + SmartDashboard.putNumber("Odometry/X Position (m)", currentPose.getX()); + SmartDashboard.putNumber("Odometry/Y Position (m)", currentPose.getY()); + SmartDashboard.putNumber("Odometry/Rotation (deg)", currentPose.getRotation().getDegrees()); + + // Get current speeds from module states + ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); + SmartDashboard.putNumber("Robot Speed/X (m/s)", speeds.vxMetersPerSecond); + SmartDashboard.putNumber("Robot Speed/Y (m/s)", speeds.vyMetersPerSecond); + SmartDashboard.putNumber("Robot Speed/Rotation (rad/s)", speeds.omegaRadiansPerSecond); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + // /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ +// @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ +// @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + }; + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getRotation()); + return chassisSpeeds; + } + + + public void end(boolean interrupted) { + setPose(new Pose2d(new Translation2d() , new Rotation2d())); + } + + /** Resets the odometry and gyro to zero UwU */ + public void resetPoseToZero() { + // Reset gyro to zero + gyroIO.setYaw(0.0); + // Reset odometry to zero position and rotation + setPose(new Pose2d(new Translation2d(), new Rotation2d())); + } + + +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..6d68776 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.math.geometry.Rotation2d; + + +public interface GyroIO { + + public static class GyroIOInputs implements LoggableInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + @Override + public void toLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'toLog'"); + } + @Override + public void fromLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'fromLog'"); + } + } + + public default void updateInputs(GyroIOInputs inputs) {} + + public default void setYaw(double degrees) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..a6296e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(40,"GTX7130"); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } + + @Override + public void setYaw(double degrees) { + pigeon.getConfigurator().setYaw(degrees); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..ce3c2bd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,177 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.subsystems.drive.ModuleIO.ModuleIOInputs; + +import org.littletonrobotics.junction.Logger; + +public class Module { + private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + private final ModuleIO io; + private final ModuleIOInputs inputs = new ModuleIOInputs(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0008, 0.05, 0.005); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + state.optimize(getAngle()); + + + // Update setpoints, controllers run in "periodic" + angleSetpoint = state.angle; + speedSetpoint = state.speedMetersPerSecond; + + return state; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..9ed71d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs implements LoggableInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + @Override + public void toLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'toLog'"); + } + @Override + public void fromLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'fromLog'"); + } + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java new file mode 100644 index 0000000..4111133 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -0,0 +1,232 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; + + + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.*; + + + + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class Swerve implements ModuleIO { + private final TalonFX driveTalon; + // private final TalonFX turnTalon; + private final SparkMax turnSparkMax; + private final CANcoder cancoder; + + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + // private final StatusSignal turnPosition; + // private final StatusSignal turnVelocity; + // private final StatusSignal turnAppliedVolts; + // private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = 6.122449; + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public Swerve(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(1,"GTX7130"); //lf + // turnTalon = new TalonFX(1); + turnSparkMax = new SparkMax(2, MotorType.kBrushless); + cancoder = new CANcoder(0,"GTX7130"); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.272461)); // MUST BE CALIBRATED + break; + case 1: + driveTalon = new TalonFX(11,"GTX7130"); //rf + // turnTalon = new TalonFX(4); + + turnSparkMax = new SparkMax(12, MotorType.kBrushless); + cancoder = new CANcoder(1,"GTX7130"); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.123047)); // MUST BE CALIBRATED + break; + case 2: + driveTalon = new TalonFX(31,"GTX7130"); //lr + // turnTalon = new TalonFX(7); + turnSparkMax = new SparkMax(32, MotorType.kBrushless); + + cancoder = new CANcoder(3,"GTX7130"); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.119141)); // MUST BE CALIBRATED + break; + case 3: + driveTalon = new TalonFX(21,"GTX7130"); //rr + // turnTalon = new TalonFX(10); + turnSparkMax = new SparkMax(22, MotorType.kBrushless); + + cancoder = new CANcoder(2,"GTX7130"); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.205322)); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + // var turnConfig = new TalonFXConfiguration(); + // turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + // turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + // turnTalon.getConfigurator().apply(turnConfig); + // setTurnBrakeMode(true); + SparkMaxConfig turnSparkConfig = new SparkMaxConfig(); + + turnSparkConfig + .smartCurrentLimit(30) + .voltageCompensation(12.0) + .inverted(isTurnMotorInverted); + + + turnSparkMax.setCANTimeout(250); + + turnSparkMax.configure(turnSparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + // turnPosition = turnTalon.getPosition(); + // turnVelocity = turnTalon.getVelocity(); + // turnAppliedVolts = turnTalon.getMotorVoltage(); + // turnCurrent = turnTalon.getSupplyCurrent(); + + // BaseStatusSignal.setUpdateFrequencyForAll(100.0, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, drivePosition); // Required for odometry, use faster rate + // BaseStatusSignal.setUpdateFrequencyForAll( + // 50.0, + // driveVelocity, + // driveAppliedVolts, + // driveCurrent, + // turnAbsolutePosition, + // turnVelocity, + // turnAppliedVolts, + // turnCurrent); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition); + driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // BaseStatusSignal.refreshAll( + // drivePosition, + // driveVelocity, + // driveAppliedVolts, + // driveCurrent, + // turnAbsolutePosition, + // turnPosition, + // turnVelocity, + // turnAppliedVolts, + // turnCurrent); + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + // inputs.turnAbsolutePosition = + // Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + // .minus(absoluteEncoderOffset); + // inputs.turnPosition = + // Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + // inputs.turnVelocityRadPerSec = + // Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + // inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + // inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + new Rotation2d(Units.rotationsToRadians(cancoder.getAbsolutePosition().getValueAsDouble())) + // turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + .plus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnSparkMax.getEncoder().getPosition() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnSparkMax.getEncoder().getVelocity()) + / TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + // turnTalon.setControl(new VoltageOut(volts)); + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + // var config = new MotorOutputConfigs(); + // config.Inverted = + // isTurnMotorInverted + // ? InvertedValue.Clockwise_Positive + // : InvertedValue.CounterClockwise_Positive; + // config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + // turnTalon.getConfigurator().apply(config); + // turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..03df051 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json new file mode 100644 index 0000000..5a8cd54 --- /dev/null +++ b/vendordeps/ChoreoLib-2025.0.3.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib-2025.0.3.json", + "name": "ChoreoLib", + "version": "2025.0.3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://lib.choreo.autos/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.3.json b/vendordeps/PathplannerLib-2025.2.3.json new file mode 100644 index 0000000..9151ce4 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.3.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.3.json", + "name": "PathplannerLib", + "version": "2025.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file From f5cf927838fa4fc31e94dacda358d3c9f802ff17 Mon Sep 17 00:00:00 2001 From: Ray Date: Wed, 12 Feb 2025 16:22:13 +0800 Subject: [PATCH 3/3] fuck you --- .vscode/launch.json | 12 +- build.gradle | 2 +- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 + src/main/deploy/pathplanner/settings.json | 32 + .../FSLib2025/chassis/OnboardModuleState.java | 60 + .../FSLib2025/chassis/SwerveModuleConfig.java | 18 + .../chassis/SwerveModuleConstants.java | 8 + .../frc/FSLib2025/control/Feedforward.java | 25 + src/main/java/frc/FSLib2025/control/PID.java | 34 + src/main/java/frc/FSLib2025/math/FS_Math.java | 21 + .../frc/FSLib2025/math/LinearRegression.java | 22 + .../FSLib2025/math/PolynomialRegression.java | 65 + .../FSLib2025/odometry/TankDriveOdometry.java | 52 + .../java/frc/FSLib2025/physics/Physics.java | 21 + .../FSLib2025/stateMachine/RobotState.java | 7 + .../FSLib2025/stateMachine/StateMachine.java | 42 + .../frc/FSLib2025/util/LocalADStarAK.java | 149 ++ .../java/frc/FSLib2025/util/MathHelpers.java | 33 + src/main/java/frc/FSLib2025/util/OSUtil.java | 36 + .../FSLib2025/vision/LimelightHelpers.java | 1236 +++++++++++++++++ src/main/java/frc/robot/Constants.java | 120 +- src/main/java/frc/robot/Main.java | 1 + src/main/java/frc/robot/Robot.java | 79 +- src/main/java/frc/robot/RobotContainer.java | 135 +- src/main/java/frc/robot/Vision.java | 6 +- .../frc/robot/commands/TeleopSuperStruct.java | 2 - .../java/frc/robot/commands/TeleopSwerve.java | 114 ++ .../frc/robot/subsystems/AlgaeIntake.java | 32 +- .../java/frc/robot/subsystems/Elevator.java | 62 +- .../java/frc/robot/subsystems/Grabber.java | 52 +- .../java/frc/robot/subsystems/Intake.java | 9 +- .../frc/robot/subsystems/SuperStruct.java | 21 +- .../java/frc/robot/subsystems/Swerve.java | 266 ++++ .../frc/robot/subsystems/SwerveModule.java | 122 ++ .../frc/robot/subsystems/drive/Drive.java | 247 ++-- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../frc/robot/subsystems/drive/Swerve.java | 8 +- 38 files changed, 2901 insertions(+), 307 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc/FSLib2025/chassis/OnboardModuleState.java create mode 100644 src/main/java/frc/FSLib2025/chassis/SwerveModuleConfig.java create mode 100644 src/main/java/frc/FSLib2025/chassis/SwerveModuleConstants.java create mode 100644 src/main/java/frc/FSLib2025/control/Feedforward.java create mode 100644 src/main/java/frc/FSLib2025/control/PID.java create mode 100644 src/main/java/frc/FSLib2025/math/FS_Math.java create mode 100644 src/main/java/frc/FSLib2025/math/LinearRegression.java create mode 100644 src/main/java/frc/FSLib2025/math/PolynomialRegression.java create mode 100644 src/main/java/frc/FSLib2025/odometry/TankDriveOdometry.java create mode 100644 src/main/java/frc/FSLib2025/physics/Physics.java create mode 100644 src/main/java/frc/FSLib2025/stateMachine/RobotState.java create mode 100644 src/main/java/frc/FSLib2025/stateMachine/StateMachine.java create mode 100644 src/main/java/frc/FSLib2025/util/LocalADStarAK.java create mode 100644 src/main/java/frc/FSLib2025/util/MathHelpers.java create mode 100644 src/main/java/frc/FSLib2025/util/OSUtil.java create mode 100644 src/main/java/frc/FSLib2025/vision/LimelightHelpers.java create mode 100644 src/main/java/frc/robot/commands/TeleopSwerve.java create mode 100644 src/main/java/frc/robot/subsystems/Swerve.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..4e29e24 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "beta" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/build.gradle b/build.gradle index 1945af5..c5ce1a7 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..3f475e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..86e876c --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 40.0, + "robotMOI": 2.0, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.0508, + "driveGearing": 6.122, + "maxDriveSpeed": 4.0, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 20.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/FSLib2025/chassis/OnboardModuleState.java b/src/main/java/frc/FSLib2025/chassis/OnboardModuleState.java new file mode 100644 index 0000000..16227cc --- /dev/null +++ b/src/main/java/frc/FSLib2025/chassis/OnboardModuleState.java @@ -0,0 +1,60 @@ +package frc.FSLib2025.chassis; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public class OnboardModuleState { + + /** + * Minimize the change in heading the desired swerve module state would require + * by potentially + * reversing the direction the wheel spins. Customized from WPILib's version to + * include placing in + * appropriate scope for CTRE and REV onboard control as both controllers as of + * writing don't have + * support for continuous input. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { + double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); + double targetSpeed = desiredState.speedMetersPerSecond; + double delta = targetAngle - currentAngle.getDegrees(); + if (Math.abs(delta) > 90) { + targetSpeed = -targetSpeed; + targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); + } + return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); + } + + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within scope + */ + private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) { + newAngle += 360; + } + while (newAngle > upperBound) { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) { + newAngle += 360; + } + return newAngle; + } +} diff --git a/src/main/java/frc/FSLib2025/chassis/SwerveModuleConfig.java b/src/main/java/frc/FSLib2025/chassis/SwerveModuleConfig.java new file mode 100644 index 0000000..d212145 --- /dev/null +++ b/src/main/java/frc/FSLib2025/chassis/SwerveModuleConfig.java @@ -0,0 +1,18 @@ +package frc.FSLib2025.chassis; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class SwerveModuleConfig { + + public int DriveMotorId; + public int AngleMotorId; + public int CancoderId; + public Rotation2d AngleOffset; + + public SwerveModuleConfig (int driveMotorId, int angleMotorId, int cancoderId, Rotation2d angleOffset) { + this.DriveMotorId = driveMotorId; + this.AngleMotorId = angleMotorId; + this.CancoderId = cancoderId; + this.AngleOffset = angleOffset; + } +} diff --git a/src/main/java/frc/FSLib2025/chassis/SwerveModuleConstants.java b/src/main/java/frc/FSLib2025/chassis/SwerveModuleConstants.java new file mode 100644 index 0000000..502a7e6 --- /dev/null +++ b/src/main/java/frc/FSLib2025/chassis/SwerveModuleConstants.java @@ -0,0 +1,8 @@ +package frc.FSLib2025.chassis; + +public class SwerveModuleConstants { + public int DriveMotorId; + public int SteerMotorId; + public int CANcoderId; + public double CANcoderOffset; +} diff --git a/src/main/java/frc/FSLib2025/control/Feedforward.java b/src/main/java/frc/FSLib2025/control/Feedforward.java new file mode 100644 index 0000000..523ea80 --- /dev/null +++ b/src/main/java/frc/FSLib2025/control/Feedforward.java @@ -0,0 +1,25 @@ +package frc.FSLib2025.control; + +public class Feedforward { + private double kA, kV, kP_Pos, kP_Vel, kS; + + public Feedforward (double ikV, double ikA, double ikP_Pos, double ikP_Vel, double ikS){ + kV = ikV; + kA = ikA; + kP_Pos = ikP_Pos; + kP_Vel = ikP_Vel; + kS = ikS; + } + + public double calculate(double vel, double accel, double posError, double velError){ + return (kV * vel + kA * accel + kP_Pos * posError + kP_Vel * velError + kS * Math.signum(vel)); + } + + public void setGains(double ikV, double ikA, double ikP_Pos, double ikP_Vel, double ikS){ + kV = ikV; + kA = ikA; + kP_Pos = ikP_Pos; + kP_Vel = ikP_Vel; + kS = ikS; + } +} diff --git a/src/main/java/frc/FSLib2025/control/PID.java b/src/main/java/frc/FSLib2025/control/PID.java new file mode 100644 index 0000000..a26bf98 --- /dev/null +++ b/src/main/java/frc/FSLib2025/control/PID.java @@ -0,0 +1,34 @@ +package frc.FSLib2025.control; + +public class PID { + private double lastError = 0, d = 0, i = 0; + double output; + private double kP, kI, kD, windup, limit; + + public PID(double ikP, double ikI, double ikD, double iwindup, double ilimit){ + kP = ikP; + kI = ikI; + kD = ikD; + windup = iwindup; + limit = ilimit; + } + + public PID(double ikP, double ikI, double ikD){ + this(ikP, ikI, ikD, 0, 0); + } + + public double calculate (double currentValue, double target) { + return calculate(target - currentValue); + } + + public double calculate (double error){ + i = (Math.abs(error) <= windup) ? i += error : 0; + double iOut = i * kI; + iOut = (iOut >= limit) ? limit : iOut; + iOut = (iOut <= -limit) ? -limit : iOut; + d = error - lastError; + lastError = error; + output = (error * kP) + iOut + (d * kD); + return output; + } +} diff --git a/src/main/java/frc/FSLib2025/math/FS_Math.java b/src/main/java/frc/FSLib2025/math/FS_Math.java new file mode 100644 index 0000000..c21a4af --- /dev/null +++ b/src/main/java/frc/FSLib2025/math/FS_Math.java @@ -0,0 +1,21 @@ +package frc.FSLib2025.math; + +public class FS_Math { + + public static boolean isWithin (double value, double min, double max) { + return Math.max(min, value) == Math.min(value, max); + } + + public static double clamp (double value, double min, double max) { + return Math.min(Math.max(value, min), max); + } + + public static double clamp (int value, int min, int max) { + return Math.min(Math.max(value, min), max); + } + + public static double constrainAngleDegrees(double angle) { + return Math.atan2(Math.sin(angle / 180.0 * Math.PI), Math.cos(angle / 180.0 * Math.PI)) * 180 / Math.PI; + } + +} diff --git a/src/main/java/frc/FSLib2025/math/LinearRegression.java b/src/main/java/frc/FSLib2025/math/LinearRegression.java new file mode 100644 index 0000000..b10c7e3 --- /dev/null +++ b/src/main/java/frc/FSLib2025/math/LinearRegression.java @@ -0,0 +1,22 @@ +package frc.FSLib2025.math; + +public class LinearRegression { + + private double[][] data; + + public LinearRegression (double[][] mapValues) { + data = mapValues; + } + + public double calculate (double xValues) { + int index = 0; + for (double[] i : data) { + if(i[0] >= xValues) break; + index++; + } + double dx = xValues - data[index-1][0]; + double x = data[index][0] - data[index-1][0]; + return data[index-1][1] * (1-dx/x) + data[index][1] * dx/x; + } + +} diff --git a/src/main/java/frc/FSLib2025/math/PolynomialRegression.java b/src/main/java/frc/FSLib2025/math/PolynomialRegression.java new file mode 100644 index 0000000..6e28e2f --- /dev/null +++ b/src/main/java/frc/FSLib2025/math/PolynomialRegression.java @@ -0,0 +1,65 @@ +package frc.FSLib2025.math; + +public class PolynomialRegression { + + private double[] coefficients; + + public PolynomialRegression (double[] xValues, double[] yValues, int degree) { + coefficients = fitPolynomial(xValues, yValues, degree); + } + + public double predictDeg(double xValue) { + return evaluatePolynomial(coefficients, xValue); + } + + private double[] fitPolynomial(double[] x, double[] y, int degree) { + int n = x.length; + double[][] matrix = new double[degree + 1][degree + 2]; + double[] result = new double[degree + 1]; + + for (int i = 0; i <= degree; i++) { + for (int j = 0; j <= degree; j++) { + matrix[i][j] = 0; + for (int k = 0; k < n; k++) { + matrix[i][j] += Math.pow(x[k], i + j); + } + } + matrix[i][degree + 1] = 0; + for (int k = 0; k < n; k++) { + matrix[i][degree + 1] += Math.pow(x[k], i) * y[k]; + } + } + + for (int i = 0; i <= degree; i++) { + for (int k = i + 1; k <= degree; k++) { + if (Math.abs(matrix[i][i]) < Math.abs(matrix[k][i])) { + double[] temp = matrix[i]; + matrix[i] = matrix[k]; + matrix[k] = temp; + } + } + for (int k = i + 1; k <= degree; k++) { + double t = matrix[k][i] / matrix[i][i]; + for (int j = 0; j <= degree + 1; j++) { + matrix[k][j] -= t * matrix[i][j]; + } + } + } + for (int i = degree; i >= 0; i--) { + result[i] = matrix[i][degree + 1] / matrix[i][i]; + for (int k = 0; k < i; k++) { + matrix[k][degree + 1] -= matrix[k][i] * result[i]; + } + } + + return result; + } + + private double evaluatePolynomial(double[] coefficients, double x) { + double result = 0; + for (int i = coefficients.length - 1; i >= 0; i--) { + result = result * x + coefficients[i]; + } + return result; + } +} \ No newline at end of file diff --git a/src/main/java/frc/FSLib2025/odometry/TankDriveOdometry.java b/src/main/java/frc/FSLib2025/odometry/TankDriveOdometry.java new file mode 100644 index 0000000..3694f8c --- /dev/null +++ b/src/main/java/frc/FSLib2025/odometry/TankDriveOdometry.java @@ -0,0 +1,52 @@ +package frc.FSLib2025.odometry; + +public class TankDriveOdometry { + + private double x; // Robot's X position + private double y; // Robot's Y position + private double theta; // Robot's heading angle (in radians) + + public TankDriveOdometry(double initialX, double initialY, double initialTheta) { + this.x = initialX; + this.y = initialY; + this.theta = initialTheta; + } + + public void updateOdometry(double leftDistance, double rightDistance, double wheelBase) { + double deltaTheta = (rightDistance - leftDistance) / wheelBase; + double deltaDistance = (leftDistance + rightDistance) / 2; + + // Update the robot's position + double thetaMid = theta + (deltaTheta / 2.0); // Midpoint orientation for more accurate position update + x += deltaDistance * Math.cos(thetaMid); + y += deltaDistance * Math.sin(thetaMid); + + theta += deltaTheta; + theta = normalizeAngle(theta); + } + + // Normalize Angle to -pi to pi + private double normalizeAngle(double angle) { + while (angle > Math.PI) angle -= 2 * Math.PI; + while (angle <= -Math.PI) angle += 2 * Math.PI; + return angle; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getTheta() { + return theta; + } + + public void resetOdometry(double newX, double newY, double newTheta) { + this.x = newX; + this.y = newY; + this.theta = newTheta; + } +} diff --git a/src/main/java/frc/FSLib2025/physics/Physics.java b/src/main/java/frc/FSLib2025/physics/Physics.java new file mode 100644 index 0000000..78db01c --- /dev/null +++ b/src/main/java/frc/FSLib2025/physics/Physics.java @@ -0,0 +1,21 @@ +package frc.FSLib2025.physics; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class Physics { + + private final static double GRAVITY = 9.81; + private final static double AIR_DENSITY = 1.225; + + public static double getProjectileYPosition(double xPosition, double initialVelocity, Rotation2d launchAngle) { + double angleInRadians = launchAngle.getRadians(); + double time = xPosition / (initialVelocity * Math.cos(angleInRadians)); + double yPosition = (initialVelocity * Math.sin(angleInRadians) * time) - (0.5 * GRAVITY * time * time); + return yPosition; + } + + public static double calculateAirResistance(double dragCoefficient, double frontalArea, double velocity) { + return 0.5 * dragCoefficient * AIR_DENSITY * frontalArea * velocity * velocity; + } + +} diff --git a/src/main/java/frc/FSLib2025/stateMachine/RobotState.java b/src/main/java/frc/FSLib2025/stateMachine/RobotState.java new file mode 100644 index 0000000..50525d2 --- /dev/null +++ b/src/main/java/frc/FSLib2025/stateMachine/RobotState.java @@ -0,0 +1,7 @@ +package frc.FSLib2025.stateMachine; + +public class RobotState { + public double ElevatorPosition = 0; + public double IntakePosition = 0; + public double IntakeState = 0; +} diff --git a/src/main/java/frc/FSLib2025/stateMachine/StateMachine.java b/src/main/java/frc/FSLib2025/stateMachine/StateMachine.java new file mode 100644 index 0000000..11c3e1b --- /dev/null +++ b/src/main/java/frc/FSLib2025/stateMachine/StateMachine.java @@ -0,0 +1,42 @@ +package frc.FSLib2025.stateMachine; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class StateMachine extends SubsystemBase { + + public static final RobotState Default = new RobotState(); + + public static final RobotState Intake = new RobotState(); + + public static final RobotState L1 = new RobotState(); + static { + L1.ElevatorPosition = 0; + L1.IntakePosition = 0; + L1.IntakeState = 0; + } + + public static final RobotState L2 = new RobotState(); + public static final RobotState L3 = new RobotState(); + public static final RobotState L4 = new RobotState(); + + public static RobotState robotState = new RobotState(); + + private final CommandXboxController commandController; + private final XboxController controller; + + public StateMachine(CommandXboxController commandController) { + this.commandController = commandController; + this.controller = commandController.getHID(); + } + + public RobotState getRobotState() { + return robotState; + } + + @Override + public void periodic() { + if (controller.getAButtonPressed()) robotState = L1; + } +} diff --git a/src/main/java/frc/FSLib2025/util/LocalADStarAK.java b/src/main/java/frc/FSLib2025/util/LocalADStarAK.java new file mode 100644 index 0000000..d16c7b3 --- /dev/null +++ b/src/main/java/frc/FSLib2025/util/LocalADStarAK.java @@ -0,0 +1,149 @@ +package frc.FSLib2025.util; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.Pathfinder; +import com.pathplanner.lib.pathfinding.LocalADStar; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/FSLib2025/util/MathHelpers.java b/src/main/java/frc/FSLib2025/util/MathHelpers.java new file mode 100644 index 0000000..e1c5617 --- /dev/null +++ b/src/main/java/frc/FSLib2025/util/MathHelpers.java @@ -0,0 +1,33 @@ +package frc.FSLib2025.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class MathHelpers { + public static final Pose2d kPose2dZero = new Pose2d(); + + public static final Pose2d pose2dFromRotation(Rotation2d rotation) { + return new Pose2d(kTranslation2dZero, rotation); + } + + public static final Pose2d pose2dFromTranslation(Translation2d translation) { + return new Pose2d(translation, kRotation2dZero); + } + + public static final Rotation2d kRotation2dZero = new Rotation2d(); + public static final Rotation2d kRotation2dPi = Rotation2d.fromDegrees(180.0); + + public static final Translation2d kTranslation2dZero = new Translation2d(); + + public static final Transform2d kTransform2dZero = new Transform2d(); + + public static final Transform2d transform2dFromRotation(Rotation2d rotation) { + return new Transform2d(kTranslation2dZero, rotation); + } + + public static final Transform2d transform2dFromTranslation(Translation2d translation) { + return new Transform2d(translation, kRotation2dZero); + } +} diff --git a/src/main/java/frc/FSLib2025/util/OSUtil.java b/src/main/java/frc/FSLib2025/util/OSUtil.java new file mode 100644 index 0000000..f3e91fd --- /dev/null +++ b/src/main/java/frc/FSLib2025/util/OSUtil.java @@ -0,0 +1,36 @@ +package frc.FSLib2025.util; + +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; + +import edu.wpi.first.wpilibj.DriverStation; + +public class OSUtil { + public static void fsSync() { + try { + System.out.println("Executing filesystem sync..."); + ProcessBuilder builder = new ProcessBuilder("sync"); + builder.redirectErrorStream(true); + Process p = builder.start(); + BufferedReader r = new BufferedReader(new InputStreamReader(p.getInputStream())); + String line; + while (true) { + line = r.readLine(); + if (line == null) { + break; + } + } + System.out.println("Done"); + } catch (IOException e) { + DriverStation.reportError("Failed to manually execute filesystem 'sync' command to flush logs to disk", + null); + } + } + + public static void fsSyncAsync() { + new Thread(() -> { + fsSync(); + }).start(); + } +} diff --git a/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java b/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java new file mode 100644 index 0000000..e66111f --- /dev/null +++ b/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java @@ -0,0 +1,1236 @@ +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) + +package frc.FSLib2025.vision; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + /** + * Makes a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles. + * + * @param pose The Pose2d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a7bc38a..b2a1f75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,11 +6,16 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import frc.FSLib2025.chassis.SwerveModuleConstants; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -34,19 +39,116 @@ public static enum Mode { REPLAY } - public static class Vision { - public static final String kCameraName = "WEB_CAM"; - // Cam mounted facing forward, half a meter forward of center, half a meter up from center. - public static final Transform3d kRobotToCam = - new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); + public static final class SwerveConstants { + public static final double DEADBAND = 0.05; + public static final double MAX_MODULE_SPEED = 5.0; + public static final double MAX_MODULE_ROTATIONAL_SPEED = Units.degreesToRadians(360 * 1.15); + public static final double WHEEL_BASE = 0.583; // 0.5715 + + public static final int PIGEON_ID = 40; + + public static final double STEER_MOTOR_KP = 0.010; + public static final double STEER_MOTOR_KI = 0.08; // 0.05 + public static final double STEER_MOTOR_KD = 0.015; + public static final double STEER_MOTOR_WINDUP = 0.0; + public static final int STEER_MOTOR_LIMIT = 0; + + public static final double DRIVE_MOTOR_GEAR_RATIO = 6.122449; + public static final double DRIVE_WHEEL_DIAMETERS = 0.0964511800486235; // meters (4 * 0.0254) + public static final double DRIVE_WHEEL_PERIMETER = Math.PI * DRIVE_WHEEL_DIAMETERS; // meters + + public static final Translation2d[] MODULE_TRANSLATOIN_METERS = new Translation2d[] { + new Translation2d(WHEEL_BASE / 2.0, WHEEL_BASE / 2.0), + new Translation2d(WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), + new Translation2d(-WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), + new Translation2d(-WHEEL_BASE / 2.0, WHEEL_BASE / 2.0) + }; + + public static final SwerveModuleConstants LF_MOD_CONSTANTS = new SwerveModuleConstants(); + static { + LF_MOD_CONSTANTS.DriveMotorId = 21; + LF_MOD_CONSTANTS.SteerMotorId = 22; + LF_MOD_CONSTANTS.CANcoderId = 0; + LF_MOD_CONSTANTS.CANcoderOffset = 0.126465; + } + + public static final SwerveModuleConstants RF_MOD_CONSTANTS = new SwerveModuleConstants(); + static { + RF_MOD_CONSTANTS.DriveMotorId = 31; + RF_MOD_CONSTANTS.SteerMotorId = 32; + RF_MOD_CONSTANTS.CANcoderId = 1; + RF_MOD_CONSTANTS.CANcoderOffset = 0.303955; + } + + public static final SwerveModuleConstants RR_MOD_CONSTANTS = new SwerveModuleConstants(); + static { + RR_MOD_CONSTANTS.DriveMotorId = 1; + RR_MOD_CONSTANTS.SteerMotorId = 2; + RR_MOD_CONSTANTS.CANcoderId = 2; + RR_MOD_CONSTANTS.CANcoderOffset = -0.043457; + } + + public static final SwerveModuleConstants LR_MOD_CONSTANTS = new SwerveModuleConstants(); + static { + LR_MOD_CONSTANTS.DriveMotorId = 11; + LR_MOD_CONSTANTS.SteerMotorId = 12; + LR_MOD_CONSTANTS.CANcoderId = 3; + LR_MOD_CONSTANTS.CANcoderOffset = 0.125977; + } + } + + public static class Vision { + // public static final String OPCam = "OrangePiCamera"; + public static final String ATCam = "AprilTagCamera"; + // Cam mounted facing forward, half a meter forward of center, half a meter up + // from center. + public static final Transform3d kRobotToOPCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToATCam = new Transform3d( + new Translation3d(Units.inchesToMeters(28 / 2), -0.025, 0.71), new Rotation3d(0, 0, 0)); // The layout of the AprilTags on the field - public static final AprilTagFieldLayout kTagLayout = - AprilTagFields.k2025Reefscape.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - // The standard deviations of our vision estimated poses, which affect correction rate + // The standard deviations of our vision estimated poses, which affect + // correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); -} + + public static final Matrix kStateStdDevs = VecBuilder.fill(0.5, 0.5, 0.8); + public static final Matrix kVisionMeasurementStdDevs = VecBuilder.fill(1, 1, 1); + } + + public static final class FieldConstants { + // starting pose + public static final Pose2d Left = new Pose2d(new Translation2d(7.231, 7.615), Rotation2d.fromDegrees(180)); + public static final Pose2d Mid = new Pose2d(new Translation2d(8.068, 6.178), Rotation2d.fromDegrees(180)); + public static final Pose2d Right = new Pose2d(new Translation2d(8.068, 5.067), Rotation2d.fromDegrees(180)); + // REEF scoring pose + public static final Pose2d A = new Pose2d(new Translation2d(3.180, 4.082), Rotation2d.fromDegrees(0)); + public static final Pose2d B = new Pose2d(new Translation2d(3.109, 3.847), Rotation2d.fromDegrees(0)); + public static final Pose2d C = new Pose2d(new Translation2d(3.654, 2.885), Rotation2d.fromDegrees(60)); + public static final Pose2d D = new Pose2d(new Translation2d(3.920, 2.731), Rotation2d.fromDegrees(60)); + public static final Pose2d E = new Pose2d(new Translation2d(5.058, 2.713), Rotation2d.fromDegrees(120)); + public static final Pose2d F = new Pose2d(new Translation2d(5.325, 2.880), Rotation2d.fromDegrees(120)); + public static final Pose2d G = new Pose2d(new Translation2d(5.883, 3.851), Rotation2d.fromDegrees(180)); + public static final Pose2d H = new Pose2d(new Translation2d(5.883, 4.190), Rotation2d.fromDegrees(180)); + public static final Pose2d I = new Pose2d(new Translation2d(5.341, 5.341), Rotation2d.fromDegrees(-120)); + public static final Pose2d J = new Pose2d(new Translation2d(5.072, 5.320), Rotation2d.fromDegrees(-120)); + public static final Pose2d K = new Pose2d(new Translation2d(3.887, 5.375), Rotation2d.fromDegrees(-60)); + public static final Pose2d L = new Pose2d(new Translation2d(3.623, 5.215), Rotation2d.fromDegrees(-60)); + // Algae & Coral + public static final Pose2d LA = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d LC = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d MA = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d MC = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d RA = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + public static final Pose2d RC = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + // PROCESSOR + public static final Pose2d PRO = new Pose2d(new Translation2d(6.347, 0.573), Rotation2d.fromDegrees(-90)); + // Coral Station + public static final Pose2d CSR = new Pose2d(new Translation2d(1.666, 0.693), Rotation2d.fromDegrees(-126)); + public static final Pose2d CSL = new Pose2d(new Translation2d(1.666, 7.338), Rotation2d.fromDegrees(126)); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..f9f41a9 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController; /** * Do NOT add any static variables to this class, or any initialization at all. Unless you know what diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bcc721c..8d4fe33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.AlgaeIntake; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; import frc.robot.subsystems.Intake; @@ -23,13 +24,16 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.TeleopSuperStruct; import java.util.Map; /** - * The methods in this class are called automatically corresponding to each mode, as described in - * the TimedRobot documentation. If you change the name of this class or the package after creating + * The methods in this class are called automatically corresponding to each + * mode, as described in + * the TimedRobot documentation. If you change the name of this class or the + * package after creating * this project, you must also update the Main.java file in the project. */ public class Robot extends TimedRobot { @@ -37,24 +41,28 @@ public class Robot extends TimedRobot { private static final String kCustomAuto = "My Auto"; private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - - private final Grabber m_grabber = new Grabber(); - private final Intake m_intake = new Intake(); - private final Elevator m_elevator = new Elevator(); + + private final Grabber m_grabber = Grabber.getInstance(); + private final Intake m_intake = Intake.getInstance(); + private final Elevator m_elevator = Elevator.getInstance(); + private final AlgaeIntake m_algaeIntake = AlgaeIntake.getInstance(); private final SuperStruct m_superStruct = SuperStruct.getInstance(); + private final RobotContainer m_robotContainer; + private Command m_teleopSuperStruct; /** - * This function is run when the robot is first started up and should be used for any + * This function is run when the robot is first started up and should be used + * for any * initialization code. */ public Robot() { // Configure our NEO 550s for best performance! >w< - - + + m_robotContainer = new RobotContainer(); m_chooser.setDefaultOption("Default Auto", kDefaultAuto); m_chooser.addOption("My Auto", kCustomAuto); - + } @Override @@ -63,28 +71,40 @@ public void robotInit() { } /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * This function is called every 20 ms, no matter the mode. Use this for items + * like diagnostics * that you want ran during disabled, autonomous, teleoperated and test. * - *

This runs after the mode specific periodic functions, but before LiveWindow and + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and * SmartDashboard integrated updating. */ @Override public void robotPeriodic() { - m_grabber.periodic(); + m_grabber.periodic(); m_intake.periodic(); m_elevator.periodic(); + m_algaeIntake.periodic(); m_superStruct.periodic(); + CommandScheduler.getInstance().run(); } /** - * This autonomous (along with the chooser code above) shows how to select between different - * autonomous modes using the dashboard. The sendable chooser code works with the Java - * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and - * uncomment the getString line to get the auto name from the text box below the Gyro + * This autonomous (along with the chooser code above) shows how to select + * between different + * autonomous modes using the dashboard. The sendable chooser code works with + * the Java + * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the + * chooser code and + * uncomment the getString line to get the auto name from the text box below the + * Gyro * - *

You can add additional auto modes by adding additional comparisons to the switch structure - * below with additional strings. If using the SendableChooser make sure to add them to the + *

+ * You can add additional auto modes by adding additional comparisons to the + * switch structure + * below with additional strings. If using the SendableChooser make sure to add + * them to the * chooser code above as well. */ @Override @@ -122,29 +142,36 @@ public void teleopExit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } /** This function is called once when the robot is disabled. */ @Override - public void disabledInit() {} + public void disabledInit() { + } /** This function is called periodically when disabled. */ @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } /** This function is called once when test mode is enabled. */ @Override - public void testInit() {} + public void testInit() { + } /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + } /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + } /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 087080e..fa898fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,5 @@ package frc.robot; - import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -12,110 +11,104 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; +import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; import frc.robot.subsystems.Intake; import frc.robot.subsystems.SuperStruct; +import frc.robot.subsystems.Swerve; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; -import frc.robot.subsystems.drive.Swerve; + public class RobotContainer { - + private CommandXboxController m_Controller; private Joystick joystick; - + private final Elevator m_elevator; private final Grabber m_grabber; private final Intake m_intake; private final SuperStruct m_SuperStruct; - - - private final Drive drive; - private final Vision vision = new Vision(); + // private final Drive drive; + private final Swerve m_swerve; + private final Vision vision = new Vision(); - private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController controller = new CommandXboxController(0); - - private final SendableChooser autoChooser; + private final SendableChooser autoChooser; + + private final TeleopSwerve teleopSwerve; public RobotContainer() { m_elevator = Elevator.getInstance(); m_grabber = Grabber.getInstance(); m_intake = Intake.getInstance(); m_SuperStruct = SuperStruct.getInstance(); + m_swerve = new Swerve(); + teleopSwerve = new TeleopSwerve(m_swerve, controller); + + m_swerve.setDefaultCommand(teleopSwerve); + + // switch (Constants.currentMode) { + // case REAL: + // // Real robot, instantiate hardware IO implementations + + // // drive = new Drive( + // // new GyroIOPigeon2(), + // // new Swerve(0), + // // new Swerve(1), + // // new Swerve(2), + // // new Swerve(3), + // // vision); + + // // break; + + // // case SIM: + // // // Sim robot, instantiate physics sim IO implementations + // // drive = + // // new Drive( + // // new GyroIO() {}, + // // new ModuleIOSim(), + // // new ModuleIOSim(), + // // new ModuleIOSim(), + // // new ModuleIOSim()); + // // break; + + // default: + // // Replayed robot, disable IO implementations + // drive = new Drive( + // new GyroIO() { + // }, + // new ModuleIO() { + // }, + // new ModuleIO() { + // }, + // new ModuleIO() { + // }, + // new ModuleIO() { + // }, + // vision); + // break; + // } + + // Set up auto routines + + autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.setDefaultOption("null", null); - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - - drive = - new Drive( - new GyroIOPigeon2(), - new Swerve(0), - new Swerve(1), - new Swerve(2), - new Swerve(3), - vision); - - break; - - // case SIM: - // // Sim robot, instantiate physics sim IO implementations - // drive = - // new Drive( - // new GyroIO() {}, - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim(), - // new ModuleIOSim()); - // break; - - default: - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - vision); - break; - } - - // Set up auto routines - - autoChooser = AutoBuilder.buildAutoChooser(); - configureButtonBindings(); } private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); + } public Command getAutonomousCommand() { return autoChooser.getSelected(); } - } diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java index 608afed..cf88283 100644 --- a/src/main/java/frc/robot/Vision.java +++ b/src/main/java/frc/robot/Vision.java @@ -30,10 +30,10 @@ public class Vision { private VisionSystemSim visionSim; public Vision() { - camera = new PhotonCamera(kCameraName); + camera = new PhotonCamera(frc.robot.Constants.Vision.ATCam); photonEstimator = - new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam); + new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frc.robot.Constants.Vision.kRobotToATCam); photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); // ----- Simulation @@ -53,7 +53,7 @@ public Vision() { // targets. cameraSim = new PhotonCameraSim(camera, cameraProp); // Add the simulated camera to view the targets on this simulated field. - visionSim.addCamera(cameraSim, kRobotToCam); + visionSim.addCamera(cameraSim, frc.robot.Constants.Vision.kRobotToATCam); cameraSim.enableDrawWireframe(true); } diff --git a/src/main/java/frc/robot/commands/TeleopSuperStruct.java b/src/main/java/frc/robot/commands/TeleopSuperStruct.java index b9969e6..b4234e8 100644 --- a/src/main/java/frc/robot/commands/TeleopSuperStruct.java +++ b/src/main/java/frc/robot/commands/TeleopSuperStruct.java @@ -23,7 +23,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled @Override public void execute() { - m_superStruct.handleControllerInput(); } @@ -37,7 +36,6 @@ public void end(boolean interrupted) { // Returns true when the command should end @Override public boolean isFinished() { - return false; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java new file mode 100644 index 0000000..10078e1 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -0,0 +1,114 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +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.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Swerve; + +public class TeleopSwerve extends Command { + + private final Swerve swerve; + + private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); + private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); + private SlewRateLimiter rotLimiter = new SlewRateLimiter(3.0); + + private double xSpeed = 0.0; + private double ySpeed = 0.0; + private double rotSpeed = 0.0; + + private CommandXboxController controller; + + public TeleopSwerve(Swerve swerve, CommandXboxController controller) { + this.swerve = swerve; + this.controller = controller; + addRequirements(swerve); + } + + @Override + public void execute() { + + if (controller.getHID().getAButtonPressed()) { + swerve.setOdometryPosition(FieldConstants.A); + } + + if (controller.getHID().getBackButtonPressed()) { + swerve.setOdometryPosition(new Pose2d()); + swerve.setGyroYaw(new Rotation2d()); + } + + double slow = 1; + if (controller.getHID().getLeftBumperButton() || controller.getHID().getRightBumperButton()) { + slow = 0.5; + } + + double deadband = 0.04; + double xyMultiplier = 1; + double zMultiplier = 0.75; + + xSpeed = xLimiter.calculate(MathUtil.applyDeadband(-controller.getLeftY(), deadband)); + ySpeed = yLimiter.calculate(MathUtil.applyDeadband(-controller.getLeftX(), deadband)); + rotSpeed = rotLimiter.calculate(MathUtil.applyDeadband(-controller.getRightX(), deadband)); + xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); + ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); + // rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); + swerve.drive( + new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED).times(xyMultiplier) + .times(slow), + rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * zMultiplier * slow, + // 0, + true); + + if (controller.getHID().getYButtonPressed()) { + AutoBuilder.pathfindToPose(FieldConstants.A, + new PathConstraints( + 5, + 6, + 5 * Math.PI, + 4 * Math.PI)) + .until(isDriverControl).schedule(); + } + if (controller.getHID().getLeftTriggerAxis() > 0.4) { + AutoBuilder.pathfindToPose(FieldConstants.CSL, + new PathConstraints( + 5, + 6, + 5 * Math.PI, + 4 * Math.PI)) + .until(isDriverControl).schedule(); + } + if (controller.getHID().getRightTriggerAxis() > 0.4) { + AutoBuilder.pathfindToPose(FieldConstants.CSR, + new PathConstraints( + 5, + 6, + 5 * Math.PI, + 4 * Math.PI)) + .until(isDriverControl).schedule(); + } + + } + + public BooleanSupplier isDriverControl = () -> { + return Math.abs(controller.getLeftX()) >= 0.1 || Math.abs(controller.getLeftY()) >= 0.1; + }; + + @Override + public void end(boolean interrupted) { + swerve.drive(new Translation2d(), 0, false); + } +} diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index 58a4ec2..edbec78 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -19,7 +19,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class AlgaeIntake extends SubsystemBase { @@ -27,15 +31,19 @@ public class AlgaeIntake extends SubsystemBase { private final SparkMax rightAngle; private final CANcoder angleEncoder; private final SparkMax intakeMotor; + private final SparkMax duckMotor; private final PIDController anglePIDController; - private static final double ANGLE_kP = 0.01; // Adjust these PID values! uwu + private static final double ANGLE_kP = 1.1; // Adjust these PID values! uwu private static final double ANGLE_kI = 0.0; private static final double ANGLE_kD = 0.0; private static final double ANGLE_TOLERANCE = 2.0; // Degrees + private final ShuffleboardTab algaeIntakeTab = Shuffleboard.getTab("AlgaeInatke"); + private final GenericEntry angleDisplay; + private static AlgaeIntake mInstance = null; - public static AlgaeIntake getInstance() { + public static synchronized AlgaeIntake getInstance() { if (mInstance == null) { mInstance = new AlgaeIntake(); } @@ -44,19 +52,25 @@ public static AlgaeIntake getInstance() { /** Creates a new AlgaeIntake. */ public AlgaeIntake() { - leftAngle = new SparkMax(10, MotorType.kBrushless); - rightAngle = new SparkMax(11, MotorType.kBrushless); - angleEncoder = new CANcoder(12, "GTX7130"); - intakeMotor = new SparkMax(13, MotorType.kBrushless); - - configureNEO(leftAngle, false, true); - configureSlaveNEO(rightAngle, false, true); + leftAngle = new SparkMax(38, MotorType.kBrushless); + rightAngle = new SparkMax(37, MotorType.kBrushless); + angleEncoder = new CANcoder(5, "GTX7130"); + intakeMotor = new SparkMax(36, MotorType.kBrushless); + duckMotor = new SparkMax(39, MotorType.kBrushless); + + configureNEO(leftAngle, true, false); + configureSlaveNEO(rightAngle, false, false); configureNEO(intakeMotor, false, false); configureCANcoder(); // Initialize angle PID controller anglePIDController = new PIDController(ANGLE_kP, ANGLE_kI, ANGLE_kD); anglePIDController.setTolerance(ANGLE_TOLERANCE); + + angleDisplay = algaeIntakeTab.add("Current Angle", 0.0) + .withWidget(BuiltInWidgets.kTextView) + .withPosition(2, 4) + .getEntry(); } public void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 661282c..4ed35be 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -33,14 +33,14 @@ public class Elevator extends SubsystemBase { private final SparkMax leftMotor; private final SparkMax rightMotor; - private final CANcoder Cancoder; + // private final CANcoder Cancoder; private static final double kG = 0.05; // Gravity feed forward constant - adjust this! private static final double kDownSpeedMultiplier = 0.5; // Reduces down speed - adjust this! private static Elevator mInstance = null; - public static Elevator getInstance() { + public static synchronized Elevator getInstance() { if (mInstance == null) { mInstance = new Elevator(); } @@ -49,17 +49,17 @@ public static Elevator getInstance() { // Shuffleboard entries private final ShuffleboardTab elevatorTab = Shuffleboard.getTab("Elevator"); - private final GenericEntry upButton = elevatorTab.add("Elevator Up", false) - .withWidget("Toggle Button") - .withPosition(0, 0) - .withSize(1, 1) - .getEntry(); + // private final GenericEntry upButton = elevatorTab.add("Elevator Up", false) + // .withWidget("Toggle Button") + // .withPosition(0, 0) + // .withSize(1, 1) + // .getEntry(); - private final GenericEntry downButton = elevatorTab.add("Elevator Down", false) - .withWidget("Toggle Button") - .withPosition(1, 0) - .withSize(1, 1) - .getEntry(); + // private final GenericEntry downButton = elevatorTab.add("Elevator Down", false) + // .withWidget("Toggle Button") + // .withPosition(1, 0) + // .withSize(1, 1) + // .getEntry(); // Add these with other instance variables at the top private final GenericEntry speedEntry; @@ -85,12 +85,12 @@ public static Elevator getInstance() { /** Creates a new ElevatorSubsystem. */ public Elevator() { - leftMotor = new SparkMax(4, MotorType.kBrushless); // Update ID as needed - rightMotor = new SparkMax(5, MotorType.kBrushless); // Update ID as needed - Cancoder = new CANcoder(3); // Update CANcoder ID as needed! + leftMotor = new SparkMax(28, MotorType.kBrushless); // Update ID as needed + rightMotor = new SparkMax(27, MotorType.kBrushless); // Update ID as needed + // Cancoder = new CANcoder(7, "rio"); // Update CANcoder ID as needed! - configureNEO(leftMotor, false); // Invert both motors to make default direction clockwise - configureNEO(rightMotor, false); // Both motors still turn the same way + configureNEO(leftMotor, false, false); // Invert both motors to make default direction clockwise + configureNEO(rightMotor, false, false); // Both motors still turn the same way // Set position conversion factor for encoder @@ -127,8 +127,8 @@ public Elevator() { .withSize(1, 1) .getEntry(); - // Initialize CANcoder (ID 3 from your existing code) - elevatorCoder = new CANcoder(3); + // // Initialize CANcoder (ID 3 from your existing code) + elevatorCoder = new CANcoder(7); configureCANcoder(); // Add absolute position display to dashboard @@ -138,16 +138,16 @@ public Elevator() { .getEntry(); } - private void configureNEO(SparkMax motor, boolean inverted) { + private void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { SparkMaxConfig neoConfig = new SparkMaxConfig(); // Create soft limit config for elevator SoftLimitConfig softLimitConfig = new SoftLimitConfig(); softLimitConfig .forwardSoftLimit(28) // Adjust these limits for your elevator! - .forwardSoftLimitEnabled(true) + .forwardSoftLimitEnabled(softlimit) .reverseSoftLimit(0.0) // Bottom position - .reverseSoftLimitEnabled(true); + .reverseSoftLimitEnabled(softlimit); neoConfig .smartCurrentLimit(40) @@ -179,7 +179,7 @@ private void configureCANcoder() { // Apply configuration elevatorCoder.getConfigurator().apply(config); - + elevatorCoder.setPosition(0); // Wait for config to apply try { Thread.sleep(100); @@ -192,7 +192,7 @@ private void configureCANcoder() { * Get the absolute position from CANcoder! (✿◠‿◠) */ public double getPosition() { - return Cancoder.getPosition().getValueAsDouble(); + return elevatorCoder.getPosition().getValueAsDouble(); } public void setPosition(double targetPosition) { @@ -290,13 +290,13 @@ public void periodic() { unlockPosition(); } // Normal button control - if (upButton.getBoolean(false)) { - up(); - } else if (downButton.getBoolean(false)) { - down(); - } else { - stop(); - } + // if (upButton.getBoolean(false)) { + // up(); + // } else if (downButton.getBoolean(false)) { + // down(); + // } else { + // stop(); + // } } // Track maximum rotations diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index 802b5cd..8a2a7de 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -1,5 +1,9 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.jni.CANCommonJNI; import com.revrobotics.spark.*; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -19,10 +23,11 @@ public class Grabber extends SubsystemBase { private final SparkMax angle; private static final double DEFAULT_KG = 0.003; + private final CANcoder cancoder; private static Grabber mInstance = null; - public static Grabber getInstance() { + public static synchronized Grabber getInstance() { if (mInstance == null) { mInstance = new Grabber(); } @@ -37,9 +42,11 @@ public static Grabber getInstance() { private final PIDController pidController; public Grabber() { - angle = new SparkMax(1, MotorType.kBrushless); - configureNEO(angle, false, true); + angle = new SparkMax(17, MotorType.kBrushless); + configureNEO(angle, true, true); angle.getEncoder().setPosition(0.0); + cancoder = new CANcoder(4); + configureCANcoder(); // Initialize Shuffleboard entries angleDisplay = grabberTab.add("Current Angle", 0.0) @@ -58,16 +65,16 @@ public Grabber() { .withPosition(2, 0) .getEntry(); - pidController = new PIDController(0.07, 0.035, 0.007); + pidController = new PIDController(0.69, 0.035, 0.007); } /** * Sets the grabber to a specific angle (✿◠‿◠) * @param targetAngleDegrees The desired angle in degrees */ - public void setAngle(double targetAngleDegrees) { + public void setAngle(double targetRotations) { // Convert degrees to motor rotations (18.75:1 gear ratio) - double targetRotations = targetAngleDegrees * (18.75 / 360.0); + // double targetRotations = targetAngleDegrees * (18.75 / 360.0); // encoder // Update PID setpoint pidController.setSetpoint(targetRotations); @@ -78,20 +85,21 @@ public void setAngle(double targetAngleDegrees) { * @return true if at target angle, false otherwise */ public boolean isAtTargetAngle() { - double currentAngle = angle.getEncoder().getPosition(); + // double currentAngle = angle.getEncoder().getPosition(); //encoder + double currentAngle = cancoder.getAbsolutePosition().getValueAsDouble(); return Math.abs(currentAngle - pidController.getSetpoint()) < 0.1; } @Override public void periodic() { - double currentAngle = angle.getEncoder().getPosition(); + double currentAngle = cancoder.getAbsolutePosition().getValueAsDouble(); double kG = kGTuner.getDouble(DEFAULT_KG); // Use PID to maintain target angle angle.set(pidController.calculate(currentAngle) + kG); // Update displays - double currentAngleDegrees = angle.getEncoder().getPosition() * (360.0 / 18.75); + double currentAngleDegrees = cancoder.getAbsolutePosition().getValueAsDouble() * (360.0 / 18.75); angleDisplay.setDouble(currentAngleDegrees); } @@ -101,9 +109,9 @@ private void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { // Create soft limit config SoftLimitConfig softLimitConfig = new SoftLimitConfig(); softLimitConfig - .forwardSoftLimit(2.76) // +53 degrees with 18.75:1 + .forwardSoftLimit(0) // +53 degrees with 18.75:1 .forwardSoftLimitEnabled(softlimit) - .reverseSoftLimit(0) // Keep starting point + .reverseSoftLimit(-2.76) // Keep starting point .reverseSoftLimitEnabled(softlimit); neoConfig @@ -118,4 +126,26 @@ private void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { motor.setCANTimeout(250); motor.configure(neoConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } + + /** + * Configure the CANcoder for absolute position reading (◕ᴗ◕✿) + */ + private void configureCANcoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + // Configure for absolute position mode + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + + // Apply configuration + cancoder.getConfigurator().apply(config); + cancoder.setPosition(0); + + // Wait for config to apply + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5658a32..60b4cef 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -24,7 +24,7 @@ public class Intake extends SubsystemBase { private static Intake mInstance = null; - public static Intake getInstance() { + public static synchronized Intake getInstance() { if (mInstance == null) { mInstance = new Intake(); } @@ -71,8 +71,8 @@ public static Intake getInstance() { .getEntry(); public Intake() { - up = new SparkMax(2, MotorType.kBrushless); - down = new SparkMax(3, MotorType.kBrushless); + up = new SparkMax(18, MotorType.kBrushless); + down = new SparkMax(19, MotorType.kBrushless); configureNEO550(up); configureNEO550(down); @@ -94,8 +94,7 @@ private void configureNEO550(SparkMax motor) { @Override public void periodic() { - - + // Manual control through Shuffleboard boolean upButtonState = upButton.getBoolean(false); boolean downButtonState = downButton.getBoolean(false); diff --git a/src/main/java/frc/robot/subsystems/SuperStruct.java b/src/main/java/frc/robot/subsystems/SuperStruct.java index f6d1ed9..4daee7a 100644 --- a/src/main/java/frc/robot/subsystems/SuperStruct.java +++ b/src/main/java/frc/robot/subsystems/SuperStruct.java @@ -22,7 +22,7 @@ public class SuperStruct extends SubsystemBase { private static SuperStruct mInstance = null; - public static SuperStruct getInstance() { + public static synchronized SuperStruct getInstance() { if (mInstance == null) { mInstance = new SuperStruct(); } @@ -66,23 +66,23 @@ public void TRAVEL() { } public void CS() { - mElevator.setPosition(0); - mGrabber.setAngle(0); + // mElevator.setPosition(0); + mGrabber.setAngle(-0.19335); mIntake.intake(); } public void PLACEMENT() { - mElevator.setPosition(0); - mGrabber.setAngle(0); + // mElevator.setPosition(0); + // mGrabber.setAngle(0); mIntake.setSpeed(0.3); } public void DEFAULT() { - mElevator.setPosition(0); + // mElevator.setPosition(0); mGrabber.setAngle(0); mIntake.setSpeed(0); mAlgaeIntake.setIntakeSpeed(0); - mAlgaeIntake.setAngle(0); + mAlgaeIntake.setAngle(-0.403076); } public void ALGAE_STOWAGE() { @@ -92,8 +92,9 @@ public void ALGAE_STOWAGE() { } public void ALGAE_INTAKE() { - mElevator.setPosition(0); - mGrabber.setAngle(0); + // mElevator.setPosition(0); + // mGrabber.setAngle(0); + mAlgaeIntake.setAngle(-0.198965); mAlgaeIntake.intake(); } @@ -169,7 +170,7 @@ else if (controller.getXButton()) { else if (controller.getRightBumperButton()) { setState(SuperStructState.TRAVEL); } - // Left bumper - CS (Charging Station) + // Left bumper - CS (CORAL Station) else if (controller.getLeftBumperButton()) { setState(SuperStructState.CS); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java new file mode 100644 index 0000000..568fa9d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -0,0 +1,266 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +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.DriveFeedforwards; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; +import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Constants.Vision; + +public class Swerve extends SubsystemBase { + + private static Swerve instance = null; + public static Swerve getInstance() { + if (instance == null) { + instance = new Swerve(); + } + return instance; + } + + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, "GTX7130"); + + private SwerveModule[] modules = new SwerveModule[] { + new SwerveModule(0, SwerveConstants.LF_MOD_CONSTANTS), + new SwerveModule(1, SwerveConstants.RF_MOD_CONSTANTS), + new SwerveModule(2, SwerveConstants.RR_MOD_CONSTANTS), + new SwerveModule(3, SwerveConstants.LR_MOD_CONSTANTS) + }; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + private SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, getGyroYaw(), getModulePositions()); + private SwerveDrivePoseEstimator poseEstimator; + private final SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + + private Field2d field = new Field2d(); + + private TalonFX driveMotor = modules[0].driveMotor; + private final VoltageOut m_Volreq = new VoltageOut(0); + private final SysIdRoutine m_sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 to prevent brownout + null, // Use default timeout (10 s) + // Log state with Phoenix SignalLogger class + (state) -> SignalLogger.writeString("state", state.toString()) + ), new Mechanism((volts) -> driveMotor.setControl(m_Volreq.withOutput(volts.in(Volts))), null, this) + ); + + public Swerve() { + poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + getGyroYaw(), + getModulePositions(), + new Pose2d(), + Vision.kStateStdDevs, + Vision.kVisionMeasurementStdDevs); + + RobotConfig config = null; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + if (config == null) { + throw new RuntimeException("Failed to load config"); + } + + AutoBuilder.configure( + this::getOdometryPosition, + this::setOdometryPosition, + this::getRobotRelativSpeeds, + (speeds, feedforwards) -> driveRobotRelative(speeds), + new PPHolonomicDriveController( + new PIDConstants(5,0, 0.5, 0.0), // 18 + new PIDConstants(40, 0.0, 5)), // 5 + config, + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + field.getObject("target").setPose(pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + field.getObject("path").setPoses(poses); + }); + + setpointGenerator = new SwerveSetpointGenerator( + config, // The robot configuration. This is the same config used for generating trajectories and running path following commands. + Units.rotationsToRadians(10.0) // The max rotation velocity of a swerve module in radians per second. This should probably be stored in your Constants file + ); + + // Initialize the previous setpoint to the robot's current speeds & module states + ChassisSpeeds currentSpeeds = getRobotRelativSpeeds(); // Method to get current robot-relative chassis speeds + SwerveModuleState[] currentStates = getModuleStates(); // Method to get the current swerve module states + previousSetpoint = new SwerveSetpoint(currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); + + // odometry.resetPose(new Pose2d()); + // pigeon.reset(); + } + + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + ChassisSpeeds chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getGyroYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); + setModuleStates(moduleStates); + } + + // public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { + // ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); + // SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); + // setModuleStates(targetStates); + // } + + /** + * This method will take in desired robot-relative chassis speeds, + * generate a swerve setpoint, then set the target state for each module + * + * @param speeds The desired robot-relative speeds + */ + public void driveRobotRelative(ChassisSpeeds speeds) { + // Note: it is important to not discretize speeds before or after + // using the setpoint generator, as it will discretize them for you + previousSetpoint = setpointGenerator.generateSetpoint( + previousSetpoint, // The previous setpoint + speeds, // The desired target speeds + 0.02 // The loop time of the robot code, in seconds + ); + setModuleStates(previousSetpoint.moduleStates()); // Method that will drive the robot given target module states + } + + public Rotation2d getGyroYaw() { + return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); + } + + public void setGyroYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } + + public Pose2d getOdometryPosition() { + return odometry.getPoseMeters(); + } + + public void setOdometryPosition(Pose2d pose) { + odometry.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + public Pose2d getEstimatedPosition() { + return poseEstimator.getEstimatedPosition(); + } + + public void resetPosewithVision(Pose2d visionPose) { + poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), visionPose);; + } + + public ChassisSpeeds getRobotRelativSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getGyroYaw()); + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getModulePosition(); + } + return positions; + } + + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getModuleState(); + } + return states; + } + + public double[] getModuleAngle() { + double[] angles = new double[4]; + for (int i = 0; i < 4; i++) { + angles[i] = modules[i].getSteerAngle().getDegrees(); + } + return angles; // Return the array + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + if (desiredStates.length != 4) { + throw new IllegalArgumentException("desiredStates must have length 4"); + } + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); + for (SwerveModule mod : modules) { + mod.setDesiredState(desiredStates[mod.ModuleNumber]); + } + } + + public void stop() { + driveRobotRelative(new ChassisSpeeds()); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */ + public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */ + public void addVisionMeasurement( + Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); + } + + public Command safetystop() { + return Commands.run(() -> {driveRobotRelative(new ChassisSpeeds());}, this); + } + + @Override + public void periodic() { + poseEstimator.update(getGyroYaw(), getModulePositions()); + odometry.update(getGyroYaw(), getModulePositions()); + + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve odometry x", getOdometryPosition().getX()); + SmartDashboard.putNumber("swerve odometry y", getOdometryPosition().getY()); + SmartDashboard.putNumber("Module 1 Angle", getModuleAngle()[0]); + SmartDashboard.putNumber("Module 2 Angle", getModuleAngle()[1]); + SmartDashboard.putNumber("Module 3 Angle", getModuleAngle()[2]); + SmartDashboard.putNumber("Module 4 Angle", getModuleAngle()[3]); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..ba24713 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,122 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.FSLib2025.chassis.OnboardModuleState; +import frc.FSLib2025.chassis.SwerveModuleConstants; +import frc.FSLib2025.control.Feedforward; +import frc.FSLib2025.control.PID; +import frc.FSLib2025.math.FS_Math; +import frc.robot.Constants.SwerveConstants; + +public class SwerveModule { + public int ModuleNumber; + public SwerveModuleConstants ModuleConstants; + + private SparkMax steerMotor; + public TalonFX driveMotor; + + private CANcoder steerCANcoder; + + private PID steerPID; + + private Rotation2d lastAngle; + + public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { + this.ModuleNumber = moduleNumber; + this.ModuleConstants = moduleConstants; + + steerPID = new PID( + SwerveConstants.STEER_MOTOR_KP, + SwerveConstants.STEER_MOTOR_KI, + SwerveConstants.STEER_MOTOR_KD, + SwerveConstants.STEER_MOTOR_WINDUP, + SwerveConstants.STEER_MOTOR_LIMIT); + + lastAngle = new Rotation2d(); + + steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); + SparkMaxConfig steerMotorConfig = new SparkMaxConfig(); + steerMotorConfig.inverted(false); + steerMotor.setControlFramePeriodMs(20); + steerMotorConfig.idleMode(IdleMode.kBrake); + steerMotor.configure(steerMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + steerCANcoder = new CANcoder(moduleConstants.CANcoderId, "GTX7130"); + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + steerCANcoder.getConfigurator().apply(cancoderConfig); + + driveMotor = new TalonFX(moduleConstants.DriveMotorId, "GTX7130"); + TalonFXConfiguration talonfxConfig = new TalonFXConfiguration(); + talonfxConfig.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_MOTOR_GEAR_RATIO; + driveMotor.getConfigurator().apply(talonfxConfig); + driveMotor.setNeutralMode(NeutralModeValue.Brake); + } + + public void setDesiredState(SwerveModuleState desiredState) { + desiredState = OnboardModuleState.optimize(desiredState, getModuleState().angle); + + desiredState.angle = Math.abs(desiredState.speedMetersPerSecond) < 0.01 ? lastAngle : desiredState.angle; + double error = getModuleState().angle.getDegrees() - desiredState.angle.getDegrees(); + error = FS_Math.constrainAngleDegrees(error); + double steerOutput = steerPID.calculate(error); + setSteerMotor(steerOutput); + lastAngle = desiredState.angle; + + double percentOutput = desiredState.speedMetersPerSecond / SwerveConstants.MAX_MODULE_SPEED; + percentOutput = FS_Math.clamp(percentOutput, -1, 1); + setDriveMotor(percentOutput-0.005); + } + + private void setSteerMotor(double speed) { + steerMotor.set(speed); + } + + private void setDriveMotor(double speed) { + driveMotor.set(speed); + } + + public Rotation2d getSteerAngle() { + return Rotation2d.fromRotations(steerCANcoder.getAbsolutePosition().getValueAsDouble()); + } + + public SwerveModulePosition getModulePosition() { + return new SwerveModulePosition( + driveMotor.getPosition().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + + public SwerveModuleState getModuleState() { + return new SwerveModuleState( + driveMotor.getVelocity().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + + public TalonFX getDriveMotor() { + return driveMotor; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a119847..352797d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -19,6 +20,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.units.measure.Voltage; @@ -40,50 +42,48 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; - public class Drive extends SubsystemBase { private static final double MAX_LINEAR_SPEED = 4; private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - private static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + private static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; private Field2d odoField2d = new Field2d(); private final Pose2d photonPose2d = new Pose2d(); - private GyroIO gyroIO; + private GyroIO gyroIO; private final GyroIOInputs gyroInputs = new GyroIOInputs(); - - private Module[] modules = new Module[4]; // FL, FR, BL, BR + + private Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId = new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), - new SysIdRoutine.Mechanism( - (voltage) -> { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(12); - } - }, - null, - this)); + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(12); + } + }, + null, + this)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, + lastModulePositions, new Pose2d()); + private final Vision vision; public Drive( @@ -102,77 +102,77 @@ public Drive( // Configure AutoBuilder for PathPlanner // AutoBuilder.configure( - // this::getPose, - // this::setPose, - // () -> kinematics.toChassisSpeeds(getModuleStates()), - // this::runVelocity, - // new HolonomicPathFollowerConfig( - // MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), - // () -> - // DriverStation.getAlliance().isPresent() - // && DriverStation.getAlliance().get() == Alliance.Red, - // this); - RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - config = null; - e.printStackTrace(); - } - - AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> runVelocity(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - );}; - - - // Pathfinding.setPathfinder(new LocalADStarAK()); - // PathPlannerLogging.setLogActivePathCallback( - // (activePath) -> { - // Logger.recordOutput( - // "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - // }); - // PathPlannerLogging.setLogTargetPoseCallback( - // (targetPose) -> { - // Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - // }); - - // Configure SysId - // Sysid = new SysIdRoutine( - // new SysIdRoutine.Config( - // null, - // null, - // null, - // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), - // new SysIdRoutine.Mechanism( - // (voltage) -> { - // for (int i = 0; i < 4; i++) { - // modules[i].runCharacterization(12); - // } - // }, - // null, - // this)); - + // this::getPose, + // this::setPose, + // () -> kinematics.toChassisSpeeds(getModuleStates()), + // this::runVelocity, + // new HolonomicPathFollowerConfig( + // MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + // () -> + // DriverStation.getAlliance().isPresent() + // && DriverStation.getAlliance().get() == Alliance.Red, + // this); + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + throw new IllegalStateException("no robot config found"); + } + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> runVelocity(speeds), // Method that will drive the robot given ROBOT RELATIVE + // ChassisSpeeds. Also optionally outputs individual module + // feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic + // drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + }; + + // Pathfinding.setPathfinder(new LocalADStarAK()); + // PathPlannerLogging.setLogActivePathCallback( + // (activePath) -> { + // Logger.recordOutput( + // "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + // }); + // PathPlannerLogging.setLogTargetPoseCallback( + // (targetPose) -> { + // Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + // }); + + // Configure SysId + // Sysid = new SysIdRoutine( + // new SysIdRoutine.Config( + // null, + // null, + // null, + // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> { + // for (int i = 0; i < 4; i++) { + // modules[i].runCharacterization(12); + // } + // }, + // null, + // this)); public void periodic() { gyroIO.updateInputs(gyroInputs); @@ -197,11 +197,10 @@ public void periodic() { SwerveModulePosition[] modulePositions = getModulePositions(); SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } @@ -216,7 +215,7 @@ public void periodic() { } // Apply odometry update - + poseEstimator.update(rawGyroRotation, modulePositions); odoField2d.setRobotPose(poseEstimator.getEstimatedPosition()); SmartDashboard.putData("nig", odoField2d); @@ -227,8 +226,7 @@ public void periodic() { // Add vision measurement with a timestamp UwU poseEstimator.addVisionMeasurement( visionPose, - Timer.getFPGATimestamp() - ); + Timer.getFPGATimestamp()); } // Add these cute wittle dashboard outputs >w< @@ -236,7 +234,7 @@ public void periodic() { SmartDashboard.putNumber("Odometry/X Position (m)", currentPose.getX()); SmartDashboard.putNumber("Odometry/Y Position (m)", currentPose.getY()); SmartDashboard.putNumber("Odometry/Rotation (deg)", currentPose.getRotation().getDegrees()); - + // Get current speeds from module states ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); SmartDashboard.putNumber("Robot Speed/X (m/s)", speeds.vxMetersPerSecond); @@ -273,8 +271,10 @@ public void stop() { } /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. + * Stops the drive and turns the modules to an X arrangement to resist movement. + * The modules will + * return to their normal orientations the next time a nonzero velocity is + * requested. */ public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; @@ -295,8 +295,11 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return sysId.dynamic(direction); } - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ -// @AutoLogOutput(key = "SwerveStates/Measured") + /** + * Returns the module states (turn angles and drive velocities) for all of the + * modules. + */ + // @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { @@ -305,7 +308,10 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + /** + * Returns the module positions (turn angles and drive positions) for all of the + * modules. + */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -315,7 +321,7 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the current odometry pose. */ -// @AutoLogOutput(key = "Odometry/Robot") + // @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } @@ -334,7 +340,7 @@ public void setPose(Pose2d pose) { * Adds a vision measurement to the pose estimator. * * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. + * @param timestamp The timestamp of the vision measurement in seconds. */ public void addVisionMeasurement(Pose2d visionPose, double timestamp) { poseEstimator.addVisionMeasurement(visionPose, timestamp); @@ -353,21 +359,21 @@ public double getMaxAngularSpeedRadPerSec() { /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), + new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } public ChassisSpeeds getRobotRelativeSpeeds() { - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getRotation()); + ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), + getRotation()); return chassisSpeeds; } - public void end(boolean interrupted) { - setPose(new Pose2d(new Translation2d() , new Rotation2d())); + setPose(new Pose2d(new Translation2d(), new Rotation2d())); } /** Resets the odometry and gyro to zero UwU */ @@ -378,5 +384,4 @@ public void resetPoseToZero() { setPose(new Pose2d(new Translation2d(), new Rotation2d())); } - } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index a6296e5..f5723df 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -12,7 +12,7 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(40,"GTX7130"); + private final Pigeon2 pigeon = new Pigeon2(20,"GTX7130"); private final StatusSignal yaw = pigeon.getYaw(); private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 4111133..7e993fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -68,7 +68,7 @@ public Swerve(int index) { // turnTalon = new TalonFX(1); turnSparkMax = new SparkMax(2, MotorType.kBrushless); cancoder = new CANcoder(0,"GTX7130"); - absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.272461)); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.127441)); // MUST BE CALIBRATED break; case 1: driveTalon = new TalonFX(11,"GTX7130"); //rf @@ -76,7 +76,7 @@ public Swerve(int index) { turnSparkMax = new SparkMax(12, MotorType.kBrushless); cancoder = new CANcoder(1,"GTX7130"); - absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.123047)); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.301270)); // MUST BE CALIBRATED break; case 2: driveTalon = new TalonFX(31,"GTX7130"); //lr @@ -84,7 +84,7 @@ public Swerve(int index) { turnSparkMax = new SparkMax(32, MotorType.kBrushless); cancoder = new CANcoder(3,"GTX7130"); - absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(-0.119141)); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.453613)); // MUST BE CALIBRATED break; case 3: driveTalon = new TalonFX(21,"GTX7130"); //rr @@ -92,7 +92,7 @@ public Swerve(int index) { turnSparkMax = new SparkMax(22, MotorType.kBrushless); cancoder = new CANcoder(2,"GTX7130"); - absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.205322)); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(0.037598)); // MUST BE CALIBRATED break; default: throw new RuntimeException("Invalid module index");