Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make swerve drive work. Some stuff on drift correction

Browse Source
This commit is contained in:
Michael Mikovsky
2024-07-25 11:41:41 -06:00
parent eb232fbe81
commit c08506dd5e
6 changed files with 103 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+16 -16
View File
@@ -40,28 +40,28 @@ public final class Constants {
public static final double TURBO_SPEED = 4.0;
public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 220;
public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = -279.08349884;
public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656;
public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25;
public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25;
public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25;
}
public static final class IDs {
public static final int RIGHT_FRONT_WHEEL_ID = 3;
public static final int RIGHT_FRONT_STEER_ID = 2;
public static final int RIGHT_FRONT_ENCODER_ID = 12;
public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10;
// public static final int LEFT_FRONT_WHEEL_ID = 4;
// public static final int LEFT_FRONT_STEER_ID = 5;
// public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11;
// public static final int LEFT_BACK_WHEEL_ID = 6;
// public static final int LEFT_BACK_STEER_ID = 7;
// public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
// public static final int RIGHT_BACK_WHEEL_ID = 8;
// public static final int RIGHT_BACK_STEER_ID = 9;
// public static final int RIGHT_BACK_ENCODER_ID = 13;
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
}
public static final class PIDConstants {
src/main/java/frc4388/robot/RobotContainer.java
+2 -1
View File
@@ -179,7 +179,8 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+24 -24
View File
@@ -48,27 +48,27 @@ public class RobotMap {
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
// public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
// public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
/* Swreve Drive Subsystem */
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
/* Swreve Drive Subsystem */
// public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
// public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
// public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
// public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
// public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
// public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
// public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
// public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
// public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
// public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
// public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
// public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
/* Shooter Subsystem */
// public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
@@ -104,7 +104,7 @@ public class RobotMap {
// rightBackSteer.configFactoryDefault();
// // config open loop ramp
// leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
@@ -117,7 +117,7 @@ public class RobotMap {
// rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// // config closed loop ramp
// leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
@@ -139,7 +139,7 @@ public class RobotMap {
// leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// // set neutral mode
@@ -156,9 +156,9 @@ public class RobotMap {
// // initialize SwerveModules
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
this.leftFront = this.rightFront;
this.leftBack = this.rightFront;
this.rightBack = this.rightFront;
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
// this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
// this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+23 -9
View File
@@ -10,10 +10,12 @@ 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.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotUnits;
public class SwerveDrive extends SubsystemBase {
@@ -68,6 +70,9 @@ public class SwerveDrive extends SubsystemBase {
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0;
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
if (fieldRelative) {
double rot = 0;
@@ -75,7 +80,8 @@ public class SwerveDrive extends SubsystemBase {
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX();
rot_correction = 0;
// rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
@@ -86,7 +92,7 @@ public class SwerveDrive extends SubsystemBase {
// SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
@@ -95,7 +101,8 @@ public class SwerveDrive extends SubsystemBase {
// 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.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
// chassisSpeeds = chassisSpeeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (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);
}
@@ -120,8 +127,8 @@ public class SwerveDrive extends SubsystemBase {
}
// SmartDashboard.putBoolean("drift correction", true);
// double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
@@ -137,9 +144,15 @@ public class SwerveDrive extends SubsystemBase {
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
Translation2d rightStick = new Translation2d(-rightX, rightY);
// Translation2d rightStick = new Translation2d(-rightX, rightY);
double rightX = rightStick.getX();
double rightY = rightStick.getY();
double rot_correction = 0;
// double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
if(fieldRelative) {
double rot = 0;
@@ -157,7 +170,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + 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);
}
@@ -191,7 +204,7 @@ public class SwerveDrive extends SubsystemBase {
}
public double getGyroAngle() {
return gyro.getAngle();
return -gyro.getAngle();
}
public void add180() {
@@ -238,7 +251,8 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run\
// SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget);
}
public void shiftDown() {
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+36 -4
View File
@@ -17,7 +17,10 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
@@ -26,6 +29,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.hardware.CANcoder;
@@ -58,7 +62,34 @@ public class SwerveModule extends SubsystemBase {
this.angleMotor = angleMotor;
this.encoder = encoder;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
var motorCfg = new TalonFXConfiguration()
.withOpenLoopRamps(
new OpenLoopRampsConfigs()
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
).withClosedLoopRamps(
new ClosedLoopRampsConfigs()
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake)
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
);
driveMotor.getConfigurator().apply(motorCfg);
TalonFXConfiguration angleConfig = new TalonFXConfiguration()
.withOpenLoopRamps(
new OpenLoopRampsConfigs()
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
).withClosedLoopRamps(
new ClosedLoopRampsConfigs()
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake)
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
);
angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
angleConfig.Slot0.kP = swerveGains.kP;
@@ -71,6 +102,7 @@ public class SwerveModule extends SubsystemBase {
CANcoderConfiguration canconfig = new CANcoderConfiguration();
canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
canconfig.MagnetSensor.MagnetOffset = offset;
encoder.getConfigurator().apply(canconfig);
rotateToAngle(0);
@@ -127,17 +159,17 @@ public class SwerveModule extends SubsystemBase {
public double getAngularVel() {
// return this.angleMotor.getSelectedSensorVelocity();
return 0;
return angleMotor.getVelocity().getValueAsDouble();
}
public double getDrivePos() {
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
return 0;
return driveMotor.getPosition().getValueAsDouble();
}
public double getDriveVel() {
// return this.driveMotor.getSelectedSensorVelocity(0);
return 0.0;
return driveMotor.getVelocity().getValueAsDouble();
}
public void stop() {
src/main/java/frc4388/utility/RobotGyro.java
+2 -2
View File
@@ -186,7 +186,7 @@ public class RobotGyro implements Gyro {
m_pigeon.getAngle();
var rotation = m_pigeon.getRotation3d();
return new double[] {rotation.getX(), (rotation.getY() - pitchZero), (rotation.getZ() - rollZero)};
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
}
@Override
@@ -197,7 +197,7 @@ public class RobotGyro implements Gyro {
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
return getPigeonAngles()[2];
} else {
return m_navX.getAngle();
}