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.

Sign up for GitHub

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

34 fieldorientedorbitrequest gets stuck fast #35

Merged
merged 5 commits into from
Dec 21, 2024
+1,174 −341
Merged

34 fieldorientedorbitrequest gets stuck fast #35

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c1205ce
formatted like 3 files
mmilunicmobile Dec 21, 2024
13240ea
Fixed a field orientation calculation being incorrect.
mmilunicmobile Dec 21, 2024
9316326
added a few more important fixes and logging
mmilunicmobile Dec 21, 2024
c15bbd7
added swerve setpoint generator fix
mmilunicmobile Dec 21, 2024
c9bed7e
applied formatting and modified moments of inertia to be consistent
mmilunicmobile Dec 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ spotless {
exclude '**/build/**', '**/build-*/**'
}
toggleOffOn()
googleJavaFormat()
googleJavaFormat().aosp()
trimTrailingWhitespace()
endWithNewline()
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/paths/1M.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
},
"prevControl": {
"x": -0.27527965608765825,
"y": 0.0
"y": 1.561767148845488e-16
},
"nextControl": null,
"isLocked": false,
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/paths/Half-Field Sprint.path
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 6.0,
"maxVelocity": 4.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
Expand Down
26 changes: 13 additions & 13 deletions src/main/deploy/pathplanner/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,23 +9,23 @@
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 74.088,
"robotMOI": 6.883,
"robotMass": 68.0389,
"robotMOI": 11.61,
"robotTrackwidth": 0.508,
"driveWheelRadius": 0.0508,
"driveGearing": 6.75,
"maxDriveSpeed": 4.572,
"driveGearing": 5.142857,
"maxDriveSpeed": 6.21,
"driveMotorType": "krakenX60FOC",
"driveCurrentLimit": 60.0,
"driveCurrentLimit": 85.0,
"wheelCOF": 1.0,
"flModuleX": 0.298,
"flModuleY": 0.298,
"frModuleX": 0.298,
"frModuleY": -0.298,
"blModuleX": -0.298,
"blModuleY": 0.298,
"brModuleX": -0.298,
"brModuleY": -0.298,
"flModuleX": 0.2921,
"flModuleY": 0.292,
"frModuleX": 0.2921,
"frModuleY": -0.2921,
"blModuleX": -0.2921,
"blModuleY": 0.2921,
"brModuleX": -0.2921,
"brModuleY": -0.2921,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
Expand Down
645 changes: 645 additions & 0 deletions src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java
View file
Open in desktop

Large diffs are not rendered by default.

115 changes: 82 additions & 33 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,18 @@
import frc.lib.controller.LogitechController;
import frc.lib.controller.ThrustmasterJoystick;
import frc.robot.constants.GlobalConstants;
import frc.robot.constants.TunerConstants;
import frc.robot.constants.GlobalConstants.ControllerConstants;
import frc.robot.constants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.WheelRadiusCharacterization;

public class RobotContainer {
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
private double MaxSpeed =
GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(
MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate =
GlobalConstants.MAX_ROTATIONAL_SPEED.in(
RadiansPerSecond); // kMaxAngularRate desired top rotational speed

private final ThrustmasterJoystick leftDriveController =
new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER);
Expand All @@ -45,48 +49,94 @@ public RobotContainer() {
configureBindings();

drivetrain.setUpPathPlanner();
// Establish the "Trajectory Field" Field2d into the dashboard
// Establish the "Trajectory Field" Field2d into the dashboard
}

private void configureBindings() {
// Note that X is defined as forward according to WPILib convention,
// and Y is defined as to the left according to WPILib convention.
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.applyRequest(
() -> {
ChassisSpeeds driverDesiredSpeeds = new ChassisSpeeds(
GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) * sps(deadband(leftDriveController.getYAxis().get(), 0.1)),
sps(deadband(leftDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond),
-sps(deadband(rightDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_ROTATIONAL_SPEED.in(RadiansPerSecond)
);
return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(driverDesiredSpeeds);
// return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds);
}
)
);

// drive.withVelocityX(-leftDriveController.getYAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward)
// .withVelocityY(-leftDriveController.getXAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left)
// .withRotationalRate(-rightDriveController.getXAxis().get() * GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left)
// Drivetrain will execute this command periodically
drivetrain.applyRequest(
() -> {
ChassisSpeeds driverDesiredSpeeds =
new ChassisSpeeds(
GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(
MetersPerSecond)
* sps(
deadband(
leftDriveController
.getYAxis()
.get(),
0.1)),
sps(deadband(leftDriveController.getXAxis().get(), 0.1))
* GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(
MetersPerSecond),
-sps(
deadband(
rightDriveController
.getXAxis()
.get(),
0.1))
* GlobalConstants.MAX_ROTATIONAL_SPEED.in(
RadiansPerSecond));
return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(
driverDesiredSpeeds);
// return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds);
}));

// drive.withVelocityX(-leftDriveController.getYAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward)
// .withVelocityY(-leftDriveController.getXAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left)
// .withRotationalRate(-rightDriveController.getXAxis().get() *
// GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left)

operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake));
operatorController.getB().whileTrue(drivetrain.applyRequest(() ->
point.withModuleDirection(new Rotation2d(-operatorController.getLeftYAxis().get(), -operatorController.getLeftXAxis().get()))
));

