Compare commits
2 Commits
faster_ele
...
main
Author | SHA1 | Date | |
---|---|---|---|
|
6c0df54b06 | ||
|
fa34ef00fe |
@ -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
|
||||
)
|
||||
|
@ -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(
|
||||
|
Loading…
Reference in New Issue
Block a user