mirror of
https://github.com/Team4388/Robot-Essentials.git
synced 2026-06-09 08:48:02 -06:00
worky
This commit is contained in:
Abhishrek05
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user