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

worky

Browse Source
This commit is contained in:
Abhishrek05
2024-01-05 13:56:01 -07:00
parent 3b5080a334
commit 5b2e8f7e98
25 changed files with 1493 additions and 729 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+91 -45
View File
@@ -7,6 +7,8 @@
package frc4388.robot;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.LEDPatterns;
import frc4388.utility.Gains;
@@ -19,52 +21,96 @@ import frc4388.utility.Gains;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 0.1;
public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 22;
public static final double HEIGHT = 22;
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
public static final double MAX_SPEED_FEET_PER_SEC = 16;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
// ofsets are in degrees
//ofsets are in degrees
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
// 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
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final int SWERVE_TIMEOUT_MS = 30;
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
// swerve configuration
public static final double NEUTRAL_DEADBAND = 0.04;
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
public static final int REMOTE_0 = 0;
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
public static final double SLOW_SPEED = 0.8;
public static final double FAST_SPEED = 1.0;
public static final double TURBO_SPEED = 4.0;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int RIGHT_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 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 {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
}
public static final class Conversions {
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 10;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
}
public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
}
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
// dimensions
public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class DriveConstants {
public static final int DRIVE_PIGEON_ID = 6;
src/main/java/frc4388/robot/RobotMap.java
+1 -1
View File
@@ -12,7 +12,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.RobotGyro;
src/main/java/frc4388/robot/subsystems/DiffDrive.java
+1 -1
View File
@@ -80,6 +80,6 @@ public class DiffDrive extends SubsystemBase {
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
SmartDashboard.putData(m_gyro);
//SmartDashboard.putData(m_gyro);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -7,7 +7,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+138 -258
View File
@@ -4,312 +4,192 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
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;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotGyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
private SwerveModule m_leftFront;
private SwerveModule m_leftBack;
private SwerveModule m_rightFront;
private SwerveModule m_rightBack;
private SwerveModule[] modules;
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
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);
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
private RobotGyro gyro;
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));
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0;
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_Pigeon2 m_gyro;
public SwerveDriveOdometry m_odometry;
// public SwerveDriveOdometry m_odometry;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private final Field2d m_field = new Field2d();
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
WPI_Pigeon2 gyro) {
m_leftFront = leftFront;
m_leftBack = leftBack;
m_rightFront = rightFront;
m_rightBack = rightBack;
m_gyro = gyro;
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
// m_poseEstimator = new SwerveDrivePoseEstimator(
// getRegGyro(),//m_gyro.getRotation2d(),
// new Pose2d(),
// m_kinematics,
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
// VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
this.gyro = gyro;
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
Translation2d speed = new Translation2d(speedX, speedY);
driveWithInput(speed, rot, fieldRelative);
}
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
/**
* Method to drive the robot using joystick info.
* @link https://github.com/ZachOrr/MK3-Swerve-Example
* @param speeds[0] Speed of the robot in the x direction (forward).
* @param speeds[1] 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(Translation2d speed, double rot, boolean fieldRelative) {
ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
double rot = 0;
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * 2);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
}
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
Translation2d speed = new Translation2d(leftX, leftY);
Translation2d head = new Translation2d(rightX, rightY);
driveWithInput(speed, head, fieldRelative);
}
// 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));
// new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0;
leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
if (ignoreAngles) {
rot = 0;
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
double xSpeedMetersPerSecond = leftStick.getX();
double ySpeedMetersPerSecond = leftStick.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
// if (ignoreAngles) {
// SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
// for (int i = 0; i < states.length; i ++) {
// SwerveModuleState state = states[i];
// lockedStates[i]= new SwerveModuleState(0, state.angle);
// }
// setModuleStates(lockedStates);
// }
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
*
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
// int i = 2; {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state, ignoreAngles);
module.setDesiredState(state);
}
// modules[0].setDesiredState(desiredStates[0], false);
}
public void setModuleRotationsToAngle(double angle) {
for (int i = 0; i < modules.length; i++) {
SwerveModule module = modules[i];
module.rotateToAngle(angle);
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
}
return false;
}
public double getGyroAngle() {
return gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
rotTarget = 0.0;
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@Override
public void periodic() {
updateOdometry();
updateSmartDash();
// SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
// SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
// SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
// SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
m_field.setRobotPose(getOdometry());
super.periodic();
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
}
private void updateSmartDash() {
// odometry
SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
// SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* Gets the current chassis speeds in m/s and rad/s.
* @return Current chassis speeds (vx, vy, ω)
*/
public ChassisSpeeds getChassisSpeeds() {
return chassisSpeeds;
}
/**
* Gets the current pose of the robot.
*
* @return Robot's current pose.
*/
public Pose2d getOdometry() {
// return m_odometry.getPoseMeters();
return m_odometry.getPoseMeters();
// return m_poseEstimator.getEstimatedPosition();
}
public Pose2d getAutoOdo() {
Pose2d workingPose = getOdometry();
return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
}
/**
* Gets the current gyro using regression formula.
*
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
// * test chassis regression
// double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
// * new robot regression
double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
return new Rotation2d(Math.toRadians(regCur));
}
/**
* Resets the odometry of the robot to the given pose.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
}
/**
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2));
Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians());
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
}
/**
* Resets pigeon.
*/
public void resetGyro() {
m_gyro.reset();
rotTarget = new Rotation2d(0);
}
/**
* Stop all four swerve modules.
*/
public void stopModules() {
modules[0].stop();
modules[1].stop();
modules[2].stop();
modules[3].stop();
}
/**
* Switches speed modes.
*
* @param shift True if fast mode, false if slow mode.
*/
public void highSpeed(boolean shift) {
if (shift) {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
public void shiftDown() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
}
public double getCurrent(){
return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
public void setToSlow() {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
}
public double getVoltage(){
return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
}
}
public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
}
}
public void toggleGear(double angle) {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+104 -124
View File
@@ -7,175 +7,155 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
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.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
public WPI_TalonFX angleMotor;
public WPI_TalonFX driveMotor;
private CANCoder canCoder;
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
private static double kEncoderTicksPerRotation = 4096;
private SwerveModuleState state;
private double canCoderFeedbackCoefficient;
public long m_currentTime;
public long m_lastTime;
public double m_deltaTime;
public double m_currentPos;
public double m_lastPos;
public SwerveModuleState lastState = new SwerveModuleState();
public SwerveModuleState currentState;
private WPI_TalonFX driveMotor;
private WPI_TalonFX angleMotor;
private CANCoder encoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.canCoder = canCoder;
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
this.encoder = encoder;
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
angleConfig.slot0.kI = swerveGains.kI;
angleConfig.slot0.kD = swerveGains.kD;
angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
// use the CANcoder as the remote sensor for the primary TalonFX PID
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
// Use the CANCoder as the remote sensor for the primary TalonFX PID
angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleTalonFXConfiguration);
// angleMotor.setInverted(true);
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
// driveTalonFXConfiguration.slot0.kP = 0.05;
// driveTalonFXConfiguration.slot0.kI = 0.0;
// driveTalonFXConfiguration.slot0.kD = 0.0;
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
// FeedbackDevice.IntegratedSensor;
driveMotor.configFactoryDefault();
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
driveMotor.configNominalOutputForward(0, 30);
driveMotor.configNominalOutputReverse(0, 30);
driveMotor.configPeakOutputForward(1, 30);
driveMotor.configPeakOutputReverse(-1, 30);
driveMotor.configAllowableClosedloopError(0, 0, 30);
// driveMotor.setInverted(true);
driveMotor.config_kP(0, 0, 30);
driveMotor.config_kI(0, 0, 30);
driveMotor.config_kD(0, 0, 30);
encoder.configMagnetOffset(offset);
// driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.sensorCoefficient = 0.087890625;
canCoderConfiguration.magnetOffsetDegrees = offset;
canCoderConfiguration.sensorDirection = true;
canCoder.configAllSettings(canCoderConfiguration);
m_currentTime = System.currentTimeMillis();
m_lastTime = System.currentTimeMillis();
m_lastPos = driveMotor.getSelectedSensorPosition();
}
private Rotation2d getAngle() {
// ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
driveMotor.setSelectedSensorPosition(0);
driveMotor.config_kP(0, 0.2);
}
/**
* Set the speed + rotation of the swerve module from a SwerveModuleState object
*
* @param desiredState - A SwerveModuleState representing the desired new state
* of the module
* Get the drive motor of the SwerveModule
* @return the drive motor of the SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
Rotation2d currentRotation = getAngle();
// currentRotation.getDegrees());
state = SwerveModuleState.optimize(desiredState, currentRotation);
// Find the difference between our current rotational position + our new
// rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// Find the new absolute position of the module based on the difference in
// rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
// Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
// Please work
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
driveMotor.set(normFtPerSec);// - angleMotor.get());
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
// between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
public WPI_TalonFX getDriveMotor() {
return this.driveMotor;
}
/**
* Get current module state.
*
* @return The current state of the module in m/s.
* Get the angle motor of the SwerveModule
* @return the angle motor of the SwerveModule
*/
public SwerveModuleState getState() {
// return state;
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
* SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
public WPI_TalonFX getAngleMotor() {
return this.angleMotor;
}
/**
* Stop the drive and steer motors of current module.
* Get the CANcoder of the SwerveModule
* @return the CANcoder of the SwerveModule
*/
public CANCoder getEncoder() {
return this.encoder;
}
/**
* Get the angle of a SwerveModule as a Rotation2d
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public double getAngularVel() {
return this.angleMotor.getSelectedSensorVelocity();
}
public double getDrivePos() {
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
}
public double getDriveVel() {
return this.driveMotor.getSelectedSensorVelocity(0);
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
this.angleMotor.set(TalonFXControlMode.Position, angle);
angleMotor.set(TalonFXControlMode.Position, angle);
}
@Override
public void periodic() {
currentState = this.getState();
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
lastState = currentState;
/**
* Get state of swerve module
* @return speed in m/s and angle in degrees
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle()
);
}
public void reset() {
canCoder.setPositionToAbsolute();
// canCoder.configSensorInitializationStrategy(initializationStrategy)
/**
* Returns the current position of the SwerveModule
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
}
public double getCurrent(){
/**
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
}
public void reset(double position) {
encoder.setPositionToAbsolute();
}
public double getCurrent() {
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
public double getVoltage(){
public double getVoltage() {
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
}
}
src/main/java/frc4388/utility/Gains.java
+83
View File
@@ -0,0 +1,83 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.utility;
/** Add your docs here. */
public class Gains {
public double kP;
public double kI;
public double kD;
public double kF;
public int kIZone;
public double kPeakOutput;
public double kMaxOutput;
public double kMinOutput;
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = kPeakOutput;
this.kMaxOutput = kPeakOutput;
this.kMinOutput = -kPeakOutput;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = 1.0;
this.kMaxOutput = 1.0;
this.kMinOutput = -1.0;
}
public Gains(double kP, double kI, double kD) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kMaxOutput = kMaxOutput;
this.kMinOutput = kMinOutput;
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
}
}
src/main/java/frc4388/utility/RobotGyro.java
+32 -12
View File
@@ -7,31 +7,34 @@
package frc4388.utility;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpiutil.math.MathUtil;
// import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.math.MathUtil;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro extends GyroBase {
public class RobotGyro implements Gyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU m_pigeon = null;
private WPI_Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
private double pitchZero = 0;
private double rollZero = 0;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
public RobotGyro(WPI_Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
@@ -45,6 +48,16 @@ public class RobotGyro extends GyroBase {
m_isGyroAPigeon = false;
}
/**
* Resets yaw, pitch, and roll.
*/
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -74,7 +87,7 @@ public class RobotGyro extends GyroBase {
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
m_pigeon.calibrate();
} else {
m_navX.calibrate();
}
@@ -82,6 +95,8 @@ public class RobotGyro extends GyroBase {
@Override
public void reset() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
@@ -98,9 +113,10 @@ public class RobotGyro extends GyroBase {
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
}
@Override
@@ -112,6 +128,10 @@ public class RobotGyro extends GyroBase {
}
}
public double getYaw() {
return this.getAngle();
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
@@ -165,7 +185,7 @@ public class RobotGyro extends GyroBase {
}
}
public PigeonIMU getPigeon(){
public WPI_Pigeon2 getPigeon(){
return m_pigeon;
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+1 -2
View File
@@ -1,7 +1,6 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
import frc4388.utility.controller.XboxController;
/**
* Mapping for the Xbox controller triggers to allow triggers to be defined as
@@ -32,7 +31,7 @@ public class XboxTriggerButton extends Button {
}
/** {@inheritDoc} */
@Override
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();