mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Merge branch 'shoot-button' into operator-controls
This commit is contained in:
@@ -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;
|
||||
// }
|
||||
}
|
||||
|
||||
@@ -94,7 +94,8 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm();
|
||||
|
||||
this.mode = ShooterMode.Shooting;
|
||||
// this.mode = ShooterMode.Shooting;
|
||||
|
||||
// TODO: get if the robot is within the angle of the hub
|
||||
|
||||
// boolean driverError =
|
||||
@@ -144,7 +145,8 @@ public class Shooter extends SubsystemBase {
|
||||
// ShooterMode.NotReady
|
||||
// );
|
||||
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
switch (mode) {
|
||||
case Shooting:
|
||||
@@ -162,4 +164,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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user