84 lines
3.3 KiB
Java
84 lines
3.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.subsystems;
|
|
|
|
import java.io.File;
|
|
import java.io.IOException;
|
|
|
|
import edu.wpi.first.cameraserver.CameraServer;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|
import edu.wpi.first.wpilibj.Filesystem;
|
|
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
|
import com.revrobotics.CANSparkMax;
|
|
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
|
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|
import edu.wpi.first.wpilibj.Filesystem;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.Constants;
|
|
import swervelib.SwerveDrive;
|
|
import swervelib.encoders.CanAndCoderSwerve;
|
|
import swervelib.parser.SwerveParser;
|
|
|
|
public class Drive extends SubsystemBase {
|
|
ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard");
|
|
ShuffleboardLayout avancer = Shuffleboard.getTab("Dashboard")
|
|
.getLayout("Avancer", BuiltInLayouts.kList)
|
|
.withSize(1, 4)
|
|
.withPosition(4, 0);
|
|
ShuffleboardLayout reculer = Shuffleboard.getTab("Dashboard")
|
|
.getLayout("Reculer", BuiltInLayouts.kList)
|
|
.withSize(1, 4)
|
|
.withPosition(3,0);
|
|
SwerveDrive swerveDrive;
|
|
File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve");
|
|
/* final CanAndCoderSwerve avantdroitdrive = new CanAndCoderSwerve(Constants.avantdroitDrive);
|
|
final CanAndCoderSwerve avantgauchedrive = new CanAndCoderSwerve(Constants.avantgaucheDrive);
|
|
final CanAndCoderSwerve arrieregauchedrive = new CanAndCoderSwerve(Constants.arrieregaucheDrive);
|
|
final CanAndCoderSwerve arrieredroitdrive = new CanAndCoderSwerve(Constants.arrieredroitDrive);
|
|
final CanAndCoderSwerve avantdroitangle = new CanAndCoderSwerve(Constants.avantdroitAngle);
|
|
final CanAndCoderSwerve avantgaucheangle = new CanAndCoderSwerve(Constants.avantgaucheAngle);
|
|
final CanAndCoderSwerve arrieregaucheangle = new CanAndCoderSwerve(Constants.arrieregaucheAngle);
|
|
final CanAndCoderSwerve arrieredroitangle = new CanAndCoderSwerve(Constants.arrieredroitAngle); */
|
|
public void drive(double x, double y, double zRotation){
|
|
swerveDrive.drive(new Translation2d(x, y), zRotation, false, false);
|
|
}
|
|
|
|
|
|
/** Creates a new Drive. */
|
|
public Drive() {
|
|
reculer.add("vitesse x", 1);
|
|
reculer.add("vitesse y", 2);
|
|
reculer.add("vitesse z", 3);
|
|
reculer.add("distance", 4);
|
|
avancer.add("vitesse x", 1);
|
|
avancer.add("vitesse y", 2);
|
|
avancer.add("vitesse z", 3);
|
|
avancer.add("distance", 4);
|
|
try {
|
|
this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive();
|
|
} 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
|
|
|
|
}
|
|
}
|