operator and driver controls change

This commit is contained in:
2024-02-29 03:38:24 -05:00
parent 4c8c449776
commit 662c31702f
7 changed files with 152 additions and 10 deletions

View File

@@ -189,7 +189,7 @@ public class RobotContainer {
);
// Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
driver.a().onTrue(
driver.leftBumper().onTrue(
drivetrain.driveAprilTagLock(
driver::getLeftY,
driver::getLeftX,
@@ -260,6 +260,13 @@ public class RobotContainer {
operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState()));
driver.a().whileTrue(indexer.shootNote(() -> 1.0));
operator.back().toggleOnTrue(shooter.manualPivot(
() -> operator.getLeftY(),
() -> operator.getRightTriggerAxis()-operator.getLeftTriggerAxis()
));
operator.start().onTrue(shooter.zeroEncoder());
}

View File

@@ -29,8 +29,10 @@ public class Climber extends SubsystemBase {
public Command setSpeedWithSupplier(DoubleSupplier speed) {
return run(() -> {
if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
if(shooterAngle.getAsDouble() > Math.toRadians(-5.0)){
motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
}else{
motor1.set(VictorSPXControlMode.PercentOutput, 0);
}
});
}

View File

@@ -4,8 +4,6 @@ import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -21,9 +19,6 @@ public class Shooter extends SubsystemBase{
private CANSparkMax topShooter;
private CANSparkMax bottomShooter;
private RelativeEncoder bottomEncoder;
private RelativeEncoder topEncoder;
private CANSparkMax shooterPivot;
private Encoder pivotEncoder;
@@ -42,9 +37,6 @@ public class Shooter extends SubsystemBase{
shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
shooterPivot.setInverted(true);
topEncoder = topShooter.getEncoder();
bottomEncoder = bottomShooter.getEncoder();
/*
topPID = topShooter.getPIDController();
topPID.setFeedbackDevice(topEncoder);
@@ -137,6 +129,12 @@ public class Shooter extends SubsystemBase{
return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
}
public Command zeroEncoder(){
return run(() -> {
pivotEncoder.reset();
});
}
public Boolean getBeamBreak(){
return shooterBeamBreak.get();
}