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

FIX and FAST

Browse Source
This commit is contained in:
Aarav Shah
2022-01-11 19:57:50 -07:00
parent a87c9a1e34
commit 06091a9bd9
5 changed files with 161 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -4
View File
@@ -42,10 +42,15 @@ public final class Constants {
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
// ofsets are in degrees // ofsets are in degrees
public static final float LEFT_FRONT_ENCODER_OFFSET = 0; //ofsets are in degrees
public static final float RIGHT_FRONT_ENCODER_OFFSET = 0; // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
public static final float LEFT_BACK_ENCODER_OFFSET = 0; // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
public static final float RIGHT_BACK_ENCODER_OFFSET = 0; // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
// swerve PID constants // swerve PID constants
public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_SLOT_IDX = 0;
src/main/java/frc4388/robot/RobotContainer.java
+2 -2
View File
@@ -54,8 +54,8 @@ public class RobotContainer {
/* Default Commands */ /* Default Commands */
// drives the swerve drive with a two-axis input from the driver controller // drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand( m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(), new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
// continually sends updates to the Blinkin LED controller to keep the lights on // continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+132 -120
View File
@@ -14,9 +14,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase { public class SwerveDrive extends SubsystemBase {
SwerveDriveKinematics m_kinematics; SwerveDriveKinematics m_kinematics;
@@ -28,147 +30,157 @@ public class SwerveDrive extends SubsystemBase {
private WPI_TalonFX m_leftBackWheelMotor; private WPI_TalonFX m_leftBackWheelMotor;
private WPI_TalonFX m_rightBackSteerMotor; private WPI_TalonFX m_rightBackSteerMotor;
private WPI_TalonFX m_rightBackWheelMotor; private WPI_TalonFX m_rightBackWheelMotor;
private CANCoder m_leftFrontEncoder; private CANCoder m_leftFrontEncoder;
private CANCoder m_rightFrontEncoder; private CANCoder m_rightFrontEncoder;
private CANCoder m_leftBackEncoder; private CANCoder m_leftBackEncoder;
private CANCoder m_rightBackEncoder; private CANCoder m_rightBackEncoder;
double halfWidth = SwerveDriveConstants.WIDTH / 2.d; double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d; double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
// setSwerveGains();
Translation2d m_frontLeftLocation =
new Translation2d(
Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation =
new Translation2d(
Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation =
new Translation2d(
Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation =
new Translation2d(
Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
//setSwerveGains();
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules; public SwerveModule[] modules;
public Gyro gyro; // TODO Add Gyro Lol public RobotGyro gyro; //TODO Add Gyro Lol
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor, WPI_TalonFX leftFrontWheelMotor,
WPI_TalonFX rightFrontSteerMotor, WPI_TalonFX rightFrontWheelMotor,
WPI_TalonFX leftBackSteerMotor, WPI_TalonFX leftBackWheelMotor,
WPI_TalonFX rightBackSteerMotor, WPI_TalonFX rightBackWheelMotor,
CANCoder leftFrontEncoder, CANCoder rightFrontEncoder,
CANCoder leftBackEncoder, CANCoder rightBackEncoder) {
m_leftFrontSteerMotor = leftFrontSteerMotor;
m_leftFrontWheelMotor = leftFrontWheelMotor;
m_rightFrontSteerMotor = rightFrontSteerMotor;
m_rightFrontWheelMotor = rightFrontWheelMotor;
m_leftBackSteerMotor = leftBackSteerMotor;
m_leftBackWheelMotor = leftBackWheelMotor;
m_rightBackSteerMotor = rightBackSteerMotor;
m_rightBackWheelMotor = rightBackWheelMotor;
m_leftFrontEncoder = leftFrontEncoder;
m_rightFrontEncoder = rightFrontEncoder;
m_leftBackEncoder = leftBackEncoder;
m_rightBackEncoder = rightBackEncoder;
modules = new SwerveModule[] { public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right CANCoder rightFrontEncoder,
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left CANCoder leftBackEncoder,
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right CANCoder rightBackEncoder)
}; {
// gyro.reset(); m_leftFrontSteerMotor = leftFrontSteerMotor;
m_leftFrontWheelMotor = leftFrontWheelMotor;
m_rightFrontSteerMotor = rightFrontSteerMotor;
m_rightFrontWheelMotor = rightFrontWheelMotor;
m_leftBackSteerMotor = leftBackSteerMotor;
m_leftBackWheelMotor = leftBackWheelMotor;
m_rightBackSteerMotor = rightBackSteerMotor;
m_rightBackWheelMotor = rightBackWheelMotor;
m_leftFrontEncoder = leftFrontEncoder;
m_rightFrontEncoder = rightFrontEncoder;
m_leftBackEncoder = leftBackEncoder;
m_rightBackEncoder = rightBackEncoder;
modules = new SwerveModule[] {
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
};
//gyro.reset();
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
{
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
driveFromSpeeds(speeds);*/
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states =
kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state);
}
}
//Converts a ChassisSpeed to SwerveModuleStates (targets)
public void driveFromSpeeds(ChassisSpeeds speeds)
{
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
// Convert to module states
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
// Front left module state
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
// Front right module state
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
// Back left module state
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
// Back right module state
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
//Set the motors
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
} }
// https://github.com/ZachOrr/MK3-Swerve-Example //Sets steering motors to PID values
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the
* field.
*/
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
// driveFromSpeeds(speeds);
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
SwerveModuleState[] states = kinematics.toSwerveModuleStates(
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state);
}
}
// Converts a ChassisSpeed to SwerveModuleStates (targets)
public void driveFromSpeeds(ChassisSpeeds speeds) {
// https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
// Convert to module states
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
// Front left module state
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
// Front right module state
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
// Back left module state
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
// Back right module state
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
// Set the motors
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
}
//Sets steering motors to PID values
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack) public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
{/* {
//Set the Wheel motor speeds /*//Set the Wheel motor speeds
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
//PID
//PID m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000); m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees()); System.out.println("Target: " + leftFront.angle.getDegrees());*/
}
System.out.println("Target: " + leftFront.angle.getDegrees());
*/}
/*public void setSwerveGains(){ /*public void setSwerveGains(){
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}*/ }*/
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate) // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
// { // {
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/) // var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
// } // }
} }
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+18 -17
View File
@@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains; import frc4388.utility.Gains;
@@ -27,8 +28,10 @@ public class SwerveModule extends SubsystemBase {
private static double kEncoderTicksPerRotation = 4096; private static double kEncoderTicksPerRotation = 4096;
/** Creates a new SwerveModule. */ /** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) { public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor; this.driveMotor = driveMotor;
this.angleMotor = angleMotor; this.angleMotor = angleMotor;
this.canCoder = canCoder; this.canCoder = canCoder;
@@ -45,35 +48,32 @@ public class SwerveModule extends SubsystemBase {
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleTalonFXConfiguration); angleMotor.configAllSettings(angleTalonFXConfiguration);
/* /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
* TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); driveTalonFXConfiguration.slot0.kP = kDriveP;
* driveTalonFXConfiguration.slot0.kI = kDriveI;
* driveTalonFXConfiguration.slot0.kP = kDriveP; driveTalonFXConfiguration.slot0.kD = kDriveD;
* driveTalonFXConfiguration.slot0.kI = kDriveI; driveTalonFXConfiguration.slot0.kF = kDriveF;
* driveTalonFXConfiguration.slot0.kD = kDriveD; driveMotor.configAllSettings(driveTalonFXConfiguration);*/
* driveTalonFXConfiguration.slot0.kF = kDriveF;
*
* driveMotor.configAllSettings(driveTalonFXConfiguration);
*/
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
// CANCODER CONFIG canCoderConfiguration.magnetOffsetDegrees = offset;
canCoder.configAllSettings(canCoderConfiguration); canCoder.configAllSettings(canCoderConfiguration);
} }
public Rotation2d getAngle() { public Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor value reports degrees. // Note: This assumes the CANCoders are setup with the default feedback coefficient
// and the sesnor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
} }
/** /**
* Set the speed + rotation of the swerve module from a SwerveModuleState object * Set the speed + rotation of the swerve module from a SwerveModuleState object
* * @param desiredState - A SwerveModuleState representing the desired new state of the module
* @param desiredState - A SwerveModuleState representing the desired new state
* of the module
*/ */
public void setDesiredState(SwerveModuleState desiredState) { public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = getAngle(); Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// Find the difference between our current rotational position + our new rotational position // Find the difference between our current rotational position + our new rotational position
@@ -86,7 +86,8 @@ public class SwerveModule extends SubsystemBase {
double desiredTicks = currentTicks + deltaTicks; double desiredTicks = currentTicks + deltaTicks;
angleMotor.set(TalonFXControlMode.Position, desiredTicks); angleMotor.set(TalonFXControlMode.Position, desiredTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER); driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
} }
} }
src/main/java/frc4388/utility/RobotGyro.java.old → src/main/java/frc4388/utility/RobotGyro.java
View File