Drive input orientation

This commit is contained in:
Michael Mikovsky
2026-02-14 12:49:33 -08:00
parent abc3dc6b14
commit 736d7ef823
6 changed files with 43 additions and 20 deletions
@@ -205,9 +205,10 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
m_robotSwerveDrive.driveIntake(
m_robotSwerveDrive.driveIntakeOrientation(
getDeadbandedDriverController().getLeft(),
false
getDeadbandedDriverController().getRight()
);
}, m_robotSwerveDrive))
.onTrue(new InstantCommand(() -> {
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 50;
public static final String GIT_SHA = "8381ac460776aaad24a82786c9fe12e040a99d00";
public static final String GIT_DATE = "2026-02-14 11:55:51 MST";
public static final int GIT_REVISION = 51;
public static final String GIT_SHA = "abc3dc6b144dc783d0ceac8861e847d5f5f9741d";
public static final String GIT_DATE = "2026-02-14 12:50:09 MST";
public static final String GIT_BRANCH = "shoot-button";
public static final String BUILD_DATE = "2026-02-14 12:44:39 MST";
public static final long BUILD_UNIX_TIME = 1771098279734L;
public static final String BUILD_DATE = "2026-02-14 13:46:45 MST";
public static final long BUILD_UNIX_TIME = 1771102005908L;
public static final int DIRTY = 1;
private BuildConstants(){}
@@ -31,7 +31,7 @@ public class IntakeConstants {
// public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3*0);
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0);
public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
@@ -62,7 +62,7 @@ public class Shooter extends SubsystemBase {
public void setShooterReady() {
// if(this.mode == ShooterMode.NotReady) {
this.mode = ShooterMode.Ready;
this.mode = ShooterMode.Shooting;
// }
}
@@ -96,7 +96,7 @@ public class Shooter extends SubsystemBase {
this.mode = ShooterMode.Shooting;
// this.mode = ShooterMode.Shooting;
// TODO: get if the robot is within the angle of the hub
@@ -147,7 +147,8 @@ public class Shooter extends SubsystemBase {
// ShooterMode.NotReady
// );
// }
}
switch (mode) {
case Shooting:
@@ -165,4 +166,4 @@ public class Shooter extends SubsystemBase {
}
}
}}
}
@@ -24,10 +24,10 @@ public class ShooterConstants {
// public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10);
// public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 0);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0);
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10);
public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", -10);
public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
@@ -306,12 +306,33 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Rotation2d heading = new Rotation2d(leftStick.getX(), -leftStick.getY());//.r otateBy(Rotation2d.fromDegrees(90));
// if (invertRotation){
heading = heading.rotateBy(Rotation2d.fromDegrees(270));
// }
heading = heading.rotateBy(Rotation2d.fromDegrees(270));
driveFieldAngle(leftStick, heading);
}
}
public void driveIntakeOrientation(Translation2d leftStick, Translation2d rightStick){
// if (invert){
// Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY());
// driveFieldAngle(stick, heading);
// } else{
// driveFieldAngle(leftStick, heading);
// }
double speed = rightStick.getNorm();
if(speed < 0.3) {
driveWithInput(leftStick, new Translation2d(), true);
} else {
Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90));
heading = heading.rotateBy(Rotation2d.fromDegrees(90));
rotTarget = heading.getDegrees();
// Only drive forward in robot direction (no strafe)
// Translation2d forwardOnly = new Translation2d(speed, 0.0);
driveFieldAngle(leftStick, heading);
}
}