From c1f4829eaf6f110374c60ebd601d32aa9c264497 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 8 Jan 2025 18:32:02 -0700 Subject: [PATCH] Functional driving of swerve drive, still some orginizational things in constants that need to be fixed --- src/main/java/frc4388/robot/Constants.java | 48 ++++++++++--------- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 27 ++++++++--- 3 files changed, 47 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d74a30..c42cad8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final double HALF_HEIGHT = HEIGHT / 2.d; // Mechanics - private static final double COUPLE_RATIO = 1; //todo: find + private static final double COUPLE_RATIO = 3; //todo: find private static final double DRIVE_RATIO = 6.12; private static final double STEER_RATIO = (150/7); private static final Distance WHEEL_RADIUS = Inches.of(2); @@ -80,39 +80,39 @@ 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 + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 private static final class ModuleSpecificConstants { //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } @@ -145,13 +145,17 @@ public final class Constants { public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() .withKP(50).withKI(0).withKD(0.32) .withKS(0).withKV(0).withKA(0); + + public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(10).withKI(0).withKD(0.3) + .withKS(0).withKV(0).withKA(0); public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(100).withKI(0).withKD(0.6) .withKS(0.1).withKV(1.91).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -218,12 +222,12 @@ public final class Constants { new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + // ).withOpenLoopRamps( + // new OpenLoopRampsConfigs() + // .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + // ).withClosedLoopRamps( + // new ClosedLoopRampsConfigs() + // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); private static final double SLIP_CURRENT = 100; // TODO: Tune??? } @@ -239,7 +243,7 @@ public final class Constants { .withSteerMotorGearRatio(STEER_RATIO) .withCouplingGearRatio(COUPLE_RATIO) .withWheelRadius(WHEEL_RADIUS) - .withSteerMotorGains(PIDConstants.CURRENT_SWERVE_ROT_GAINS) // ? + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 95787cd..4990efa 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -18,7 +18,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveModule; +// import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d8ef7b6..f127a84 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,11 +4,15 @@ package frc4388.robot.subsystems; +import java.util.Optional; + +import com.ctre.phoenix6.Utils; 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.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -18,6 +22,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.Constants.SwerveDriveConstants; @@ -41,13 +46,16 @@ public class SwerveDrive extends Subsystem { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private Field2d field = new Field2d(); + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { super(); - + + SmartDashboard.putData(field); + this.swerveDriveTrain = swerveDriveTrain; } - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // // rightStick.getAngle() @@ -67,13 +75,13 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput return; // don't bother doing swerve drive math and return early. - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); // double rot = 0; @@ -110,9 +118,9 @@ public class SwerveDrive extends Subsystem { // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); } } @@ -164,6 +172,11 @@ public class SwerveDrive extends Subsystem { // This method will be called once per scheduler run\ SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); + + Optional e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()); + + if(e.isPresent()) + field.setRobotPose(e.get()); } private void reset_index() {