diff --git a/src/main/deploy/pathplanner/paths/Center Sub to Note 1.path b/src/main/deploy/pathplanner/paths/Center Sub to Note 1.path index a2e0332..5e78848 100644 --- a/src/main/deploy/pathplanner/paths/Center Sub to Note 1.path +++ b/src/main/deploy/pathplanner/paths/Center Sub to Note 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.421132842265685, - "y": 6.728691171668058 + "x": 2.5050384003901907, + "y": 6.829386194384925 }, "prevControl": { - "x": 2.1572626002394863, - "y": 6.3328858086287605 + "x": 2.241168158363992, + "y": 6.433580831345627 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b17fb25..ab6cda3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,7 +140,7 @@ public class RobotContainer { 1.0, 1.0 ).alongWith(indexer.shootNote(() -> 1.0)) - .withTimeout(1.0) + .withTimeout(0.75) ) ); @@ -168,7 +168,7 @@ public class RobotContainer { .andThen( intake.stopAll() ) - //.withTimeout(1.0) + .withTimeout(2.5) ); //NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note")); @@ -226,9 +226,9 @@ public class RobotContainer { drivetrain.setDefaultCommand( drivetrain.teleopCommand( - () -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); }, - () -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); }, - () -> { return driver.getRightX() * (invertXYDrive ? -1 : 1); }, + () -> { return driver.getLeftY(); /*(invertXYDrive ? -1 : 1);*/ }, + () -> { return driver.getLeftX(); /*(invertXYDrive ? -1 : 1);*/ }, + () -> { return driver.getRightX(); /*(invertXYDrive ? -1 : 1);*/ }, OIConstants.kTeleopDriveDeadband ) ); @@ -409,17 +409,19 @@ public class RobotContainer { //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0)); - operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState(), intake.climbingStateCommand())); + operator.y().toggleOnTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState(), intake.climbingStateCommand())); driver.a().whileTrue(indexer.shootNote(() -> 1.0)); operator.back().toggleOnTrue(shooter.manualPivot( - () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5), + () -> MathUtil.clamp(-operator.getLeftY(), -0.25, 0.25), () -> MathUtil.clamp(driver.getRightTriggerAxis(), -1, 1) )); driver.povDown().whileTrue(indexer.shootNote(() -> -1.0)); + operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeConstants.kDownAngle, () -> -1.0))); + operator.start().toggleOnTrue(intake.manualPivot(() -> -operator.getRightY()*0.2, () -> driver.getLeftTriggerAxis())); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 31a6078..13c8c3f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -326,9 +326,9 @@ public class Drivetrain extends SubsystemBase { public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) { return run(() -> { drive( - MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), - MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), - MathUtil.applyDeadband(rotation.getAsDouble(), deadband), + -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), + -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), + -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), () -> fieldRelativeControl, true ); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0aab41f..05185f0 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -116,6 +116,7 @@ public class Intake extends SubsystemBase{ ); }); } + public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){ return run(() ->{ diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5e355d4..90a89f6 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; @@ -144,8 +145,12 @@ public class Shooter extends SubsystemBase{ public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){ return run(() ->{ shooterPivot.setIdleMode(IdleMode.kBrake); - if(getShooterAngle() >= ShooterConstants.kShooterLoadAngle || getShooterAngle() <= ShooterConstants.kShooterAmpAngle){ + if(getShooterAngle() >= ShooterConstants.kShooterLoadAngle && getShooterAngle() <= ShooterConstants.kShooterAmpAngle){ shooterPivot.set(pivotPower.getAsDouble()); + }else if(getShooterAngle() > ShooterConstants.kShooterAmpAngle){ + shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), -1.0, 0.0)); + }else if(getShooterAngle() < ShooterConstants.kShooterLoadAngle){ + shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), 0.0, 1.0)); } topShooter.set(spinny.getAsDouble());