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

Shooter hub position and distance

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-17 16:09:58 -07:00
parent da6f9b50b5
commit 8e3ee46b4c
5 changed files with 38 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+15 -16
View File
@@ -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,20 +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;
@@ -153,11 +152,11 @@ public class Shooter extends SubsystemBase {
switch (mode) {
case Shooting:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get()));
break;
case Ready:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
break;
case NotReady:
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+11 -32
View File
@@ -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_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
// 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)
@@ -63,29 +65,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);