diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf42547..d2ce347 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); - public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + //public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); public final TurretSubsystem turretSubsystem = new TurretSubsystem(); @@ -75,11 +75,11 @@ public class RobotContainer { m_driverController.a().onTrue(intakeSubsystem.retractCommand()); m_driverController.b().onTrue(intakeSubsystem.reverseCommand());*/ - m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); + /*m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand()); - intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); + intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));*/ } /** diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index 3827b32..8a23efb 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -26,6 +26,7 @@ public class ManualShootCommand extends Command { if(m_controller.getLeftTriggerAxis()>.5 || m_controller.getRightTriggerAxis()>.5) { m_subsystem.setLauncher(1); + System.out.println("Test"); } else { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 0b40532..eb162e0 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -33,11 +33,9 @@ public class TurretSubsystem extends TestableSubsystem { //private TalonFX turretMotor; private SparkMax launcherMotor; private Servo hood; - private CANcoder hoodEncoder; private Angle limitPos; private Angle limitNeg; private boolean turretGoingPos; - private Angle targElev; public TurretSubsystem() { turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL); @@ -50,22 +48,17 @@ public class TurretSubsystem extends TestableSubsystem { config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); - hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID); currentState = TurretState.INIT; limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS; turretGoingPos = false; - targElev = edu.wpi.first.units.Units.Degrees.of(45); } public void setLauncher(double speed) {launcherMotor.set(speed);} public void setTargetElevation(Angle elev) { - if(elev.gt(Constants.ShooterConstants.LIMIT_ELEVATION_NEG) && elev.lt(Constants.ShooterConstants.LIMIT_ELEVATION_POS)) - { - targElev=elev; - } + hood.setAngle(elev.in(Units.Degrees)); } public void startHoming() @@ -133,26 +126,6 @@ public class TurretSubsystem extends TestableSubsystem { //turretMotor.set(0); }*/ - int k = 1; - if(Constants.ShooterConstants.HOOD_SERVO_INVERTED) { - k=-1; - } - Angle curElev = hoodEncoder.getAbsolutePosition().getValue(); - if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees)) - { - if(targElev.gt(curElev)) - { - hood.set(k * 0.5); - } - else - { - hood.set(k * -0.5); - } - } - else - { - hood.set(0); - } } }