Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Reduce joystick skew calculations a bit

Browse Source
This commit is contained in:
nathanrsxtn
2022-04-04 13:37:20 -06:00
parent ae77425c21
commit d3d7157802
5 changed files with 49 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+15 -21
View File
@@ -96,8 +96,8 @@ public class RobotContainer {
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
public static boolean softLimits = true;
@@ -225,25 +225,19 @@ public class RobotContainer {
configureButtonBindings();
/* Default Commands */
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> {
if (currentDriveMode.equals(DriveMode.ON)) {
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true); }
if (currentDriveMode.equals(DriveMode.OFF)) {
m_robotSwerveDrive.driveWithInput( 0,
0,
0,
0,
false);
}}
, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
if (currentDriveMode == DriveMode.ON) {
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(),
getDriverController().getRight(), true);
}
if (currentDriveMode == DriveMode.OFF) {
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
}
}, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
// Intake with Triggers
m_robotIntake.setDefaultCommand(
new RunCommand(() -> m_robotIntake.runWithTriggers(
getOperatorController().getLeftTriggerAxis(),
@@ -631,11 +625,11 @@ public class RobotContainer {
}
}
public static XboxController getDriverController() {
public static DeadbandedXboxController getDriverController() {
return m_driverXbox;
}
public XboxController getOperatorController() {
public static DeadbandedXboxController getOperatorController() {
return m_operatorXbox;
}
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+1 -1
View File
@@ -186,7 +186,7 @@ public class Shoot extends CommandBase {
SmartDashboard.putNumber("Normalized Output", this.normOutput);
this.turret.runTurretWithCustomPID(normOutput);
this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative
this.swerve.driveWithInput(RobotContainer.getDriverController().getLeft(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative
if (this.toShoot) {
this.hood.runAngleAdjustPID(this.targetHood);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+21 -12
View File
@@ -81,6 +81,11 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putData("Field", m_field);
}
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
Translation2d speed = new Translation2d(speedX, speedY);
driveWithInput(speed, rot, fieldRelative);
}
/**
* Method to drive the robot using joystick info.
* @link https://github.com/ZachOrr/MK3-Swerve-Example
@@ -90,10 +95,9 @@ public class SwerveDrive extends SubsystemBase {
* @param fieldRelative Whether the provided x and y speeds are relative to the
* field.
*/
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0);
Translation2d speed = new Translation2d(speedX, speedY);
public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
@@ -108,23 +112,28 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(states);
}
// new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
Translation2d head = new Translation2d(rightX, rightY);
driveWithInput(speed, head, fieldRelative);
}
// new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0;
leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
if (ignoreAngles) {
rot = 0;
}
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
double xSpeedMetersPerSecond = leftStick.getX();
double ySpeedMetersPerSecond = leftStick.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 2);
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
// if (ignoreAngles) {
src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java
+6 -4
View File
@@ -7,10 +7,12 @@ import frc4388.robot.Constants.OIConstants;
public class DeadbandedRawXboxController extends RawXboxController {
public DeadbandedRawXboxController(int port) { super(port); }
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
@Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+6 -5
View File
@@ -8,11 +8,12 @@ import frc4388.robot.Constants.OIConstants;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
@Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();