PLS5618/robot-2024
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of https://git.demerso.net/pls5618/2024/robot

Browse Source
This commit is contained in:
Olivier Dubois
2024-02-05 18:08:29 -05:00
parent 4330ab07f4 353ea19d71
commit b4496c8987
2 changed files with 19 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/java/frc/robot/subsystem/Drive.java
View File

@ -7,10 +7,14 @@ package frc.robot.subsystem;
import java.io.File;
import java.io.IOException;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import swervelib.SwerveDrive;
@ -28,6 +32,17 @@ public class Drive extends SubsystemBase {
}
public Drive() {
AutoBuilder.configureHolonomic(swerveDrive::getPose, swerveDrive::resetOdometry, swerveDrive::getRobotVelocity, swerveDrive::setChassisSpeeds, new HolonomicPathFollowerConfig(
new PIDConstants(5,0,0), new PIDConstants(5,0,0), 4.5,
0.275,
new ReplanningConfig()
), ()->{
var alliance = DriverStation.getAlliance();
if(alliance.isPresent()){
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
}, this);
try {
this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
} catch (IOException e) {