Skip to content

Commit

Permalink
type?
Browse files Browse the repository at this point in the history
  • Loading branch information
carokhan committed Nov 29, 2024
1 parent c3dd943 commit 6d02609
Show file tree
Hide file tree
Showing 10 changed files with 195 additions and 113 deletions.
1 change: 1 addition & 0 deletions networktables.json
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
[]
98 changes: 98 additions & 0 deletions simgui-ds.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "78696e70757401000000000000000000",
"useGamepad": true
}
]
}
10 changes: 10 additions & 0 deletions simgui.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo"
}
},
"NetworkTables Info": {
"visible": true
}
}
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/BuildConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "BlockOfCheese2024";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 6;
public static final String GIT_SHA = "8b9ce8c04b9ca40f8fdeb380df43d3217945a1c6";
public static final String GIT_DATE = "2024-11-26 18:25:42 EST";
public static final int GIT_REVISION = 7;
public static final String GIT_SHA = "c3dd943f6b907ba21842b935230ddb749fa5f547";
public static final String GIT_DATE = "2024-11-28 17:39:46 EST";
public static final String GIT_BRANCH = "main";
public static final String BUILD_DATE = "2024-11-28 14:08:46 EST";
public static final long BUILD_UNIX_TIME = 1732820926220L;
public static final String BUILD_DATE = "2024-11-28 23:35:14 EST";
public static final long BUILD_UNIX_TIME = 1732854914679L;
public static final int DIRTY = 1;

private BuildConstants() {}
Expand Down
24 changes: 17 additions & 7 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -25,7 +26,8 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final Mode currentMode = Mode.REAL;
public static final Mode currentMode =
RobotBase.isSimulation() ? Mode.SIM : (RobotBase.isReal() ? Mode.REAL : Mode.REPLAY);

public static enum Mode {
/** Running on a real robot. */
Expand Down Expand Up @@ -112,13 +114,13 @@ public static class DriveConstants {
public static double kPTurnReal = 2.5; // 1.5?
public static double kDTurnReal = 0.0;

public static double kPDriveSim = 0.3;
public static double kDDriveSim = 0.0;
public static double kSDriveSim = 0.0;
public static double kVDriveSim = 2.0;
public static double kADriveSim = 0.0;
public static double kPDriveSim = 2.0;
public static double kDDriveSim = 0.2;
public static double kSDriveSim = 0.4;
public static double kVDriveSim = 1.93;
public static double kADriveSim = 0.25;

public static double kPTurnSim = 100.0;
public static double kPTurnSim = 2.5;
public static double kDTurnSim = 0.0;

public static double kPDriveReplay = 0.0;
Expand All @@ -130,4 +132,12 @@ public static class DriveConstants {
public static double kPTurnReplay = 0.0;
public static double kDTurnReplay = 0.0;
}

public static class ControlConstants {
public static final double deadband = 0.01;
}

public static class SimConstants {
public static final double loopTime = 0.02;
}
}
27 changes: 14 additions & 13 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.GyroIO;
import frc.robot.subsystems.drive.GyroIOPigeon2;
Expand All @@ -42,7 +41,7 @@ public class RobotContainer {
private final CommandXboxController controller = new CommandXboxController(0);

// Dashboard inputs
// private final LoggedDashboardChooser<Command> autoChooser;
// private final LoggedDashboardChooser<Command> autoChooser;

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
Expand Down Expand Up @@ -88,19 +87,22 @@ public RobotContainer() {
break;
}

// autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
// autoChooser = new LoggedDashboardChooser<>("Auto Choices",
// AutoBuilder.buildAutoChooser());

// Set up SysId routines
// autoChooser.addOption(
// "Drive SysId (Quasistatic Forward)",
// drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// "Drive SysId (Quasistatic Forward)",
// drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Drive SysId (Quasistatic Reverse)",
// drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// "Drive SysId (Quasistatic Reverse)",
// drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// autoChooser.addOption(
// "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
// "Drive SysId (Dynamic Forward)",
// drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
// autoChooser.addOption(
// "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
// "Drive SysId (Dynamic Reverse)",
// drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -114,10 +116,9 @@ public RobotContainer() {
*/
private void configureButtonBindings() {
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
drive.joystickDrive(
() -> controller.getLeftY(),
() -> controller.getLeftX(),
() -> -controller.getRightX()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller
Expand Down
77 changes: 0 additions & 77 deletions src/main/java/frc/robot/commands/DriveCommands.java

This file was deleted.

42 changes: 42 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,22 +15,27 @@

import static edu.wpi.first.units.Units.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants.ControlConstants;
import frc.robot.Constants.DriveConstants;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

Expand Down Expand Up @@ -197,6 +202,43 @@ public void stop() {
runVelocity(new ChassisSpeeds());
}

public Command joystickDrive(
DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) {
return this.run(
() -> {
// Apply deadband
double linearMagnitude =
MathUtil.applyDeadband(
Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()),
ControlConstants.deadband);
Rotation2d linearDirection =
new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
double omega =
MathUtil.applyDeadband(omegaSupplier.getAsDouble(), ControlConstants.deadband);

// Square values
linearMagnitude = linearMagnitude * linearMagnitude;
omega = Math.copySign(omega * omega, omega);

// Calcaulate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
.getTranslation();

// Convert to field relative speeds & send command
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
omega * getMaxAngularSpeedRadPerSec(),
isFlipped ? getRotation().plus(new Rotation2d(Math.PI)) : getRotation()));
});
}

/**
* Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
* return to their normal orientations the next time a nonzero velocity is requested.
Expand Down
Loading

0 comments on commit 6d02609

Please sign in to comment.