Skip to content

Commit

Permalink
Merge branch 'dev' into feat/odometry
Browse files Browse the repository at this point in the history
  • Loading branch information
@JayK445
JayK445 committed Nov 27, 2024
2 parents 591d76b + 19d149d commit c5fbcdb
Show file tree
Hide file tree
Showing 8 changed files with 193 additions and 146 deletions.
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static double PERIODIC_LOOP_SEC = 0.02;

public static RobotType ROBOT_TYPE = RobotType.COMP;

public static Mode getRobotMode() {
Expand Down
50 changes: 13 additions & 37 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,11 @@

package frc.robot;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants.Mode;
import frc.robot.subsystems.flywheels.Flywheels;
import frc.robot.subsystems.flywheels.Flywheels.VelocityTarget;
import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX;
import frc.robot.subsystems.rollers.Rollers;
import frc.robot.subsystems.rollers.Rollers.RollerState;
import frc.robot.subsystems.rollers.intake.Intake;
import frc.robot.subsystems.rollers.intake.IntakeIOTalonFX;
import frc.robot.subsystems.swerve.Drive;
Expand Down Expand Up @@ -95,44 +92,23 @@ public RobotContainer() {

private void configureBindings() {
// -----Driver Controls-----
/*swerve.setDefaultCommand(
swerve
.run(
() -> {
swerve.driveTeleopController(
-driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX());
})
.withName("Drive Teleop"));*/
swerve.setDefaultCommand(
swerve
.run(
() -> {
swerve.driveTeleopController(
-driverA.getLeftY(),
-driverA.getLeftX(),
driverA.getLeftTriggerAxis() - driverA.getRightTriggerAxis());
})
.withName("Drive Teleop"));

driverA.start().onTrue(swerve.zeroGyroCommand());

// -----Intake Controls-----
driverA.x().whileTrue(rollers.setTargetCommand(RollerState.INTAKE));

// -----Flywheel Controls-----
//
driverA
.y()
.onTrue(
new InstantCommand(
() -> {
flywheels.setVelocityTarget(VelocityTarget.SHOOT);
},
flywheels));
driverA
.b()
.onTrue(
new InstantCommand(
() -> {
flywheels.setVelocityTarget(VelocityTarget.SLOW);
},
flywheels));
driverA
.a()
.onTrue(
new InstantCommand(
() -> {
flywheels.setVelocityTarget(VelocityTarget.IDLE);
},
flywheels));

}

private void configureAutos() {}
Expand Down
59 changes: 27 additions & 32 deletions src/main/java/frc/robot/subsystems/swerve/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG;
import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
Expand All @@ -12,8 +11,11 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.RobotState;
import frc.robot.subsystems.swerve.controllers.TeleopController;
import java.util.Arrays;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand All @@ -36,16 +38,19 @@ public enum DriveModes {
@AutoLogOutput(key = "Swerve/YawOffset")
private Rotation2d gyroYawOffset = new Rotation2d(0);

private ChassisSpeeds teleopTargetSpeeds = new ChassisSpeeds();
private ChassisSpeeds targetSpeeds = new ChassisSpeeds();

private final TeleopController teleopController;

public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) {
this.gyroIO = gyroIO;

modules[0] = new Module(fl, 0);
modules[1] = new Module(fr, 1);
modules[2] = new Module(bl, 2);
modules[3] = new Module(br, 3);

teleopController = new TeleopController();
}

@Override
Expand Down Expand Up @@ -75,13 +80,17 @@ public void periodic() {

switch (driveMode) {
case TELEOP -> {
targetSpeeds = teleopTargetSpeeds;
targetSpeeds = teleopController.update(arbitraryYaw);
}
case TRAJECTORY -> {}
}

// run modules
SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(targetSpeeds);

/* use kinematics to get desired module states */
ChassisSpeeds discretizedSpeeds =
ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC);
SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(
moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity());

Expand All @@ -99,40 +108,26 @@ public void periodic() {
}

public void driveTeleopController(double xAxis, double yAxis, double omega) {
if (driveMode != DriveModes.TELEOP) { // auto align override?
driveMode = DriveModes.TELEOP;
}
if (DriverStation.isTeleopEnabled()) {
if (driveMode != DriveModes.TELEOP) {
driveMode = DriveModes.TELEOP;
}

// NWU convention
double theta = Math.atan2(xAxis, yAxis);
double radiusDeadband =
MathUtil.applyDeadband(Math.sqrt((xAxis * xAxis) + (yAxis + yAxis)), 0.07);
double radiusExp = Math.copySign(Math.pow(radiusDeadband, 1.5), radiusDeadband);

double xVelocity = radiusExp * Math.sin(theta) * DRIVE_CONFIG.maxLinearVelocity();
double yVelocity = radiusExp * Math.cos(theta) * DRIVE_CONFIG.maxLinearVelocity();

/*double xVelocity =
MathUtil.applyDeadband(Math.copySign(xAxis * xAxis, xAxis), 0.07)
* Swerve.DRIVE_CONFIG.maxLinearVelocity();
double yVelocity =
MathUtil.applyDeadband(Math.copySign(yAxis * yAxis, yAxis), 0.07)
* Swerve.DRIVE_CONFIG.maxLinearVelocity();*/
double radianVelocity =
MathUtil.applyDeadband(Math.copySign(omega * omega, omega), 0.07)
* DRIVE_CONFIG.maxAngularVelocity();

this.teleopTargetSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(xVelocity, yVelocity, radianVelocity, arbitraryYaw);

Logger.recordOutput("Swerve/Teleop/xVelocity", xVelocity);
Logger.recordOutput("Swerve/Teleop/yVelocity", yVelocity);
Logger.recordOutput("Swerve/Teleop/radianVelocity", radianVelocity);
teleopController.acceptJoystickInput(xAxis, yAxis, omega);
}
}

public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) {
if (DriverStation.isAutonomousEnabled()) {
driveMode = DriveModes.TRAJECTORY;
}
}

private void zeroGyro() {
gyroYawOffset = gyroInputs.yawPosition;
}

public Command zeroGyroCommand() {
return this.runOnce(() -> zeroGyro());
}
}
61 changes: 22 additions & 39 deletions src/main/java/frc/robot/subsystems/swerve/DriveConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,20 +42,20 @@ public class DriveConstants {

public static final int GYRO_ID = 0;

// fl, fr, bl, br
// fl, fr, bl, br; negate offsets
public static final ModuleConfig[] MODULE_CONFIGS =
switch (getRobotType()) {
case COMP -> new ModuleConfig[] {
new ModuleConfig(5, 6, 1, new Rotation2d(0), true, false),
new ModuleConfig(7, 8, 2, new Rotation2d(0), true, true),
new ModuleConfig(9, 10, 3, new Rotation2d(0), true, false),
new ModuleConfig(11, 12, 4, new Rotation2d(0), true, true)
new ModuleConfig(5, 6, 1, new Rotation2d(1.1397), true, false),
new ModuleConfig(7, 8, 2, new Rotation2d(0.8038), true, true),
new ModuleConfig(11, 12, 3, new Rotation2d(1.4327), true, false),
new ModuleConfig(9, 10, 4, new Rotation2d(-1.8208), true, true)
};
case DEV -> new ModuleConfig[] {
new ModuleConfig(2, 1, 27, new Rotation2d(1.954), true, false),
new ModuleConfig(13, 12, 26, new Rotation2d(1.465), true, true),
new ModuleConfig(4, 3, 24, new Rotation2d(2.612), true, false),
new ModuleConfig(11, 10, 25, new Rotation2d(-2.563), true, true)
new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false),
new ModuleConfig(13, 12, 26, new Rotation2d(0), true, true),
new ModuleConfig(4, 3, 24, new Rotation2d(0), true, false),
new ModuleConfig(11, 10, 25, new Rotation2d(0), true, true)
};
case SIM -> new ModuleConfig[] {
new ModuleConfig(0, 0, 0, new Rotation2d(0), true, false),
Expand All @@ -68,30 +68,16 @@ public class DriveConstants {
public static final ModuleConstants MODULE_CONSTANTS =
switch (getRobotType()) {
case COMP, SIM -> new ModuleConstants(
0, // steerkS
0, // steerkV
0, // steerkA
0, // steerkP
0, // steerkD
0, // drivekS
0, // drivekV
0, // drivekA
0, // drivekP
0, // drivekD
new Gains(0.25, 2.62, 0, 100, 0, 0), // revisit kP
new MotionProfileGains(4, 64, 640), // revisit all
new Gains(0.3, 0.63, 0, 2, 0, 0), // FIXME placeholder, to do
5.357142857142857,
21.428571428571427,
3.125);
case DEV -> new ModuleConstants(
0, // steerkS
0, // steerkV
0, // steerkA
11, // steerkP
0, // steerkD
0, // drivekS
0, // drivekV
0, // drivekA
1.5, // drivekP
0, // drivekD
new Gains(0, 0, 0, 11, 0, 0),
new MotionProfileGains(0, 0, 0),
new Gains(0, 0, 0, 1.5, 0, 0),
5.357142857142857,
21.428571428571427,
3.125);
Expand All @@ -114,22 +100,19 @@ public record ModuleConfig(
boolean driveInverted) {}

public record ModuleConstants(
double steerkS,
double steerkV,
double steerkA,
double steerkP,
double steerkD,
double drivekS,
double drivekV,
double drivekA,
double drivekP,
double drivekD,
Gains steerGains,
MotionProfileGains steerMotionGains,
Gains driveGains,
double driveReduction,
double steerReduction,
double couplingGearReduction) {}

public record TrajectoryFollowerConstants() {}

public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {}

public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {}

private enum Mk4iReductions {
MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)),
STEER(150 / 7);
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/swerve/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,10 @@ public void runToSetpoint(SwerveModuleState targetState) {

moduleIO.runDriveVelocitySetpoint(driveVelocityRads);

Logger.recordOutput("Swerve/Module" + index + "/SteerSetpoint", targetState.angle.getRadians());
Logger.recordOutput(
"Swerve/Module" + index + "/SteerError",
targetState.angle.getRadians() - inputs.steerAbsolutePostion.getRadians());
Logger.recordOutput("Swerve/Module" + index + "/DriveVelRadsScalar", driveVelocityRads);
}

Expand Down
10 changes: 8 additions & 2 deletions src/main/java/frc/robot/subsystems/swerve/ModuleIO.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.robot.subsystems.swerve;

import edu.wpi.first.math.geometry.Rotation2d;
import frc.robot.subsystems.swerve.DriveConstants.Gains;
import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains;
import org.littletonrobotics.junction.AutoLog;

public interface ModuleIO {
Expand All @@ -14,11 +16,15 @@ class ModuleIOInputs {
public double drivePositionMeters = 0;
public double driveVelocityRadsPerSec = 0;
public double driveAppliedVolts = 0;
public double driveSupplyCurrent = 0;
public double driveStatorCurrent = 0;

public Rotation2d steerAbsolutePostion = new Rotation2d();
public Rotation2d steerPosition = new Rotation2d();
public double steerVelocityRadsPerSec = 0;
public double steerAppliedVolts = 0;
public double steerSupplyCurrent = 0;
public double steerStatorCurrent = 0;
}

default void updateInputs(ModuleIOInputs inputs) {}
Expand All @@ -31,9 +37,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {}

default void runSteerPositionSetpoint(double angleRads) {}

default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {}
default void setDriveGains(Gains gains) {}

default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {}
default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {}

default void stop() {}
}
Loading

0 comments on commit c5fbcdb

Please sign in to comment.