50 lines
1.3 KiB
Java
50 lines
1.3 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 edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|
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();
|
|
}
|
|
}
|
|
|
|
public SwerveModulePosition[] distance(){
|
|
return swerveDrive.getModulePositions();
|
|
}
|
|
public void reset(){
|
|
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|