mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 08:48:03 -06:00
Janky working prototype, Most code removed, Using phoenix 6
This commit is contained in:
Michael Mikovsky
@@ -4,17 +4,18 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
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.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.RobotGyro;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
private SwerveModule leftFront;
|
||||
private SwerveModule rightFront;
|
||||
@@ -24,17 +25,20 @@ public class SwerveDrive extends SubsystemBase {
|
||||
private SwerveModule[] modules;
|
||||
|
||||
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 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);
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
@@ -51,14 +55,51 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
boolean stopped = false;
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
rightFront.go(leftStick);
|
||||
// if (fieldRelative) {
|
||||
|
||||
// double rot = 0;
|
||||
|
||||
// // ! drift correction
|
||||
// if (rightStick.getNorm() > 0.05) {
|
||||
// rotTarget = gyro.getAngle();
|
||||
// rot = rightStick.getX();
|
||||
// // SmartDashboard.putBoolean("drift correction", false);
|
||||
// stopped = false;
|
||||
// } else if(leftStick.getNorm() > 0.05) {
|
||||
// if (!stopped) {
|
||||
// stopModules();
|
||||
// stopped = true;
|
||||
// }
|
||||
|
||||
// // SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
// }
|
||||
|
||||
// // 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));
|
||||
|
||||
// // Convert field-relative speeds to robot-relative speeds.
|
||||
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
|
||||
// } else { // Create robot-relative speeds.
|
||||
// chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
// }
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
@@ -66,36 +107,63 @@ public class SwerveDrive extends SubsystemBase {
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), 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);
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
|
||||
|
||||
Translation2d rightStick = new Translation2d(-rightX, rightY);
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
|
||||
|
||||
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
|
||||
double min = tmp.getDegrees();
|
||||
min = Math.max(Math.abs(min), 2);
|
||||
if(tmp.getDegrees() < 0)
|
||||
min*=-1;
|
||||
tmp = new Rotation2d(min * Math.PI / 180);
|
||||
rot = tmp.getRadians(); // x x - y/x
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
// 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_SECOND));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
// public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
// 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);
|
||||
// }
|
||||
// }
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
double currentAngle = getGyroAngle();
|
||||
@@ -114,9 +182,30 @@ public class SwerveDrive extends SubsystemBase {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
|
||||
public void add180() {
|
||||
gyro.reset(gyro.getAngle()+180);
|
||||
rotTarget = gyro.getAngle();
|
||||
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
rotTarget = 0.0;
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroFlip() {
|
||||
gyro.resetFlip();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightBlue() {
|
||||
gyro.resetRightSideBlue();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightAmp() {
|
||||
gyro.resetAmpSide();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
@@ -129,10 +218,15 @@ public class SwerveDrive extends SubsystemBase {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
public boolean getSpeedState() {
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
// SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
@@ -142,6 +236,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -192,4 +287,14 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -4,56 +4,92 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
// import javax.swing.text.StyleContext.SmallAttributeSet;
|
||||
|
||||
// import com.ctre.phoenix.ErrorCode;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
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.motorcontrol.TalonFXControlMode;
|
||||
// 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.SensorInitializationStrategy;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
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;
|
||||
// import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder encoder;
|
||||
|
||||
private TalonFX driveMotor;
|
||||
private TalonFX angleMotor;
|
||||
private CANcoder encoder;
|
||||
// private int selfid;
|
||||
// private ConfigurableDouble offsetGetter;
|
||||
private static int swerveId = 0;
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
// // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
|
||||
// this.selfid = swerveId;
|
||||
// swerveId++;
|
||||
// TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
// angleConfig.slot0.kP = swerveGains.kP;
|
||||
// angleConfig.slot0.kI = swerveGains.kI;
|
||||
// angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
angleConfig.slot0.kP = swerveGains.kP;
|
||||
angleConfig.slot0.kI = swerveGains.kI;
|
||||
angleConfig.slot0.kD = 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);
|
||||
|
||||
encoder.configMagnetOffset(offset);
|
||||
// // 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);
|
||||
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
// //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
|
||||
// reset(0);
|
||||
// encoder.configMagnetOffset(offset);
|
||||
|
||||
// driveMotor.setSelectedSensorPosition(0);
|
||||
// driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
public void go(Translation2d leftStick){
|
||||
System.out.println(leftStick.getY());
|
||||
driveMotor.set(leftStick.getY());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
//encoder.configMagnetOffset(offsetGetter.get());
|
||||
//SmartDashboard.putString("Error Code: " + selfid, getstuff());
|
||||
// SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
|
||||
// SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
|
||||
// SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
|
||||
}
|
||||
/**
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getDriveMotor() {
|
||||
public TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
@@ -61,7 +97,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getAngleMotor() {
|
||||
public TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
@@ -69,7 +105,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANCoder getEncoder() {
|
||||
public CANcoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
@@ -79,19 +115,23 @@ public class SwerveModule extends SubsystemBase {
|
||||
*/
|
||||
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());
|
||||
// return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
return new Rotation2d();
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
return this.angleMotor.getSelectedSensorVelocity();
|
||||
// return this.angleMotor.getSelectedSensorVelocity();
|
||||
return 0;
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
return 0;
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
// return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
@@ -100,62 +140,67 @@ public class SwerveModule extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
// angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 SwerveModuleState getState() {
|
||||
// return new SwerveModuleState(
|
||||
// Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
|
||||
// getAngle()
|
||||
// );
|
||||
// }
|
||||
|
||||
/**
|
||||
* 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 SwerveModulePosition getPosition() {
|
||||
// return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
// }
|
||||
|
||||
/**
|
||||
* 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();
|
||||
// */
|
||||
// public void setDesiredState(SwerveModuleState desiredState) {
|
||||
// Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
// 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 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;
|
||||
// // 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();
|
||||
// // convert the CANCoder from its position reading to ticks
|
||||
// double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
// angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
// double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
|
||||
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
}
|
||||
// driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
// }
|
||||
|
||||
public void reset(double position) {
|
||||
encoder.setPositionToAbsolute();
|
||||
}
|
||||
// public void reset(double position) {
|
||||
// encoder.setPositionToAbsolute();
|
||||
// }
|
||||
|
||||
public double getCurrent() {
|
||||
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
}
|
||||
// public double getCurrent() {
|
||||
// return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
// }
|
||||
|
||||
public double getVoltage() {
|
||||
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
}
|
||||
// public double getVoltage() {
|
||||
// return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
// }
|
||||
|
||||
// public String getstuff() {
|
||||
// encoder.getPosition();
|
||||
// return "" + encoder.getLastError().value;
|
||||
// }
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user