leftDriveController.getTrigger().whileTrue(new WheelRadiusCharacterization(WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));
operatorController
.getB()
.whileTrue(
drivetrain.applyRequest(
() ->
point.withModuleDirection(
new Rotation2d(
-operatorController.getLeftYAxis().get(),
-operatorController
.getLeftXAxis()
.get()))));

leftDriveController
.getTrigger()
.whileTrue(
new WheelRadiusCharacterization(
WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));

// Run SysId routines when holding back/start and X/Y.
// Note that each routine should be run exactly once in a single log.
operatorController.getBack().and(operatorController.getY()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
operatorController.getBack().and(operatorController.getX()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
operatorController.getStart().and(operatorController.getY()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
operatorController.getStart().and(operatorController.getX()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
operatorController
.getBack()
.and(operatorController.getY())
.whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
operatorController
.getBack()
.and(operatorController.getX())
.whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
operatorController
.getStart()
.and(operatorController.getY())
.whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
operatorController
.getStart()
.and(operatorController.getX())
.whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));

// reset the field-centric heading on left bumper press
operatorController.getLeftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
operatorController.getRightBumper().onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d())));
operatorController
.getLeftBumper()
.onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
operatorController
.getRightBumper()
.onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d())));

drivetrain.registerTelemetry(logger::telemeterize);
}
Expand All @@ -106,5 +156,4 @@ private double sps(double value) {
public Command getAutonomousCommand() {
return auto.getAuto();
}

}
76 changes: 49 additions & 27 deletions src/main/java/frc/robot/constants/GlobalConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.pathplanner.lib.config.ModuleConfig;
import com.pathplanner.lib.config.RobotConfig;

import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.measure.AngularVelocity;
Expand All @@ -14,19 +13,21 @@
import edu.wpi.first.units.measure.MomentOfInertia;

public class GlobalConstants {

private static final SwerveModuleConstants EXAMPLE_MODULE = TunerConstants.FrontLeft;

public static final LinearVelocity MAX_TRANSLATIONAL_SPEED = TunerConstants.kSpeedAt12Volts;
public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1);
public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1);

public static final Mass ROBOT_MASS = Pounds.of(150);
public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(6.883);
public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(11.61);

public static final double COEFFICIENT_OF_FRICTION = 1.0;

public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio);
public static final DCMotor STEER_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio);
public static final DCMotor DRIVE_MOTOR =
DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio);
public static final DCMotor STEER_MOTOR =
DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio);

public static class ControllerConstants {
public static final int LEFT_DRIVE_CONTROLLER = 0;
Expand All @@ -38,31 +39,52 @@ public static class ControllerConstants {

public static RobotConfig getRobotConfigPathplanner() {
if (robotConfigPathplanner == null) {
try{
Translation2d[] moduleOffsets = new Translation2d[4];
SwerveModuleConstants[] constants = new SwerveModuleConstants[]{
TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight
};
for (int i = 0; i < constants.length; i++) {
moduleOffsets[i] = new Translation2d(constants[i].LocationX, constants[i].LocationY);
}
try {
Translation2d[] moduleOffsets = new Translation2d[4];
SwerveModuleConstants[] constants =
new SwerveModuleConstants[] {
TunerConstants.FrontLeft,
TunerConstants.FrontRight,
TunerConstants.BackLeft,
TunerConstants.BackRight
};
for (int i = 0; i < constants.length; i++) {
moduleOffsets[i] =
new Translation2d(constants[i].LocationX, constants[i].LocationY);
}

robotConfigPathplanner = new RobotConfig(ROBOT_MASS, ROBOT_MOI,
new ModuleConfig(
Meters.of(EXAMPLE_MODULE.WheelRadius),
MAX_TRANSLATIONAL_SPEED,
COEFFICIENT_OF_FRICTION,
DRIVE_MOTOR,
EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits.getStatorCurrentLimitMeasure(),
1
),
moduleOffsets);
robotConfigPathplanner =
new RobotConfig(
ROBOT_MASS,
ROBOT_MOI,
new ModuleConfig(
Meters.of(EXAMPLE_MODULE.WheelRadius),
MAX_TRANSLATIONAL_SPEED,
COEFFICIENT_OF_FRICTION,
DRIVE_MOTOR,
EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits
.getStatorCurrentLimitMeasure(),
1),
moduleOffsets);
} catch (Exception e) {
// Handle exception as needed
e.printStackTrace();
throw new RuntimeException("Failed to load robot config from pathplanner.");
// Handle exception as needed
e.printStackTrace();
throw new RuntimeException("Failed to load robot config from pathplanner.");
}
}
return robotConfigPathplanner;
}

// public static RobotConfig getRobotConfigPathplanner() {
// if (robotConfigPathplanner == null) {
// try{
// robotConfigPathplanner = RobotConfig.fromGUISettings();
// } catch (Exception e) {
// // Handle exception as needed
// e.printStackTrace();
// throw new RuntimeException("Failed to load robot config from pathplanner.");
// }
// }
// return robotConfigPathplanner;
// }
}
Loading
Loading