Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Swerve #8

Closed
wants to merge 16 commits into from
Prev Previous commit
Next Next commit
'fixed' the error in my rotation code
  • Loading branch information
ConradSkelly committed Jan 7, 2025
commit 63ecb02987a6362a51a7c9f2a80bf2432e585dca
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public static class SwerveConstants {

public static final LinearVelocity MAX_SPEED = MetersPerSecond.of(4.25);

public static final AngularVelocity MAX_ROTATION = RotationsPerSecond.of(Math.PI * 2);
public static final AngularVelocity MAX_ROTATION = RadiansPerSecond.of(2 * Math.PI);

public static final double LEFT_STICK_SCAILING = 2;

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ private void configureBindings() {
}

private void defualtCommands(){
swerve.setDefaultCommand(swerve.driveFieldRelativeCommand(controller::getLeftY, controller::getLeftX, controller::getRightX));
swerve.setDefaultCommand(swerve.driveFieldRelativeCommand(controller::getLeftX, controller::getLeftX, controller::getLeftY));
}

public void teleopInit() {}
Expand Down
12 changes: 9 additions & 3 deletions src/main/java/frc/robot/subsystems/Swerve.java
Original file line number Diff line number Diff line change
@@ -1,9 +1,13 @@
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Degree;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Seconds;

import java.util.function.DoubleSupplier;
Expand Down Expand Up @@ -44,17 +48,19 @@ private void DriveFiledRelativeScailier(double x, double y, double omega){

y = liniarMagintued * -directions.getCos() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
x = liniarMagintued * directions.getSin() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
omega = Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(DegreesPerSecond);

System.out.println("max speed in radians is " + SwerveConstants.MAX_ROTATION.in(RadiansPerSecond));
System.out.println("rotaton speed is " + Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(RadiansPerSecond));
omega = Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(RadiansPerSecond);

driveFieldRelative(MetersPerSecond.of(MathUtil.applyDeadband(-x, Constants.SwerveConstants.DEADBAND)), MetersPerSecond.of(MathUtil.applyDeadband(y, Constants.SwerveConstants.DEADBAND)), DegreesPerSecond.of(MathUtil.applyDeadband(-omega, Constants.SwerveConstants.DEADBAND)));
}


private void driveFieldRelative(LinearVelocity x, LinearVelocity y, AngularVelocity omega){

estimatedPosition = estimatedPosition.transformBy(new Transform2d(x.times(Seconds.of(.02)).in(Meters), y.times(Seconds.of(.02)).in(Meters), new Rotation2d(omega.times(Seconds.of(.02)).in(Radians))));
estimatedPosition = estimatedPosition.transformBy(new Transform2d(x.times(Seconds.of(.02)).in(Meters), y.times(Seconds.of(.02)).in(Meters), new Rotation2d(omega.times(Seconds.of(.02)).in(Degrees))));


}

/**
Expand Down