diff --git a/2020Robot/src/main/java/frc/robot/Constants.java b/2020Robot/src/main/java/frc/robot/Constants.java index 2f49dcd..b215c6c 100644 --- a/2020Robot/src/main/java/frc/robot/Constants.java +++ b/2020Robot/src/main/java/frc/robot/Constants.java @@ -36,7 +36,10 @@ public final class Constants { public static final int ElevatorId1 = 22; public static final int CollectorPivotID = 24; public static final int CollectorIntakeID = 23; - public static final double RaisePower = 0.4; //Percent + public static final double RaisePower = 0.45; //Percent public static final double LowerPower = 0.2; //Percent - public static final int TouchSensorID = 30; + public static final int TouchSensorID = 0; + public static final int Climber1ID = 12; + public static final int Climber2ID = 13; + public static final double ClimberSpeed = 0.2; } diff --git a/2020Robot/src/main/java/frc/robot/Robot.java b/2020Robot/src/main/java/frc/robot/Robot.java index 9795357..536dd89 100644 --- a/2020Robot/src/main/java/frc/robot/Robot.java +++ b/2020Robot/src/main/java/frc/robot/Robot.java @@ -13,10 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.Drive; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -35,7 +32,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // Instantiate our This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } @@ -72,7 +69,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - //m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + //m_autonomousCommand = m_getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { diff --git a/2020Robot/src/main/java/frc/robot/RobotContainer.java b/2020Robot/src/main/java/frc/robot/RobotContainer.java index 57ea8ed..4b8e3ef 100644 --- a/2020Robot/src/main/java/frc/robot/RobotContainer.java +++ b/2020Robot/src/main/java/frc/robot/RobotContainer.java @@ -16,16 +16,17 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.commands.Collect123; import frc.robot.commands.Drive; import frc.robot.commands.Shoot; -import frc.robot.commands.collect4; -import frc.robot.commands.collect5; +import frc.robot.commands.collect; import frc.robot.commands.lignUp; +import frc.robot.commands.stopSpin; import frc.robot.subsystems.Chassis; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Collector; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Shooter; @@ -44,6 +45,8 @@ public class RobotContainer { private Joystick joystickOne = new Joystick(0); public static Elevator elevator = new Elevator(); //Defines the Elevator Subsystem. public static Collector collector = new Collector(); + public static Climber climber = new Climber(); + //public static Climber climber = new Climber(); //Defines Climber /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -51,6 +54,8 @@ public class RobotContainer { public RobotContainer() { // set default commands chassis.setDefaultCommand(new Drive(joystickOne)); + collector.setDefaultCommand(new RunCommand(() -> collector.Raise(), collector)); + //climber.setDefaultCommand(new RunCommand(() -> climber.stopClimber())); // Configure the button bindings configureButtonBindings(); } @@ -85,21 +90,14 @@ private void configureButtonBindings() { JoystickButton trigger = new JoystickButton(joystickOne, 1); trigger.whenHeld(new Shoot()); JoystickButton eleven = new JoystickButton(joystickOne, 11); - JoystickButton twelve = new JoystickButton(joystickOne, 12); - eleven.whenPressed(new RunCommand(() -> collector.Raise())); - twelve.whenPressed(new RunCommand(() -> collector.Lower())); - if(collector.getBallCount() == 0 || collector.getBallCount() == 1 || collector.getBallCount() == 2 || collector.getBallCount() == 3){ - new Collect123(); - } - if (collector.getBallCount() == 4){ - new collect4(); - } - if(collector.getBallCount() == 5){ - new collect5(); - } - if (collector.getBallCount() == 5){ - collector.Raise(); - } + JoystickButton eight = new JoystickButton(joystickOne, 8); + eight.whenHeld(new RunCommand(() -> elevator.driveElevator(-0.2))); + eleven.toggleWhenActive(new collect()); + eleven.whenHeld(new RunCommand(() -> collector.Lower())); + JoystickButton ten = new JoystickButton(joystickOne, 10); + JoystickButton nine = new JoystickButton(joystickOne, 9); + ten.whenHeld(new RunCommand(() -> climber.raiseClimber())); + nine.whenHeld(new RunCommand(() -> climber.lowerClimber())); } /** @@ -123,3 +121,8 @@ public Command getAutonomousCommand() { ); } } + ////////// + // // + // ////////// + // // // // // + // // // // // \ No newline at end of file diff --git a/2020Robot/src/main/java/frc/robot/commands/Shoot.java b/2020Robot/src/main/java/frc/robot/commands/Shoot.java index a0e4b34..9a4c769 100644 --- a/2020Robot/src/main/java/frc/robot/commands/Shoot.java +++ b/2020Robot/src/main/java/frc/robot/commands/Shoot.java @@ -21,6 +21,7 @@ public class Shoot extends CommandBase { public Shoot() { addRequirements(RobotContainer.shooter); addRequirements(RobotContainer.elevator); + addRequirements(RobotContainer.collector); // Use addRequirements() here to declare subsystem dependencies. } @@ -36,22 +37,17 @@ public void initialize() { public void execute() { double timerCount = timer.getFPGATimestamp(); if (timerCount >= 1.5){ - RobotContainer.elevator.driveElevator(0.3); + RobotContainer.elevator.driveElevator(0.45); + } else { + RobotContainer.elevator.driveElevator(0); } - if (timerCount <= 0.5){ - NetworkTable ty = NetworkTableInstance.getDefault().getTable("limelight"); - double y = ty.getEntry("ty").getDouble(0.0); - double distance = 53 / Math.tan(Math.toRadians(31) + Math.toRadians(y)); - double shooterset = distance * 0.002; - RobotContainer.shooter.shoot(-0.8, -0.1); - } - if (timerCount > 0.5){ NetworkTable ty = NetworkTableInstance.getDefault().getTable("limelight"); double y = ty.getEntry("ty").getDouble(0.0); - double distance = 53 / Math.tan(Math.toRadians(31) + Math.toRadians(y)); + double distance = 57 / Math.tan(Math.toRadians(31) + Math.toRadians(y)); double shooterset = distance * 0.002; RobotContainer.shooter.shoot(-0.8, 0.5); - } // Le Epic Gamer has died + RobotContainer.collector.spinIntake(0.2); + // Le Epic Gamer has died } // Called once the command ends or is interrupted. @@ -60,6 +56,7 @@ public void end(boolean interrupted) { RobotContainer.shooter.shoot(0, 0); RobotContainer.elevator.driveElevator(0); RobotContainer.collector.resetBallCount(); + RobotContainer.collector.spinIntake(0); } // Returns true when the command should end.