Compare commits

...

2 Commits

Author SHA1 Message Date
Tylr-J42
6c0df54b06 fixing dt bindings divide by zero flaw 2025-06-06 18:22:12 -04:00
Tylr-J42
fa34ef00fe exponents on joystick now implemented correctly 2025-05-19 19:06:48 -04:00
3 changed files with 16 additions and 4 deletions

View File

@ -111,8 +111,8 @@ public class RobotContainer {
drivetrain.setDefaultCommand(
drivetrain.drive(
() -> Math.pow(driver.getLeftY(), 3),
() -> Math.pow(driver.getLeftX(), 3),
() -> driver.getLeftY(),
() -> driver.getLeftX(),
() -> driver.getRightX(),
() -> true
)

View File

@ -9,4 +9,6 @@ public class OIConstants {
public static final String kAutoTab = "Auto Tab";
public static final String kSensorsTab = "Sensors Tab";
public static final String kApriltagTab = "Apriltag Tab";
public static final double kJoystickExponential = 3;
}

View File

@ -323,8 +323,18 @@ public class Drivetrain extends SubsystemBase {
*/
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// Convert the commanded speeds into the correct units for the drivetrain
double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
double xSpeedDelivered = 0;
double ySpeedDelivered = 0;
if(p != 0){
xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
}else{
xSpeedDelivered = 0;
ySpeedDelivered = 0;
}
double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(