From d1254f1e1d6b27de70ab9151f197fd556e619e57 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 13 Mar 2020 08:37:28 -0600 Subject: [PATCH] Program Simple Auto --- PathWeaver/Groups/TenBallAuto | 2 + PathWeaver/Paths/TenBallMidComplete | 2 - .../driverStation/GOAT DRIVERSTATION.json | 159 +++++++++++++++++- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 12 +- .../commands/auto/TenBallAutoMiddle.java | 35 +++- .../commands/shooter/CalibrateShooter.java | 5 + .../robot/commands/shooter/PrepChecker.java | 52 ++++++ 8 files changed, 255 insertions(+), 14 deletions(-) create mode 100644 PathWeaver/Groups/TenBallAuto create mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java diff --git a/PathWeaver/Groups/TenBallAuto b/PathWeaver/Groups/TenBallAuto new file mode 100644 index 0000000..87ec839 --- /dev/null +++ b/PathWeaver/Groups/TenBallAuto @@ -0,0 +1,2 @@ +SixBallMidComplete +TenBallMidComplete diff --git a/PathWeaver/Paths/TenBallMidComplete b/PathWeaver/Paths/TenBallMidComplete index ead609f..a87ff34 100644 --- a/PathWeaver/Paths/TenBallMidComplete +++ b/PathWeaver/Paths/TenBallMidComplete @@ -1,6 +1,4 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -3.2,-2.4,0.2,2.5,true, -5.0,-1.1,3.0,0.0,true, 7.2,-1.1,1.5,0.0,true, 6.981146126504424,-1.7267749571307522,-0.16656954801462032,-0.6424825423421034,true, 6.766985279057057,-2.6072139966365975,-0.1189782485818709,-0.2974456214546768,true, diff --git a/src/main/driverStation/GOAT DRIVERSTATION.json b/src/main/driverStation/GOAT DRIVERSTATION.json index a191e6e..e61f688 100644 --- a/src/main/driverStation/GOAT DRIVERSTATION.json +++ b/src/main/driverStation/GOAT DRIVERSTATION.json @@ -335,6 +335,155 @@ "Colors/Color when true": "#7CFC00FF", "Colors/Color when false": "#8B0000FF" } + }, + "8,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance to Target", + "_title": "Distance to Target" + } + }, + "9,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball in Intake!", + "_title": "!Ball in Intake!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "0,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball Storage!", + "_title": "!Ball Storage!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "1,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball Shooter!", + "_title": "!Ball Shooter!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "2,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Center Displacement", + "_title": "Center Displacement" + } + }, + "3,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Is Auto Start?", + "_title": "Is Auto Start?" + } + }, + "4,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/trajectoryPath Initial", + "_title": "trajectoryPath Initial" + } + }, + "5,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Motor Pos Inches", + "_title": "Left Motor Pos Inches" + } + }, + "6,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Motor Pos Inches", + "_title": "Right Motor Pos Inches" + } + }, + "7,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Motor Pos Meters", + "_title": "Left Motor Pos Meters" + } + }, + "8,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Motor Pos Meters", + "_title": "Right Motor Pos Meters" + } + }, + "9,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Odometry Values Meters", + "_title": "Odometry Values Meters" + } + }, + "0,5": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Trajectory Total Time", + "_title": "Trajectory Total Time" + } } } } @@ -645,8 +794,8 @@ "Controls/Rotation": "NONE", "compression": -1.0, "fps": -1, - "imageWidth": -1, - "imageHeight": -1 + "imageWidth": 0, + "imageHeight": 0 } }, "8,4": { @@ -857,9 +1006,9 @@ } ], "windowGeometry": { - "x": 40.0, - "y": 142.39999389648438, + "x": -6.400000095367432, + "y": 1.600000023841858, "width": 1547.199951171875, - "height": 1481.5999755859375 + "height": 828.7999877929688 } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a69160e..543cbb5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -171,7 +171,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 1.0; + public static final double STORAGE_SPEED = 0.5; public static final double STORAGE_TIMEOUT = 3000; /* Storage Characteristics */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dca2c7e..788f91b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -269,6 +269,8 @@ public class RobotContainer { } public void buildAutos() { + resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); + String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; @@ -300,10 +302,12 @@ public class RobotContainer { m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); String[] tenBallAutoMiddlePaths = new String[]{ + "SixBallMidComplete", "TenBallMidComplete" }; - m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); + m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, + m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths)); } /** @@ -321,12 +325,12 @@ public class RobotContainer { try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); diff --git a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java index 7ff19da..28ae59c 100644 --- a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java @@ -7,9 +7,21 @@ package frc4388.robot.commands.auto; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.PrepChecker; +import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.storage.RunStorage; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -18,11 +30,30 @@ public class TenBallAutoMiddle extends SequentialCommandGroup { /** * Creates a new TenBallAutoMiddle. */ - public TenBallAutoMiddle(Drive drive, RamseteCommand[] paths) { + public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( - paths[0] + new ParallelDeadlineGroup( + new Wait(drive, 0.1, 0), + new CalibrateShooter(shooter, shooterAim, shooterHood) + ), + new ParallelDeadlineGroup( + new Wait(drive, 1, 0), + new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim) + ), + new ParallelDeadlineGroup( + new Wait(drive, 4, 0), + new PrepChecker(shooter, shooterAim), + new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake), + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage) + ), + new ParallelDeadlineGroup( + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage), + new RunStorage(storage) + ) + //paths[0], + //paths[1] ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 23be5fa..5e28114 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.shooter; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.SoftLimitDirection; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -59,6 +60,10 @@ public class CalibrateShooter extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() && + m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) { + return true; + } return false; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java new file mode 100644 index 0000000..cc978fd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; + +public class PrepChecker extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + + /** + * Creates a new PrepChecker. + */ + public PrepChecker(Shooter shooter, ShooterAim shooterAim) { + // Use addRequirements() here to declare subsystem dependencies. + m_shooter = shooter; + m_shooterAim = shooterAim; + } + + // Called when the command is initially scheduled. + @Override + 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) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_shooterAim.m_isAimReady && m_shooter.m_isDrumReady) { + return true; + } + + return false; + } +}