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

not working

Browse Source
This commit is contained in:
mimigamin
2026-03-12 18:05:14 -06:00
parent c4b8e2972e
commit 3ef0a876f6
12 changed files with 28 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+14 -7
View File
@@ -40,6 +40,7 @@ import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.led.LED;
import frc4388.robot.subsystems.shooter.Shooter;
import frc4388.robot.subsystems.shooter.ShooterConstants;
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Lidar;
import frc4388.robot.subsystems.vision.Vision;
@@ -69,6 +70,7 @@ public class RobotContainer {
/* Subsystems */
// public final Lidar m_lidar = new Lidar();
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
//Testing of Colors
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
@@ -243,14 +245,19 @@ public class RobotContainer {
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
.onTrue(new InstantCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get()
);
}, m_robotSwerveDrive)
m_robotSwerveSIM.driveFacingPosition(
FieldPositions.HUB_POSITION
);
})
// () -> {
// m_robotSwerveDrive.driveFacingPosition(
// getDeadbandedDriverController().getLeft(),
// FieldPositions.HUB_POSITION,
// ShooterConstants.AIM_LEAD_TIME.get()
// );
// }, m_robotSwerveDrive)
// () -> {
// m_robotSwerveDrive.driveFacingVelocity(
// getDeadbandedDriverController().getLeft(),
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 152;
public static final String GIT_SHA = "4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72";
public static final String GIT_DATE = "2026-03-11 22:49:27 MDT";
public static final int GIT_REVISION = 153;
public static final String GIT_SHA = "c4b8e2972e7be654bd544019760a174afc4871ae";
public static final String GIT_DATE = "2026-03-12 00:08:36 MDT";
public static final String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-12 00:02:41 MDT";
public static final long BUILD_UNIX_TIME = 1773295361724L;
public static final String BUILD_DATE = "2026-03-12 18:03:03 MDT";
public static final long BUILD_UNIX_TIME = 1773360183045L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java
+1 -42
View File
@@ -132,48 +132,7 @@ public class SimpleSwerveSim implements SwerveIO {
}
public synchronized void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
System.out.println("It has worked!");
System.out.println("It has worked!");
System.out.println("It has worked!");
System.out.println("It has worked!");
System.out.println("It has worked!");
System.out.println("It has worked!");
System.out.println("It has worked!");
if (leftStick == null || fieldPos == null) return;
// current robot speed (robot-relative)
Translation2d robotSpeed = new Translation2d(vx, vy);
// lead the target by robot motion over aimLeadTime
Translation2d fieldPosLead = fieldPos.plus(robotSpeed.times(aimLeadTime));
// compute angle from robot to lead point (field frame)
Rotation2d toLead = fieldPosLead.minus(pose.getTranslation()).getAngle();
// Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle();
// compute shortest angle error (ang_error = desired - current) normalized to [-pi,pi]
double currentYaw = pose.getRotation().getRadians();
double desiredYaw = toLead.getRadians();
double error = desiredYaw - currentYaw;
// normalize
while (error > Math.PI) error -= 2 * Math.PI;
while (error < -Math.PI) error += 2 * Math.PI;
// simple P controller for rotation
final double KP_ROT = 2.0; // tune as needed
final double MAX_OMEGA = 6.0; // rad/s cap
double commandedOmega = KP_ROT * error;
if (commandedOmega > MAX_OMEGA) commandedOmega = MAX_OMEGA;
if (commandedOmega < -MAX_OMEGA) commandedOmega = -MAX_OMEGA;
this.omega = commandedOmega;
// apply translational command from leftStick (assume stick in [-1,1])
final double STICK_SPEED_MPS = 3.0; // 3 m/s at full stick; tune as needed
this.vx = leftStick.getX() * STICK_SPEED_MPS;
this.vy = leftStick.getY() * STICK_SPEED_MPS;
}
@Override
public synchronized void resetPose(Pose2d p) {
if (p == null) return;
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
-1
View File
@@ -362,7 +362,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// Drive with the robot facing towards a specific position
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
// Get the current speed of the robot
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,