mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
Ryan
80 lines
3.5 KiB
Java
80 lines
3.5 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 frc4388.robot.subsystems;
|
|
|
|
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
|
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
|
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
|
import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc4388.robot.Constants.SwerveDriveConstants;
|
|
import frc4388.utility.RobotGyro;
|
|
|
|
public class SwerveDrive extends SubsystemBase {
|
|
|
|
private SwerveModule leftFront;
|
|
private SwerveModule rightFront;
|
|
private SwerveModule leftBack;
|
|
private SwerveModule rightBack;
|
|
|
|
private SwerveModule[] modules;
|
|
|
|
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
|
|
|
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
|
|
|
private RobotGyro gyro;
|
|
|
|
/** Creates a new SwerveDrive. */
|
|
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
|
this.leftFront = leftFront;
|
|
this.rightFront = rightFront;
|
|
this.leftBack = leftBack;
|
|
this.rightBack = rightBack;
|
|
|
|
this.modules = new SwerveModule[] {leftFront, rightFront, leftBack, rightBack};
|
|
|
|
this.gyro = gyro;
|
|
}
|
|
|
|
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
|
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND;
|
|
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND;
|
|
|
|
SwerveModuleState[] states = kinematics.toSwerveModuleStates(
|
|
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
|
|
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
|
|
);
|
|
|
|
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
|
setModuleStates(states);
|
|
}
|
|
|
|
/**
|
|
* Set each module of the swerve drive to the corresponding desired state.
|
|
* @param desiredStates Array of module states to set.
|
|
*/
|
|
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
|
for (int i = 0; i < desiredStates.length; i++) {
|
|
SwerveModule module = modules[i];
|
|
SwerveModuleState state = desiredStates[i];
|
|
module.setDesiredState(state);
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|