Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

cleanup before master merge

Browse Source
This commit is contained in:
aarav18
2023-02-02 20:54:12 -07:00
parent e3d591674a
commit eb8a4ac6be
9 changed files with 9 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+1 -79
View File
@@ -4,21 +4,13 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
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.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase {
@@ -29,8 +21,6 @@ public class SwerveDrive extends SubsystemBase {
private SwerveModule[] modules;
// private RobotGyro gyro3
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));
@@ -61,45 +51,7 @@ public class SwerveDrive extends SubsystemBase {
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
// public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// Translation2d speed = new Translation2d(-xSpeed, ySpeed);
// double mag = speed.getNorm();
// speed = speed.times(mag * speedAdjust);
// double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
// double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
// // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
// // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
// //);
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
// setModuleStates(states);
// }
// public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
// // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
// Translation2d speed = new Translation2d(-leftX, leftY);
// speed = speed.times(speed.getNorm() * speedAdjust);
// // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
// // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
// // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
// double xSpeedMetersPerSecond = -speed.getX();
// double ySpeedMetersPerSecond = speed.getY();
// // chassisSpeeds = fieldRelative
// // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
// // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
// // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
// ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds);
// setModuleStates(states);
// }
// ! experimental WPILib swerve drive example
// WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
@@ -179,32 +131,6 @@ public class SwerveDrive extends SubsystemBase {
// );
// }
public void runAllDriveMotors(double output) {
this.leftFront.driveMotor.set(output);
this.rightFront.driveMotor.set(output);
this.leftBack.driveMotor.set(output);
this.rightBack.driveMotor.set(output);
}
public void runAllSteerMotors(double output) {
this.leftFront.angleMotor.set(output);
this.rightFront.angleMotor.set(output);
this.leftBack.angleMotor.set(output);
this.rightBack.angleMotor.set(output);
}
public void rotateCANCodersToAngle(double angle) {
for (SwerveModule module : this.modules) {
module.rotateToAngle(angle);
}
}
public void resetCANCoders(double position) {
for (SwerveModule module : this.modules) {
module.reset(position);
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@@ -213,10 +139,6 @@ public class SwerveDrive extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
// updateOdometry();
SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees());
SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees());
SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees());
SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees());
}
/**