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

readd rotation delta calculations in swerve module

Browse Source
This commit is contained in:
Michael Mikovsky
2024-06-27 11:35:13 -06:00
parent 3df6fbf7d9
commit da1c29f913
2 changed files with 27 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8 -2
View File
@@ -54,9 +54,15 @@ public class SwerveDrive extends SubsystemBase {
} }
public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// rightStick.getAngle()
double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
// System.out.println(ang); // System.out.println(ang);
module.go(ang); // module.go(ang);
// Rotation2d rot = Rotation2d.fromRadians(ang);
Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
SwerveModuleState state = new SwerveModuleState(speed, rot);
module.setDesiredState(state);
} }
boolean stopped = false; boolean stopped = false;
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+19 -9
View File
@@ -76,11 +76,11 @@ public class SwerveModule extends SubsystemBase {
rotateToAngle(0); rotateToAngle(0);
} }
public void go(double ang){ // public void go(double ang){
// double curang = this.encoder.getAbsolutePosition().getValue(); // // double curang = this.encoder.getAbsolutePosition().getValue();
System.out.println(getAngle().getDegrees()); // System.out.println(getAngle().getDegrees());
rotateToAngle(ang); // rotateToAngle(ang);
} // }
@Override @Override
public void periodic() { public void periodic() {
@@ -122,7 +122,7 @@ public class SwerveModule extends SubsystemBase {
public Rotation2d getAngle() { public Rotation2d getAngle() {
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. // * 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 Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue()); return Rotation2d.fromRotations(encoder.getPosition().getValue());
} }
public double getAngularVel() { public double getAngularVel() {
@@ -163,6 +163,11 @@ public class SwerveModule extends SubsystemBase {
); );
} }
// private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
// Rotation2d curRot = this.getAngle();
// }
/** /**
* Returns the current position of the SwerveModule * 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. * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
@@ -176,12 +181,17 @@ public class SwerveModule extends SubsystemBase {
* @param desiredState a SwerveModuleState representing the desired new state of the module * @param desiredState a SwerveModuleState representing the desired new state of the module
// */ // */
public void setDesiredState(SwerveModuleState desiredState) { public void setDesiredState(SwerveModuleState desiredState) {
SwerveModuleState state = SwerveModuleState.optimize(desiredState, this.getAngle()); 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);
Rotation2d rot = state.angle;
double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
rotateToAngle(rot.getRotations()); rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
} }