Skip to content

Commit

Permalink
Merge pull request #228 from FRC5010/dev
Browse files Browse the repository at this point in the history 
Merge Swerve configuration test changes
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch authored Oct 4, 2024
2 parents 8937401 + 6d28bc4 commit ed3e23e
Show file tree
Hide file tree
Showing 4 changed files with 134 additions and 72 deletions.
10 changes: 3 additions & 7 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,8 @@ public void teleopInit()
if (m_autonomousCommand != null)
{
m_autonomousCommand.cancel();
} else {
CommandScheduler.getInstance().cancelAll();
}
m_robotContainer.setDriveMode();
m_robotContainer.setMotorBrake(true);
Expand All @@ -143,13 +145,7 @@ public void testInit()
{
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
try
{
new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve"));
} catch (IOException e)
{
throw new RuntimeException(e);
}
m_robotContainer.setDriveMode();
}

/**
Expand Down
132 changes: 73 additions & 59 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,11 @@
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.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
Expand All @@ -32,6 +35,50 @@ public class RobotContainer
// The robot's subsystems and commands are defined here...
private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the rotational velocity
// buttons are quick rotation positions to different ways to face
// WARNING: default buttons are on the same buttons as the ones defined in configureBindings
AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
OperatorConstants.LEFT_X_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getRightX(),
OperatorConstants.RIGHT_X_DEADBAND),
driverXbox.getHID()::getYButtonPressed,
driverXbox.getHID()::getAButtonPressed,
driverXbox.getHID()::getXButtonPressed,
driverXbox.getHID()::getBButtonPressed);

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the desired angle NOT angular rotation
Command driveFieldOrientedDirectAngle = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX(),
() -> driverXbox.getRightY());

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the angular velocity of the robot
Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY() * -1, OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX() * -1, OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX() * -1);

Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRawAxis(2));

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand All @@ -40,54 +87,6 @@ public RobotContainer()
{
// Configure the trigger bindings
configureBindings();

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the rotational velocity
// buttons are quick rotation positions to different ways to face
// WARNING: default buttons are on the same buttons as the ones defined in configureBindings
AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
OperatorConstants.LEFT_X_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getRightX(),
OperatorConstants.RIGHT_X_DEADBAND),
driverXbox.getHID()::getYButtonPressed,
driverXbox.getHID()::getAButtonPressed,
driverXbox.getHID()::getXButtonPressed,
driverXbox.getHID()::getBButtonPressed);

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the desired angle NOT angular rotation
Command driveFieldOrientedDirectAngle = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX(),
() -> driverXbox.getRightY());

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the angular velocity of the robot
Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX() * 0.5);

Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRawAxis(2));

drivebase.setDefaultCommand(
!RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
}

/**
Expand All @@ -99,16 +98,31 @@ public RobotContainer()
*/
private void configureBindings()
{
// Schedule `ExampleCommand` when `exampleCondition` changes to `true`

driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.b().whileTrue(
Commands.deferredProxy(() -> drivebase.driveToPose(
new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
));
driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
// driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
if (DriverStation.isTest()) {
driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2));
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
drivebase.setDefaultCommand(
!RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim);
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.b().whileTrue(
Commands.deferredProxy(() -> drivebase.driveToPose(
new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
));
driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
drivebase.setDefaultCommand(
!RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
}
}

/**
Expand All @@ -124,7 +138,7 @@ public Command getAutonomousCommand()

public void setDriveMode()
{
//drivebase.setDefaultCommand();
configureBindings();
}

public void setMotorBrake(boolean brake)
Expand Down
60 changes: 56 additions & 4 deletions src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,23 @@

package frc.robot.subsystems.swervedrive;

import java.io.File;
import java.util.Arrays;
import java.util.function.DoubleSupplier;

import org.photonvision.PhotonCamera;
import org.photonvision.targeting.PhotonPipelineResult;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -23,19 +32,18 @@
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import frc.robot.Constants;
import frc.robot.Constants.AutonConstants;
import java.io.File;
import java.util.function.DoubleSupplier;
import org.photonvision.PhotonCamera;
import org.photonvision.targeting.PhotonPipelineResult;
import swervelib.SwerveController;
import swervelib.SwerveDrive;
import swervelib.SwerveDriveTest;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.parser.SwerveParser;
Expand Down Expand Up @@ -351,6 +359,50 @@ public Command sysIdAngleMotorCommand()
3.0, 5.0, 3.0);
}

/**
* Returns a Command that centers the modules of the SwerveDrive subsystem.
*
* @return a Command that centers the modules of the SwerveDrive subsystem
*/
public Command centerModulesCommand() {
return run(() -> Arrays.asList(swerveDrive.getModules())
.forEach(it -> it.setAngle(0.0)));
}

/**
* Returns a Command that drives the swerve drive to a specific distance at a given speed.
*
* @param distanceInMeters the distance to drive in meters
* @param speedInMetersPerSecond the speed at which to drive in meters per second
* @return a Command that drives the swerve drive to a specific distance at a given speed
*/
public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) {
return Commands.deferredProxy(
() -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this)
.until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters)
);
}

/**
* Sets the maximum speed of the swerve drive.
*
* @param maximumSpeed the maximum speed to set for the swerve drive in meters per second
*/
public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) {
swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, false, swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
}

/**
* Replaces the swerve module feedforward with a new SimpleMotorFeedforward object.
*
* @param kS the static gain of the feedforward
* @param kV the velocity gain of the feedforward
* @param kA the acceleration gain of the feedforward
*/
public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
}

/**
* Command to drive the robot using translative values and heading as angular velocity.
*
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/swervelib/SwerveDriveTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -356,8 +356,8 @@ public static void logAngularMotorVoltage(SwerveModule module, SysIdRoutineLog l
public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied)
{
double power = powerSupplied.get();
double angle = module.getAbsolutePosition();
double velocity = module.getAbsoluteEncoder().getVelocity();
double angle = module.getAngleMotor().getPosition();
double velocity = module.getAngleMotor().getVelocity();
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity",
Expand Down

0 comments on commit ed3e23e

Please sign in to comment.