From ae57ae61b87e86196b556d1aac1114198fba1c5f Mon Sep 17 00:00:00 2001 From: Nayan Smuek <44534701+NikolaTesla17@users.noreply.github.com> Date: Wed, 30 Mar 2022 11:34:00 -0700 Subject: [PATCH] fixed auto --- src/main/java/frc/robot/Constants.java | 16 ++++++++-------- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++------ .../commands/autonomous/AutonProcedureHH.java | 2 +- .../commands/autonomous/AutonProcedureLH.java | 2 +- .../robot/commands/climber/ClimberCenter.java | 14 +++++++------- .../java/frc/robot/subsystems/Drivetrain.java | 2 +- 6 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e8df3ff..eafb980 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,14 +29,14 @@ public static final class DipSwitches{ public static final class DrivetrainConstants { public static final class Autonomous { public static final class Turning { - public static final double TURN_SPEED = 0.15; - public static final double TURN_ADJUSMENT =220; + public static final double TURN_SPEED = 0.25; + public static final double TURN_ADJUSMENT =237; - public static final int HHH_TURN = 86; + public static final int HHH_TURN = 72; // public static final int SECOND_HHH_TURN = -70; //public static final int SECOND_HHH_OFFSET = 46; public static final double TURNING_PP = 0.0006; - public static final int SECOND_HHH_TURN = -30; + public static final int SECOND_HHH_TURN = 40; } public static final class Distance { public static final int HIGH_GOAL_SHOT = -40000; @@ -45,7 +45,7 @@ public static final class Distance { } public static final class Offsets { public static final int AUTO_H_H_OFFSET = 0;//-56000; - public static final int AUTO_L_H_OFFSET = -56000; + public static final int AUTO_L_H_OFFSET = -41000; public static final int AUTO_LF_H_OFFSET = -40000; public static final double STEP_TWO_RETURN_OFFSET = -10000; } @@ -216,11 +216,11 @@ public static final class ShooterMotionParameters { public static final double SHOOTER_VELOCITY_HIGH = -4100; //0.17 //0.175 public static final double NOMINAL_HIGH_VELOCITY = 4080; - public static final double SHOOTER_VELOCITY_HIGH_FAR = -4900; //0.17 //0.175 - public static final double NOMINAL_HIGH_VELOCITY_FAR = 4880; + public static final double SHOOTER_VELOCITY_HIGH_FAR = -5520; //0.17 //0.175 + public static final double NOMINAL_HIGH_VELOCITY_FAR = 5500; - public static final double KP = 0.00011; + public static final double KP = 0.00012; public static final double KI = 0; public static final double KD = 0; public static final double KF = 0.00008947; // 0.17/1900 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b2a3c8..9cdbc16 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public class RobotContainer { private final JoystickButton stopAll = new JoystickButton(buttonBoard, 4); - private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7); + //private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7); private final JoystickButton HHHButton = new JoystickButton(leftJoystick, 10); private final JoystickButton TurnButton = new JoystickButton(leftJoystick, 11); @@ -87,9 +87,11 @@ public class RobotContainer { private final JoystickButton indexTwoButton = new JoystickButton(buttonBoard, gitforcepushorginmaster); private final JoystickButton climberControlButton = new JoystickButton(leftJoystick, 3); - private final JoystickButton climberTiltZeroButton = new JoystickButton(rightJoystick, 8); + //private final JoystickButton climberTiltZeroButton = new JoystickButton(rightJoystick, 8); private final JoystickButton climberTiltCenterButton = new JoystickButton(rightJoystick, 6); + private final JoystickButton shootWayDowntownButton = new JoystickButton(leftJoystick, 6); + //private final JoystickButton lockStationariesButton = new JoystickButton(gamepad, 1); //private final JoystickButton nextBarButton = new JoystickButton(gamepad, 2); @@ -104,6 +106,8 @@ public class RobotContainer { private final IntakeIntake intakeIntakeCommand = new IntakeIntake(intake); private final IntakeEject intakeEjectCommand = new IntakeEject(intake); private final BetterIntakeStop intakeStopCommand = new BetterIntakeStop(intake); + private final ShootWayDowntown shootWayDowntownCommand = new ShootWayDowntown(shooter); + private final IndexerFeedWayDowntown indexWayDowntownCommand = new IndexerFeedWayDowntown(indexer, shooter); private final ShootHigh shootHighCommand = new ShootHigh(shooter); private final ShootLow shootLowCommand = new ShootLow(shooter); private final ShootEject shootEjectCommand = new ShootEject(shooter); @@ -121,7 +125,7 @@ public class RobotContainer { private final HookUpLow hookUpLowCommand = new HookUpLow(climber); private final HookDown hookDownCommand = new HookDown(climber); - private final HomeAlgorithm homeAlgorithmCommand = new HomeAlgorithm(climber); + // private final HomeAlgorithm homeAlgorithmCommand = new HomeAlgorithm(climber); private final NextBar nextBarCommand = new NextBar(climber); private final LockStationary lockStationariesCommand = new LockStationary(climber); @@ -158,7 +162,8 @@ private void configureButtonBindings() { shootLowButton.whileHeld(new ParallelCommandGroup(shootLowCommand, feedLowCommand)); ejectButton.whileHeld(new ParallelCommandGroup(intakeEjectCommand, indexerEjectCommand, shootEjectCommand)); - hookZeroButton.whileHeld(hookZeroCommand); + shootWayDowntownButton.whileHeld(new ParallelCommandGroup(shootWayDowntownCommand, indexWayDowntownCommand)); + //hookZeroButton.whileHeld(hookZeroCommand); // hookDownButton.whenPressed(hookDownCommand); // hookUpButton.whenPressed(hookUpCommand); @@ -167,9 +172,9 @@ private void configureButtonBindings() { hookUpLowButton.whenPressed(hookUpLowCommand); hookDownButton.whenPressed(hookDownCommand); - climberTiltZeroButton.whenPressed(homeAlgorithmCommand); + //climberTiltZeroButton.whenPressed(homeAlgorithmCommand); - climberTiltCenterButton.whenPressed(centerClimberCommand); + climberTiltCenterButton.whileHeld(centerClimberCommand); nextBarButton.whenPressed(nextBarCommand); lockStationariesButton.whenPressed(lockStationariesCommand); diff --git a/src/main/java/frc/robot/commands/autonomous/AutonProcedureHH.java b/src/main/java/frc/robot/commands/autonomous/AutonProcedureHH.java index 39bcbbd..e4fcaf5 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutonProcedureHH.java +++ b/src/main/java/frc/robot/commands/autonomous/AutonProcedureHH.java @@ -34,7 +34,7 @@ public AutonProcedureHH(Drivetrain drivetrain, Intake intake, Indexer indexer, S addCommands(new ParallelDeadlineGroup(new WaitCommand(0.6), new IntakeIntake(intake)));//drop the intake addCommands(new StepOne(intake, indexer, drivetrain)); //drive back and grab another ball addCommands(new ParallelDeadlineGroup(new StepTwo(drivetrain, Constants.DrivetrainConstants.Autonomous.Offsets.AUTO_H_H_OFFSET), new BetterIntakeStop(intake), new IndexerStop(indexer), new ShootHigh(shooter))); //drive back to correct point to sink two high shots - addCommands(new ParallelDeadlineGroup(new WaitCommand(6), new ShootHigh(shooter), new IndexerFeedHigh(indexer, shooter))); //hold down the shoot high button for the same number of seconds as the wait command + addCommands(new ParallelDeadlineGroup(new WaitCommand(4), new ShootHigh(shooter), new IndexerFeedHigh(indexer, shooter))); //hold down the shoot high button for the same number of seconds as the wait command addCommands(new ParallelCommandGroup(new IndexerStop(indexer), new ShootStop(shooter))); //stop everything } } diff --git a/src/main/java/frc/robot/commands/autonomous/AutonProcedureLH.java b/src/main/java/frc/robot/commands/autonomous/AutonProcedureLH.java index b9baae9..2a989b2 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutonProcedureLH.java +++ b/src/main/java/frc/robot/commands/autonomous/AutonProcedureLH.java @@ -38,7 +38,7 @@ public AutonProcedureLH(Drivetrain drivetrain, Intake intake, Indexer indexer, S addCommands(new InstantCommand(() -> { shooter.set(0); }, shooter)); //stops the shoot addCommands(new StepOne(intake, indexer, drivetrain)); //drive back and grab another ball addCommands(new ParallelDeadlineGroup(new StepTwo(drivetrain, Constants.DrivetrainConstants.Autonomous.Offsets.AUTO_L_H_OFFSET), new BetterIntakeStop(intake), new IndexerStop(indexer), new ShootHigh(shooter))); //drive back to start point(maybe just put drivetrain falcons in brake?) - addCommands(new ParallelDeadlineGroup(new WaitCommand(3), new ShootHigh(shooter), new IndexerFeedHigh(indexer, shooter))); //shoot your next loaded ball into high goal(needs to be tested) + addCommands(new ParallelDeadlineGroup(new WaitCommand(2), new ShootHigh(shooter), new IndexerFeedHigh(indexer, shooter))); //shoot your next loaded ball into high goal(needs to be tested) addCommands(new ParallelCommandGroup(new IndexerStop(indexer), new ShootStop(shooter))); //stop everything } diff --git a/src/main/java/frc/robot/commands/climber/ClimberCenter.java b/src/main/java/frc/robot/commands/climber/ClimberCenter.java index a49e307..cae7fd1 100644 --- a/src/main/java/frc/robot/commands/climber/ClimberCenter.java +++ b/src/main/java/frc/robot/commands/climber/ClimberCenter.java @@ -21,7 +21,6 @@ public class ClimberCenter extends CommandBase { public ClimberCenter(Climber climber) { this.climber = climber; - done = false; // Use addRequirements() here to declare subsystem dependencies. addRequirements(climber); @@ -30,6 +29,13 @@ public ClimberCenter(Climber climber) { // Called when the command is initially scheduled. @Override public void initialize() { + done = false; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { if(climber.tiltAngle()>0){ climber.setTilt(-ClimberConstants.ClimberMotionParameters.CLIMBER_TILT_ZERO_SPEED); }else if(climber.tiltAngle()<0){ @@ -40,12 +46,6 @@ public void initialize() { } } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 08d2e1e..5540e9c 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -180,7 +180,7 @@ public boolean getAutoEnd(int distance) { } public boolean getAutoEnd() { - return getCurrentEncoderPosition() <= -150000; + return getCurrentEncoderPosition() <= -100000; } public boolean getAutoEndHighReverse() {