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

Functional driving of swerve drive, still some orginizational things in constants that need to be fixed

Browse Source
This commit is contained in:
C4llSiqn
2025-01-08 18:32:02 -07:00
parent da2a1e46fa
commit c1f4829eaf
3 changed files with 47 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+26 -22
View File
@@ -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)
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -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;
/**
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+20 -7
View File
@@ -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<TalonFX, TalonFX, CANcoder> 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<Pose2d> e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds());
if(e.isPresent())
field.setRobotPose(e.get());
}
private void reset_index() {