Compare commits
3 Commits
c9316cebc3
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
6c0df54b06 | ||
|
|
fa34ef00fe | ||
|
|
68da3c630c |
@@ -111,8 +111,8 @@ public class RobotContainer {
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftY(),
|
||||
() -> driver.getLeftX(),
|
||||
() -> driver.getRightX(),
|
||||
() -> true
|
||||
)
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user