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

Minor changes to Drivetrain to grant the ability to flip between field and robot relative controls. Plus a small amount of Shuffleboard stuff

Browse Source
This commit is contained in:
Bradley Bickford 2024-01-15 17:18:45 -05:00
parent f26c9e76c2
commit 6f9f53011e
4 changed files with 80 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
.OutlineViewer/outlineviewer.json
View File

@ -13,5 +13,18 @@
"open": true
},
"open": true
},
"retained": {
"Shuffleboard": {
"open": true
}
},
"transitory": {
"photonvision": {
"c922_Pro_Stream_Webcam": {
"open": true
},
"open": true
}
}
}

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

@ -12,18 +12,28 @@ import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain;
public class RobotContainer {
private static final String kAutoTabName = "Autonomous";
private static final String kTeleopTabName = "Teloperated";
private Drivetrain drivetrain;
private CommandXboxController primary;
private CommandXboxController secondary;
private Trigger isTeleopTrigger;
private SendableChooser<Command> autoChooser;
private int ampTagID;
@ -50,6 +60,8 @@ public class RobotContainer {
primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
@ -63,14 +75,18 @@ public class RobotContainer {
}
configureBindings();
shuffleboardSetup();
}
// The objective should be to have the subsystems expose methods that return commands
// that can be bound to the triggers provided by the CommandXboxController class.
// This mindset should help keep RobotContainer a little cleaner this year.
// This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
// but generally avoid any situation where the keyword "new" is involved
// but generally reduce/avoid any situation where the keyword "new" is involved if you're working
// with a subsystem
private void configureBindings() {
isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
drivetrain.setDefaultCommand(
drivetrain.teleopCommand(
primary::getLeftX,
@ -131,6 +147,34 @@ public class RobotContainer {
// This was originally a run while held, not sure that's really necessary, change it if need be
primary.rightBumper().onTrue(drivetrain.setXCommand());
/*
* This has been added because interest has been expressed in trying field relative vs
* robot relative control. This should <i>default</i> to field relative, but give the option
* for the person practicing driving to push start on the driver's controller to quickly switch to
* the other style.
*
* If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
* mechanism will need to be devised, this will work for now.
*/
primary.start().onTrue(drivetrain.toggleFieldRelativeControl());
}
private void shuffleboardSetup() {
ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName);
autoTab.add("Autonomous Selector", autoChooser)
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kComboBoxChooser);
//Always select the auto tab on startup
Shuffleboard.selectTab(kAutoTabName);
ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
}
public Command getAutonomousCommand() {

1
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -49,5 +49,6 @@ public final class DrivetrainConstants {
public static final boolean kGyroReversed = true;
// TODO Is this offset going to continue to be correct for this year
public static final double kRobotStartOffset = 180;
}

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

@ -14,6 +14,7 @@ import edu.wpi.first.util.WPIUtilJNI;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import com.kauailabs.navx.frc.AHRS;
@ -53,6 +54,8 @@ public class Drivetrain extends SubsystemBase {
// Odometry class for tracking robot pose
private final SwerveDrivePoseEstimator m_poseEstimator;
private boolean fieldRelativeControl;
// Slew rate filter variables for controlling lateral acceleration
private double m_currentRotation;
private double m_currentTranslationDir;
@ -107,6 +110,8 @@ public class Drivetrain extends SubsystemBase {
m_visualPoseProvider = visualPoseProvider;
fieldRelativeControl = true;
m_currentRotation = 0.0;
m_currentTranslationDir = 0.0;
m_currentTranslationMag = 0.0;
@ -151,6 +156,16 @@ public class Drivetrain extends SubsystemBase {
}
}
public boolean isFieldRelativeControl() {
return fieldRelativeControl;
}
public Command toggleFieldRelativeControl() {
return runOnce(() -> {
fieldRelativeControl = !fieldRelativeControl;
});
}
/**
* Returns the currently-estimated pose of the robot.
*
@ -205,7 +220,7 @@ public class Drivetrain extends SubsystemBase {
* field.
* @param rateLimit Whether to enable rate limiting for smoother control.
*/
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
public void drive(double xSpeed, double ySpeed, double rot, BooleanSupplier fieldRelative, boolean rateLimit) {
double xSpeedCommanded;
double ySpeedCommanded;
@ -259,7 +274,7 @@ public class Drivetrain extends SubsystemBase {
double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
fieldRelative.getAsBoolean()
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds(
@ -285,7 +300,7 @@ public class Drivetrain extends SubsystemBase {
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
true,
() -> fieldRelativeControl,
true
);
});
@ -308,7 +323,7 @@ public class Drivetrain extends SubsystemBase {
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
output,
true,
() -> fieldRelativeControl,
true
);
},
@ -340,12 +355,12 @@ public class Drivetrain extends SubsystemBase {
},
() -> { return 0; },
(output) -> {
// TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field.
// TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field, it shouldn't be affected by the drivers mode choice I don't think.
this.drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
output,
true,
() -> true,
true
);
},