Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Auto behavior is better with hokey fix, coming out of auto into teleop has ok field oriented, resetting heading is weird.

Browse Source
This commit is contained in:
Bradley Bickford 2024-03-10 12:27:22 -04:00
parent 3046f2d256
commit 5f15633044
2 changed files with 27 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/main/java/frc/robot/RobotContainer.java
View File

@ -17,6 +17,7 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -53,6 +54,7 @@ public class RobotContainer {
private CommandXboxController operator;
private Trigger isTeleopTrigger;
private Trigger isEnabledTrigger;
private SendableChooser<Command> autoChooser;
@ -65,6 +67,7 @@ public class RobotContainer {
private int speakerTag;
private boolean setAmpAngle;
private boolean invertXYDrive;
public RobotContainer() {
/*try {
@ -107,18 +110,7 @@ public class RobotContainer {
operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
ampTagID = 5;
sourceTagID = 9;
speakerTag = 4;
} else {
ampTagID = 6;
sourceTagID = 1;
speakerTag = 8;
}
isEnabledTrigger = new Trigger(DriverStation::isEnabled);
setAmpAngle = false;
@ -135,10 +127,30 @@ public class RobotContainer {
private void configureBindings() {
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
isEnabledTrigger.onTrue(
new InstantCommand(
() -> {
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
ampTagID = 5;
sourceTagID = 9;
speakerTag = 4;
invertXYDrive = true;
} else {
ampTagID = 6;
sourceTagID = 1;
speakerTag = 8;
invertXYDrive = false;
}
}
)
);
drivetrain.setDefaultCommand(
drivetrain.teleopCommand(
() -> { return -driver.getLeftY(); },
() -> { return -driver.getLeftX(); },
() -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); },
() -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); },
() -> { return -driver.getRightX(); },
OIConstants.kTeleopDriveDeadband
)

2
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -299,7 +299,7 @@ public class Drivetrain extends SubsystemBase {
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
//speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
m_frontLeft.setDesiredState(states[0]);