65 lines
2.0 KiB
Java
65 lines
2.0 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
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;
|
|
import swervelib.parser.SwerveParser;
|
|
|
|
public class Drive extends SubsystemBase {
|
|
/** Creates a new Drive. */
|
|
|
|
SwerveDrive swerveDrive;
|
|
|
|
File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
|
|
|
|
public void drive(double x, double y, double zRotation){
|
|
swerveDrive.drive(new Translation2d(x*2, y*2), zRotation, true, false);
|
|
}
|
|
|
|
public Drive() {
|
|
try {
|
|
this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
|
|
} catch (IOException e) {
|
|
e.printStackTrace();
|
|
}
|
|
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);
|
|
}
|
|
|
|
public SwerveModulePosition[] distance(){
|
|
return swerveDrive.getModulePositions();
|
|
}
|
|
public void reset(){
|
|
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|