Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -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){
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);
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;
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+19 -9
View File
@@ -76,11 +76,11 @@ public class SwerveModule extends SubsystemBase {
rotateToAngle(0);
}
public void go(double ang){
// double curang = this.encoder.getAbsolutePosition().getValue();
System.out.println(getAngle().getDegrees());
rotateToAngle(ang);
}
// public void go(double ang){
// // double curang = this.encoder.getAbsolutePosition().getValue();
// System.out.println(getAngle().getDegrees());
// rotateToAngle(ang);
// }
@Override
public void periodic() {
@@ -122,7 +122,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(encoder.getAbsolutePosition());
return Rotation2d.fromRotations(encoder.getAbsolutePosition().getValue());
return Rotation2d.fromRotations(encoder.getPosition().getValue());
}
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
* @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
// */
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;
rotateToAngle(rot.getRotations());
rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
}