diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c00d837..7c28598 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,7 +5,7 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; public static final int GIT_REVISION = 66; public static final String GIT_SHA = "3a5057fc4187877fa770eac46b7ffd7ff06413b0"; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index a0e706a..185ff55 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -6,6 +6,8 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; 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.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; @@ -76,7 +78,6 @@ public class Shooter extends SubsystemBase { } - @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter @@ -84,18 +85,18 @@ public class Shooter extends SubsystemBase { Logger.processInputs("Shooter", state); io.updateInputs(state); + + ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + + double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); + Logger.recordOutput("Hub Dist", distanceToHub); + + if(this.mode != ShooterMode.NotReady) { - - ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); - - Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - - double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); - - // this.mode = ShooterMode.Shooting; - // TODO: get if the robot is within the angle of the hub boolean driverError = false; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index ef06902..3299c03 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,11 +1,14 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -14,13 +17,6 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - - // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); - // public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15); - // public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - - // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); - // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10); @@ -29,7 +25,7 @@ public class ShooterConstants { public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.1); - // Tolerances + // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); @@ -40,8 +36,14 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 5); - + // + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { + double speed = SHOOTER_ACTIVE_VELOCITY.get(); + return RotationsPerSecond.of(speed); + } + + // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) @@ -57,29 +59,6 @@ public class ShooterConstants { public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); - // Limits - - // 0 is the forward angle on the robot. - // negative is left, positive is right - // public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180); - // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); - - // 0 is paralell to the ground, 90 is directly up - // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); - - // Motor configs - // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index 19457d8..b9c8df0 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -3,13 +3,16 @@ package frc4388.utility.compute; import edu.wpi.first.math.geometry.Translation2d; public class FieldPositions { - public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534); + // We set the default position to one just in case it doesn't update // Setting to null could cause a null pointer, and setting to (0,0) could cause problems // I would rather have the 50/50 chance than a code error - public static Translation2d HUB_POSITION = RED_HUB_POSITION; + public static Translation2d HUB_POSITION = BLUE_HUB_POSITION; public static void update() { if(TimesNegativeOne.isRed) {