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

Drive shoot auto and shooter tolerence

Browse Source
This commit is contained in:
mimigamin
2026-03-20 20:01:06 -06:00
parent 69677d5ae9
commit 95c8a167a5
8 changed files with 351 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+11 -1
View File
@@ -9,6 +9,7 @@ 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.units.measure.AngularVelocity;
@@ -114,12 +115,19 @@ public class Shooter extends SubsystemBase {
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Shooter", state);
io.updateInputs(state);
// Get robot positon and speeds
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
@@ -171,8 +179,10 @@ public class Shooter extends SubsystemBase {
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b001: // No errors and shoot button is pressed
case 0b001: // No errors and shoot button is pressed
if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){ // robot is aimed at hub
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
}
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;