Skip to content

Commit b084a26

Browse files
dabeycorndabeycorn
committed
Fixed issue where input based velocties weren't run
1 parent 03f8883 commit b084a26

File tree

13 files changed

+972
-874
lines changed

13 files changed

+972
-874
lines changed

‎simgui-ds.json‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,9 @@
11
{
2+
"System Joysticks": {
3+
"window": {
4+
"enabled": false
5+
}
6+
},
27
"keyboardJoysticks": [
38
{
49
"axisConfig": [

‎src/main/java/org/frc6423/frc2025/Robot.java‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,13 @@
88

99
import static org.frc6423.frc2025.Constants.*;
1010

11+
import edu.wpi.first.wpilibj.PS5Controller;
1112
import edu.wpi.first.wpilibj.RobotController;
1213
import edu.wpi.first.wpilibj.Threads;
1314
import edu.wpi.first.wpilibj2.command.CommandScheduler;
15+
import org.frc6423.frc2025.subsystems.swerve.SwerveSubsystem;
16+
import org.frc6423.frc2025.subsystems.swerve.constants.CompBotSwerveConfigs;
17+
import org.frc6423.frc2025.util.ControllerUtil;
1418
import org.littletonrobotics.junction.LogFileUtil;
1519
import org.littletonrobotics.junction.LoggedRobot;
1620
import org.littletonrobotics.junction.Logger;
@@ -20,6 +24,10 @@
2024

2125
public class Robot extends LoggedRobot {
2226

27+
private final PS5Controller m_driveController;
28+
29+
private final SwerveSubsystem m_swerveSubsystem;
30+
2331
public Robot() {
2432
// AKit init
2533
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
@@ -52,9 +60,16 @@ public Robot() {
5260

5361
RobotController.setBrownoutVoltage(6.0);
5462

63+
m_driveController = new PS5Controller(0);
5564
// Subsystem init
65+
m_swerveSubsystem = new SwerveSubsystem(new CompBotSwerveConfigs());
5666

5767
// Default Commands
68+
m_swerveSubsystem.setDefaultCommand(
69+
m_swerveSubsystem.teleopSwerveCommmand(
70+
ControllerUtil.applyDeadband(m_driveController::getLeftY),
71+
ControllerUtil.applyDeadband(m_driveController::getLeftX),
72+
ControllerUtil.applyDeadband(m_driveController::getRightX)));
5873
}
5974

6075
@Override

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java‎

Lines changed: 129 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/FIRSTTeam6423
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
6+
17
package org.frc6423.frc2025.subsystems.swerve;
28

39
import static org.frc6423.frc2025.Constants.kTickSpeed;
410

5-
import java.util.Arrays;
6-
7-
import org.frc6423.frc2025.subsystems.swerve.module.Module;
8-
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
9-
1011
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
1112
import edu.wpi.first.math.geometry.Pose2d;
1213
import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,98 +17,139 @@
1617
import edu.wpi.first.math.kinematics.SwerveModuleState;
1718
import edu.wpi.first.math.system.plant.DCMotor;
1819
import edu.wpi.first.wpilibj.DriverStation;
20+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
21+
import edu.wpi.first.wpilibj2.command.Command;
1922
import edu.wpi.first.wpilibj2.command.SubsystemBase;
23+
import java.util.Arrays;
24+
import java.util.function.DoubleSupplier;
25+
import org.frc6423.frc2025.subsystems.swerve.module.Module;
26+
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
2027

2128
public class SwerveSubsystem extends SubsystemBase {
22-
private final SwerveConfig m_config;
23-
24-
private final Module[] m_modules;
29+
private final SwerveConfig m_config;
2530

26-
private SwerveDriveKinematics m_kinematics;
27-
private SwerveDrivePoseEstimator m_poseEstimator;
31+
private final Module[] m_modules;
2832

29-
public SwerveSubsystem(SwerveConfig config) {
30-
// Create modules
31-
var moduleConfigs = config.getModuleConfigs();
32-
m_modules = new Module[moduleConfigs.length];
33-
Arrays.stream(moduleConfigs)
34-
.forEach((c) -> m_modules[c.kIndex] = new Module(c));
33+
private SwerveDriveKinematics m_kinematics;
34+
private SwerveDrivePoseEstimator m_poseEstimator;
3535

36-
// Create math objects
37-
m_kinematics = new SwerveDriveKinematics(config.getModuleLocs());
38-
m_poseEstimator = new SwerveDrivePoseEstimator(m_kinematics, getHeading(), getModulePoses(), new Pose2d());
36+
public SwerveSubsystem(SwerveConfig config) {
37+
// Create modules
38+
var moduleConfigs = config.getModuleConfigs();
39+
m_modules = new Module[moduleConfigs.length];
40+
Arrays.stream(moduleConfigs).forEach((c) -> m_modules[c.kIndex - 1] = new Module(c));
3941

40-
m_config = config;
41-
}
42-
43-
@Override
44-
public void periodic() {
45-
46-
// Stop module input when driverstation is disabled
47-
if (DriverStation.isDisabled()) {
48-
for (Module module : m_modules) {
49-
module.stop();
50-
}
51-
}
52-
}
53-
54-
/**
55-
* Runs swerve at desired robot relative velocity
56-
*
57-
* @param velocity desired velocity
58-
* @param openloopEnabled enable or disable open loop voltage control
59-
*/
60-
public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) {
61-
velocity = ChassisSpeeds.discretize(velocity, kTickSpeed);
62-
63-
SwerveModuleState[] desiredStates = m_kinematics.toSwerveModuleStates(velocity);
64-
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, m_config.getMaxLinearSpeedMetersPerSec());
65-
66-
for(int i = 0; i < desiredStates.length; i++) {
67-
if (openloopEnabled) {
68-
ChassisSpeeds currentVelocities = getVelocitiesRobotRelative();
69-
boolean focEnabled =
70-
Math.sqrt(
71-
Math.pow(currentVelocities.vxMetersPerSecond, 2)
72-
+ Math.pow(currentVelocities.vyMetersPerSecond, 2)) // converts linear velocity components to linear velocity
73-
< 1 * m_config.getMaxLinearSpeedMetersPerSec();
74-
75-
// Converts desired motor velocity into input voltage
76-
// OmegaRadsPerSec/(KvRadsPerVolt)
77-
double driveVoltage = desiredStates[i].speedMetersPerSecond / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt;
78-
m_modules[i].runSetpointOpenloop(driveVoltage, desiredStates[i].angle, focEnabled);
79-
} else {
80-
m_modules[i].runSetpoint(desiredStates[i]);
81-
}
82-
}
83-
}
42+
// Create math objects
43+
m_kinematics = new SwerveDriveKinematics(config.getModuleLocs());
44+
m_poseEstimator =
45+
new SwerveDrivePoseEstimator(m_kinematics, getHeading(), getModulePoses(), new Pose2d());
8446

85-
/** Gets current robot velocity (robot relative) */
86-
public ChassisSpeeds getVelocitiesRobotRelative() {
87-
return m_kinematics.toChassisSpeeds(getModuleStates());
88-
}
47+
m_config = config;
48+
}
8949

90-
/** Gets current robot heading */
91-
public Rotation2d getHeading() {
92-
return new Rotation2d();
93-
}
50+
@Override
51+
public void periodic() {
52+
Arrays.stream(m_modules).forEach((m) -> m.updateInputs());
9453

95-
/** Gets current robot field pose */
96-
public Pose2d getPose() {
97-
return m_poseEstimator.getEstimatedPosition();
54+
// Stop module input when driverstation is disabled
55+
if (DriverStation.isDisabled()) {
56+
for (Module module : m_modules) {
57+
module.stop();
58+
}
9859
}
99-
100-
/** Returns an array of module field positions */
101-
public SwerveModulePosition[] getModulePoses() {
102-
return Arrays.stream(m_modules)
103-
.map(Module::getModulePose)
104-
.toArray(SwerveModulePosition[]::new);
105-
}
106-
107-
/** Returns an array of module states */
108-
public SwerveModuleState[] getModuleStates() {
109-
return Arrays.stream(m_modules)
110-
.map(Module::getModuleState)
111-
.toArray(SwerveModuleState[]::new);
60+
}
61+
62+
/**
63+
* Takes axis inputs, scales them to velocties, and converts those velocties to field relative
64+
* speeds
65+
*
66+
* @param xSupplier + is towards alliance wall
67+
* @param ySupplier + is left of alliance side
68+
* @param omegaSupplier + is counterclockwise
69+
*/
70+
public Command teleopSwerveCommmand(
71+
DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) {
72+
return this.run(
73+
() -> {
74+
double maxSpeed = m_config.getMaxLinearSpeedMetersPerSec();
75+
76+
var fieldRelativeVelocities =
77+
new ChassisSpeeds(
78+
xSupplier.getAsDouble() * maxSpeed,
79+
ySupplier.getAsDouble() * maxSpeed,
80+
omegaSupplier.getAsDouble() * maxSpeed);
81+
fieldRelativeVelocities =
82+
ChassisSpeeds.fromFieldRelativeSpeeds(
83+
fieldRelativeVelocities,
84+
DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
85+
? getPose().getRotation()
86+
: getPose()
87+
.getRotation()
88+
.plus(
89+
Rotation2d.fromDegrees(180))); // Flips orientation if on red alliance
90+
91+
this.runVelocities(fieldRelativeVelocities, false);
92+
});
93+
}
94+
95+
/**
96+
* Runs swerve at desired robot relative velocities
97+
*
98+
* @param velocity desired velocities
99+
* @param openloopEnabled enable or disable open loop voltage control
100+
*/
101+
public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) {
102+
velocity = ChassisSpeeds.discretize(velocity, kTickSpeed);
103+
System.out.println(velocity.vxMetersPerSecond);
104+
105+
SwerveModuleState[] desiredStates = m_kinematics.toSwerveModuleStates(velocity);
106+
SwerveDriveKinematics.desaturateWheelSpeeds(
107+
desiredStates, m_config.getMaxLinearSpeedMetersPerSec());
108+
109+
for (int i = 0; i < desiredStates.length; i++) {
110+
if (openloopEnabled) {
111+
ChassisSpeeds currentVelocities = getVelocitiesRobotRelative();
112+
boolean focEnabled =
113+
Math.sqrt(
114+
Math.pow(currentVelocities.vxMetersPerSecond, 2)
115+
+ Math.pow(
116+
currentVelocities.vyMetersPerSecond,
117+
2)) // converts linear velocity components to linear velocity
118+
< 1 * m_config.getMaxLinearSpeedMetersPerSec();
119+
120+
// Converts desired motor velocity into input voltage
121+
// OmegaRadsPerSec/(KvRadsPerVolt)
122+
double driveVoltage =
123+
desiredStates[i].speedMetersPerSecond / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt;
124+
m_modules[i].runSetpointOpenloop(driveVoltage, desiredStates[i].angle, focEnabled);
125+
} else {
126+
m_modules[i].runSetpoint(desiredStates[i]);
127+
}
112128
}
129+
}
130+
131+
/** Gets current robot velocity (robot relative) */
132+
public ChassisSpeeds getVelocitiesRobotRelative() {
133+
return m_kinematics.toChassisSpeeds(getModuleStates());
134+
}
135+
136+
/** Gets current robot heading */
137+
public Rotation2d getHeading() {
138+
return new Rotation2d();
139+
}
140+
141+
/** Gets current robot field pose */
142+
public Pose2d getPose() {
143+
return m_poseEstimator.getEstimatedPosition();
144+
}
145+
146+
/** Returns an array of module field positions */
147+
public SwerveModulePosition[] getModulePoses() {
148+
return Arrays.stream(m_modules).map(Module::getModulePose).toArray(SwerveModulePosition[]::new);
149+
}
150+
151+
/** Returns an array of module states */
152+
public SwerveModuleState[] getModuleStates() {
153+
return Arrays.stream(m_modules).map(Module::getModuleState).toArray(SwerveModuleState[]::new);
154+
}
113155
}

0 commit comments

Comments
 (0)