Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

offsets (INCOMPLETE DO NOT TOUCH)

Browse Source
This commit is contained in:
aarav18
2023-01-21 15:16:22 -07:00
parent bf4088d9db
commit b24bc6c6ab
6 changed files with 149 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+48 -11
View File
@@ -5,6 +5,7 @@
package frc4388.robot.subsystems;
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;
@@ -12,7 +13,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
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.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.RobotGyro;
@@ -55,26 +58,43 @@ public class SwerveDrive extends SubsystemBase {
this.rightBack = rightBack;
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
for (SwerveModule m : this.modules) {
m.reset();
}
// this.gyro = gyro;
}
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
double xSpeedMetersPerSecond = xSpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
double ySpeedMetersPerSecond = ySpeed * this.speedAdjust; //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
Translation2d speed = new Translation2d(-xSpeed, ySpeed);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
//);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
setModuleStates(states);
}
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
// ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(-leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
// if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
// rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
// double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
// chassisSpeeds = fieldRelative
// ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
// rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
// : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
@@ -83,6 +103,7 @@ public class SwerveDrive extends SubsystemBase {
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
@@ -161,10 +182,26 @@ public class SwerveDrive extends SubsystemBase {
this.rightBack.angleMotor.set(output);
}
public void rotateCANCodersToAngle(double angle) {
for (SwerveModule module : this.modules) {
module.rotateToAngle(angle);
}
}
public void resetCANCoders(double position) {
for (SwerveModule module : this.modules) {
module.reset(position);
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// updateOdometry();
SmartDashboard.putNumber("LeftFront CC", this.modules[0].getAngle().getDegrees());
SmartDashboard.putNumber("RightFront CC", this.modules[1].getAngle().getDegrees());
SmartDashboard.putNumber("LeftBack CC", this.modules[2].getAngle().getDegrees());
SmartDashboard.putNumber("RightBack CC", this.modules[3].getAngle().getDegrees());
}
/**
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+17 -11
View File
@@ -19,19 +19,21 @@ import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotEncoder;
public class SwerveModule extends SubsystemBase {
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private CANCoder canCoder;
// private CANCoder canCoder;
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 canCoder*/CANCoder encoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.canCoder = canCoder;
this.encoder = encoder;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
@@ -39,14 +41,16 @@ public class SwerveModule extends SubsystemBase {
angleConfig.slot0.kD = swerveGains.kD;
// use the CANcoder as the remote sensor for the primary TalonFX PID
angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
canCoderConfig.magnetOffsetDegrees = offset;
canCoder.configAllSettings(canCoderConfig);
encoder.configAllSettings(canCoderConfig);
// canCoderConfig.magnetOffsetDegrees = 270;
}
/**
@@ -70,7 +74,7 @@ public class SwerveModule extends SubsystemBase {
* @return the CANcoder of the SwerveModule
*/
public CANCoder getEncoder() {
return this.canCoder;
return this.encoder;
}
/**
@@ -79,7 +83,7 @@ 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(canCoder.getAbsolutePosition());
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public void stop() {
@@ -126,15 +130,17 @@ public class SwerveModule extends SubsystemBase {
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
}
public void reset() {
canCoder.setPositionToAbsolute();
public void reset(double position) {
// encoder.setPosition(position);
encoder.setPositionToAbsolute();
// canCoder.setPosition(1024);
}
public double getCurrent() {