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

WIP

Browse Source 
Tweaked shooter offset model and new progress with X position
This commit is contained in:
mimigamin
2026-03-14 18:29:15 -06:00
parent 07ec609b01
commit ebab028818
11 changed files with 87 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+3 -2
View File
@@ -89,8 +89,9 @@ public class Shooter extends SubsystemBase {
shooterButtonReady = false;
}
public AngularVelocity getBallVelocity() {
return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond)));
public double getBallVelocity() {
return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI;
//Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS)
}
@AutoLogOutput
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+2 -1
View File
@@ -18,7 +18,8 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
public static final double INDEXER_GEAR_RATIO = 1.;
public static final double T_CONSTANT = 2;
public static final double SHOOTER_RADIUS = 2/39.37;
public static final double INDEXER_RADIUS = 0.625/39.37;
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
// public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+1 -1
View File
@@ -26,7 +26,7 @@ public interface ShooterIO {
AngularVelocity motor1Velocity = RotationsPerSecond.of(0);
AngularVelocity motor2Velocity = RotationsPerSecond.of(0);
AngularVelocity indexerVelocity = RotationsPerSecond.of(0);
AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(42.53); //guestimate
double indexerOutput = 0;
Current motor1Current = Amps.of(0);
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+3 -1
View File
@@ -87,6 +87,9 @@ public class ShooterReal implements ShooterIO {
public void setIndexerOutput(ShooterState state, double percentOutput) {
state.indexerTargetOutput = percentOutput;
m_indexerMotor.set(percentOutput);
if (state.indexerTargetOutput - state.indexerOutput > 0.05){
state.indexerForwardVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
}
}
@@ -95,7 +98,6 @@ public class ShooterReal implements ShooterIO {
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO);
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO);
state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
state.indexerOutput = m_indexerMotor.get();
// state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+25 -12
View File
@@ -150,14 +150,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public void setInitalPose(Pose2d startingAutoPose){
initalPose2d = startingAutoPose;
}
// MIRA public void setOdoPose(Pose2d pose) {
// if (pose == null) return;
// io.tareEverything();
// initalPose2d = pose;
// io.resetPose(pose);
// robotKnowsWhereItIs = true;
// rotTarget = pose.getRotation().getDegrees();
// }
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -378,20 +370,41 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
driveFieldAngle(leftStick, ang);
}
public void defenseXPosition(){
io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0));
}
public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) {
public void stopDefenseXPosition(){
io.restoreSteerOffsets();
}
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
// Calculate the angle between the current position and the lead position
//Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle();
Rotation2d ang = new Rotation2d(0,1);
System.out.println(ang);
driveFieldAngle(leftStick, ang);
}
public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) {
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity.in(RotationsPerSecond)));
if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity));
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Aim Offset", aimOffset);
// double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity.in(RotationsPerSecond)) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub)));
// double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub)));
// Logger.recordOutput("Aim Offset", aimOffset);
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+9 -5
View File
@@ -75,9 +75,10 @@ public final class SwerveDriveConstants {
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
private static final class ModuleSpecificConstants { //2025
public static final class ModuleSpecificConstants { //2026
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -85,7 +86,8 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103);
public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -93,7 +95,8 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562);
public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -101,7 +104,8 @@ public final class SwerveDriveConstants {
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366);
public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java
+6
View File
@@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
import frc4388.utility.status.CanDevice;
public interface SwerveIO {
@AutoLog
@@ -25,6 +27,10 @@ public interface SwerveIO {
public default void tareEverything() {}
public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {}
public default void restoreSteerOffsets(){}
public default void resetPose(Pose2d pose) {}
public default void addVisionMeasurement(List<PoseObservation> poses) {}
src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java
+11
View File
@@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
import frc4388.utility.status.CanDevice;
public class SwerveReal implements SwerveIO {
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
@@ -55,6 +57,15 @@ public class SwerveReal implements SwerveIO {
}
}
@Override
public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {
}
@Override
public void restoreSteerOffsets() {
}
@Override
public void setLimits(double limitInAmps) {
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {