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

Implement shooting while moving vertically

Browse Source
This commit is contained in:
mimigamin
2026-03-19 15:28:01 -06:00
parent 73c3849569
commit d1055a4f27
5 changed files with 141 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
elastic-layout.json
+103 -57
View File
@@ -8,10 +8,10 @@
"layouts": [
{
"title": "Tag Processed",
"x": 384.0,
"x": 896.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"height": 384.0,
"type": "List Layout",
"properties": {
"label_position": "TOP"
@@ -55,27 +55,11 @@
}
],
"containers": [
{
"title": "MatchTime",
"x": 0.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Match Time",
"properties": {
"topic": "/AdvantageKit/DriverStation/MatchTime",
"period": 0.06,
"data_type": "double",
"time_display_mode": "Minutes and Seconds",
"red_start_time": 15,
"yellow_start_time": 30
}
},
{
"title": "RetractedLimit",
"x": 0.0,
"y": 384.0,
"width": 128.0,
"x": 512.0,
"y": 0.0,
"width": 256.0,
"height": 128.0,
"type": "Boolean Box",
"properties": {
@@ -90,9 +74,9 @@
},
{
"title": "Auto Chooser",
"x": 1024.0,
"y": 256.0,
"width": 384.0,
"x": 896.0,
"y": 384.0,
"width": 256.0,
"height": 128.0,
"type": "ComboBox Chooser",
"properties": {
@@ -103,9 +87,9 @@
},
{
"title": "Roller Percent Output",
"x": 0.0,
"x": 512.0,
"y": 256.0,
"width": 256.0,
"width": 384.0,
"height": 128.0,
"type": "Number Slider",
"properties": {
@@ -120,8 +104,8 @@
},
{
"title": "Shooter OVERRIDE Velocity",
"x": 128.0,
"y": 384.0,
"x": 768.0,
"y": 0.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
@@ -134,8 +118,8 @@
},
{
"title": "TRIM SHOOTER SPEED",
"x": 256.0,
"y": 256.0,
"x": 512.0,
"y": 384.0,
"width": 384.0,
"height": 128.0,
"type": "Number Slider",
@@ -151,8 +135,8 @@
},
{
"title": "Mode",
"x": 256.0,
"y": 384.0,
"x": 512.0,
"y": 128.0,
"width": 384.0,
"height": 128.0,
"type": "Text Display",
@@ -164,30 +148,49 @@
}
},
{
"title": "Time to rev",
"x": 768.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Large Text Display",
"title": "IsActive",
"x": 0.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Boolean Box",
"properties": {
"topic": "/AdvantageKit/RealOutputs/Time to rev",
"topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
"period": 0.06,
"data_type": "double"
"data_type": "boolean",
"true_color": 4283215696,
"false_color": 4294198070,
"true_icon": "None",
"false_icon": "None"
}
},
{
"title": "Arm angle extended",
"x": 1152.0,
"y": 128.0,
"width": 256.0,
"title": "RemainingInShift",
"x": 0.0,
"y": 256.0,
"width": 384.0,
"height": 128.0,
"type": "Text Display",
"type": "Match Time",
"properties": {
"topic": "/SmartDashboard/Arm angle extended",
"topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
"time_display_mode": "Minutes and Seconds",
"red_start_time": 15,
"yellow_start_time": 30
}
},
{
"title": "Phase",
"x": 0.0,
"y": 384.0,
"width": 384.0,
"height": 128.0,
"type": "Large Text Display",
"properties": {
"topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
"period": 0.06,
"data_type": "string"
}
}
]
@@ -527,8 +530,8 @@
},
{
"title": "Shooter Idle max current",
"x": 384.0,
"y": 512.0,
"x": 1024.0,
"y": 384.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
@@ -555,8 +558,8 @@
},
{
"title": "Shooter OVERRIDE Velocity",
"x": 640.0,
"y": 512.0,
"x": 1024.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
@@ -576,16 +579,59 @@
"layouts": [],
"containers": [
{
"title": "Auto Chooser",
"title": "Hub Dist",
"x": 640.0,
"y": 128.0,
"width": 256.0,
"height": 256.0,
"type": "Text Display",
"properties": {
"topic": "/AdvantageKit/RealOutputs/Hub Dist",
"period": 0.06,
"data_type": "double",
"show_submit_button": false
}
},
{
"title": "Distance Tolerence",
"x": 256.0,
"y": 128.0,
"width": 256.0,
"height": 256.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Distance Tolerence",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
}
},
{
"title": "Chassis positive speed offset",
"x": 0.0,
"y": 256.0,
"width": 256.0,
"height": 256.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Chassis positive speed offset",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
}
},
{
"title": "Chassis negative speed offset",
"x": 0.0,
"y": 0.0,
"width": 640.0,
"height": 768.0,
"type": "ComboBox Chooser",
"width": 256.0,
"height": 256.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Auto Chooser",
"topic": "/SmartDashboard/Chassis negative speed offset",
"period": 0.06,
"sort_options": false
"data_type": "double",
"show_submit_button": true
}
}
]
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -125,7 +125,7 @@ public class RobotContainer {
// );
private Command RobotRev = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
);
@@ -315,7 +315,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Idle);
m_robotIntake.rollerStop();
m_robotShooter.spinUpShooting();
m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
}))
.onFalse(new InstantCommand(() -> {
m_robotShooter.spinUpIdle();
@@ -352,7 +352,7 @@ public class RobotContainer {
// m_robotClimber.toggleDeployed();
// }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.indexerStalled();
}))
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+6 -3
View File
@@ -33,6 +33,8 @@ public class Shooter extends SubsystemBase {
public boolean badShooterVelocity;
public double distanceToHub = 5;
public double chassisXSpeed = 0;
public Shooter(
ShooterIO io,
@@ -66,7 +68,8 @@ public class Shooter extends SubsystemBase {
private ShooterMode mode = ShooterMode.Idle;
private boolean shooterButtonReady = false;
public void spinUpShooting() {
public void spinUpShooting(double chassisXSpeed) {
this.chassisXSpeed = chassisXSpeed;
this.mode = ShooterMode.Shooting;
}
@@ -157,14 +160,14 @@ public class Shooter extends SubsystemBase {
//revtime calculations
// double shooterAcceleration =
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond);
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
Logger.recordOutput("Time to rev", revTime);
switch (mode) {
case Shooting:
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
int bitmask = (
(shooterButtonReady ? 1 : 0) +
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+20 -6
View File
@@ -36,7 +36,8 @@ public class ShooterConstants {
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0);
public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0);
// Shoot mode tolerances
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
@@ -49,16 +50,29 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
//
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
// Model derived from points
// double speed =
// 1.11576*hubDistMeters*hubDistMeters +
// 0.318464*hubDistMeters +
// 30.6293;
double speed =
double speed;
if (Math.abs(chassisXSpeed) < 0.1) {
speed =
0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + MODEL_TRIM.get();
} else if (chassisXSpeed > 0) { // positive = further from hub
speed =
0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get();
} else { // negative = moving towards hub
speed =
0.0593402*hubDistMeters*hubDistMeters +
4.90561*hubDistMeters +
30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get();
}
// double speed =
// 0.00610938*hubDistMeters*hubDistMeters
@@ -82,8 +96,8 @@ public class ShooterConstants {
// Motor Configuration
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.05)
.withKI(0.20)
.withKP(0.02)
.withKI(0.15)
.withKD(0.0);
@@ -91,7 +105,7 @@ public class ShooterConstants {
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+9 -1
View File
@@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Offset Simple", aimOffset);
Logger.recordOutput("Offset Value", aimOffset);
}
@@ -512,6 +512,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
); // stop the modules without breaking
}
public double chassisXSpeeds(){
if (TimesNegativeOne.isRed) {
return chassisSpeeds.vxMetersPerSecond;
} else {
return -chassisSpeeds.vxMetersPerSecond;
}
}
public void stopModules() {
// stopped = true;
// swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());