FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

exposed more swerve

This commit is contained in:
Tyler Wilcox 2026-04-02 19:29:13 -04:00
parent cd9dee7098
commit 0584a07e80
2 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
View File

@ -2,6 +2,8 @@ package org.fairportrobotics.frc.robolib.drivesystems.swerve;
import java.util.Arrays; import java.util.Arrays;
import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2;
@ -16,6 +18,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
public class SwerveDriveSystem implements Subsystem{ public class SwerveDriveSystem implements Subsystem{
@ -196,7 +201,10 @@ public class SwerveDriveSystem implements Subsystem{
} }
public ChassisSpeeds getRobotRelativeSpeeds(){ public ChassisSpeeds getRobotRelativeSpeeds(){
return this.chassisSpeeds; if(!RobotBase.isReal()){
return driveKiniematics.toChassisSpeeds(getRequestedModuleStates());
}
return driveKiniematics.toChassisSpeeds(getActualModuleStates());
} }
/** /**

12
lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
View File

@ -18,6 +18,8 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
import com.ctre.phoenix6.sim.CANcoderSimState;
import com.ctre.phoenix6.sim.TalonFXSimState;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
@ -38,11 +40,14 @@ public class SwerveModule {
private TalonFX driveMotor; private TalonFX driveMotor;
private VelocityVoltage driveRequest = new VelocityVoltage(0); private VelocityVoltage driveRequest = new VelocityVoltage(0);
private TalonFXSimState driveSim;
private TalonFX steerMotor; private TalonFX steerMotor;
private PositionVoltage steerPosRequest = new PositionVoltage(0); private PositionVoltage steerPosRequest = new PositionVoltage(0);
private TalonFXSimState steerSim;
private CANcoder steerEncoder; private CANcoder steerEncoder;
private CANcoderSimState steerEncoderSim;
private String moduleName; private String moduleName;
@ -75,16 +80,19 @@ public class SwerveModule {
steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
steerEncoder.optimizeBusUtilization(); steerEncoder.optimizeBusUtilization();
steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset)); steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
steerEncoderSim = steerEncoder.getSimState();
driveMotor = new TalonFX(driveMotorId, canBusObj); driveMotor = new TalonFX(driveMotorId, canBusObj);
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
driveMotor.optimizeBusUtilization(); driveMotor.optimizeBusUtilization();
driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted)); driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted));
driveSim = driveMotor.getSimState();
steerMotor = new TalonFX(steerMotorId, canBusObj); steerMotor = new TalonFX(steerMotorId, canBusObj);
steerMotor.optimizeBusUtilization(); steerMotor.optimizeBusUtilization();
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
// steerMotor.setNeutralMode(NeutralModeValue.Brake); // steerMotor.setNeutralMode(NeutralModeValue.Brake);
steerSim = steerMotor.getSimState();
this.moduleLocation = moduleLocation; this.moduleLocation = moduleLocation;
@ -167,6 +175,10 @@ public class SwerveModule {
SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations()); SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations());
} }
public void simulationPeriodic(){
// driveSim.se
}
public String getModuleName(){ public String getModuleName(){
return moduleName; return moduleName;
} }