Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

start overhauling swerve drive class

Browse Source
This commit is contained in:
C4llSiqn
2025-01-06 21:34:40 -07:00
parent 7f93deef6e
commit f27ca5aab7
3 changed files with 47 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -2
View File
@@ -80,6 +80,8 @@ public final class Constants {
public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
private static final class ModuleSpecificConstants {
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0);
@@ -148,13 +150,13 @@ public final class Constants {
// The steer motor uses any SwerveModule.SteerRequestType control request with the
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.5)
.withKS(0.1).withKV(1.91).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
.withKP(0.1).withKI(0).withKD(0)
.withKS(0).withKV(0.124);
}
src/main/java/frc4388/robot/RobotMap.java
+1
View File
@@ -43,6 +43,7 @@ public class RobotMap {
Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT
);
void configureDriveMotorControllers() {}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+42 -49
View File
@@ -6,6 +6,11 @@ package frc4388.robot.subsystems;
import java.util.logging.Level;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -30,22 +35,7 @@ import frc4388.utility.Status.Report;
import frc4388.utility.Status.ReportLevel;
public class SwerveDrive extends Subsystem {
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;
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
private int gear_index;
private boolean stopped = false;
@@ -59,16 +49,12 @@ public class SwerveDrive extends Subsystem {
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
super();
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
this.swerveDriveTrain = swerveDriveTrain;
reset_index();
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
@@ -84,40 +70,47 @@ public class SwerveDrive extends Subsystem {
}
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (fieldRelative) {
double rot = 0;
// if (rightStick.getNorm() > 0.05 &&
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getY()*rotSpeedAdjust)
);
// double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot_correction = 0;
// rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
// if (rightStick.getNorm() > 0.05) {
// rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
// swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
// .withVelocityX(leftStick.getX()*speedAdjust)
// .withVelocityY(leftStick.getY()*speedAdjust)
// .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
// );
// // SmartDashboard.putBoolean("drift correction", false);
// stopped = false;
// } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) {
// stopModules();
// stopped = true;
// }
// SmartDashboard.putBoolean("drift correction", true);
// // SmartDashboard.putBoolean("drift correction", true);
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
// // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// }
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
// chassisSpeeds = chassisSpeeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
// // Convert field-relative speeds to robot-relative speeds.
// // chassisSpeeds = chassisSpeeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}