diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 48a40c4..66537a3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -191,8 +191,8 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return new AutoPath1FromCenter(m_robotDrive); - //return new AutoPath2FromRight(m_robotDrive); + return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); + //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); } TrajectoryConfig getTrajectoryConfig() { diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java index e941c05..4ec080a 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java @@ -9,32 +9,35 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class AutoPath1FromCenter extends SequentialCommandGroup { Drive m_drive; + Pneumatics m_pneumatics; /** * Creates a new AutoPath1FromCenter. */ - public AutoPath1FromCenter(Drive subsystem) { + public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; + m_pneumatics = subsystem2; addCommands( new Wait(m_drive, 0, 1), //shoot pre-loaded 3 balls - new GotoCoordinates(m_drive, 75, 44, -90), + new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90), //Start Intake Ball 1 - new GotoCoordinates(m_drive, 0, 12), - new GotoCoordinates(m_drive, 0, 28), + new GotoCoordinates(m_drive, m_pneumatics, 0, 12), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, 0, 8), - new GotoCoordinates(m_drive, 0, 28), + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 3 - new GotoCoordinates(m_drive, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), new Wait(m_drive, 0, 2) //Shoot 3 Balls ); diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java index 2411523..43657c3 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -9,34 +9,37 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class AutoPath2FromRight extends SequentialCommandGroup { Drive m_drive; + Pneumatics m_pneumatics; /** * Creates a new AutoPath2FromRight. */ - public AutoPath2FromRight(Drive subsystem) { + public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; + m_pneumatics = subsystem2; addCommands( new Wait(m_drive, 0, 1), - new GotoCoordinates(m_drive, 0, 77), + new GotoCoordinates(m_drive, m_pneumatics, 0, 77), //Start Intake Ball 1 - new GotoCoordinates(m_drive, 0, 8), - new GotoCoordinates(m_drive, 0, 28), + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), //Shoot 5 Balls - new GotoCoordinates(m_drive, 0, 28), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 1 (second round) - new GotoCoordinates(m_drive, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), //Start Moving to 4th Ball - new GotoCoordinates(m_drive, 60, -50), + new GotoCoordinates(m_drive, m_pneumatics, 60, -50), new Wait(m_drive, 0, 2) ); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index b7c74bb..c9aa927 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -30,6 +30,7 @@ public class DriveStraightToPositionMM extends CommandBase { public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = subsystem2; try { if (m_pneumatics.m_isSpeedShiftHigh) { m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index c8a339b..a457e26 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -29,6 +29,7 @@ public class DriveStraightToPositionPID extends CommandBase { public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = subsystem2; try { if (m_pneumatics.m_isSpeedShiftHigh) { m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 244905b..6ca3deb 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -10,12 +10,14 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class GotoCoordinates extends SequentialCommandGroup { Drive m_drive; + Pneumatics m_pneumatics; double m_xTarget; double m_yTarget; @@ -26,10 +28,11 @@ public class GotoCoordinates extends SequentialCommandGroup { /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) { + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; + m_pneumatics = subsystem2; m_xTarget = xTarget; m_yTarget = yTarget; @@ -42,12 +45,13 @@ public class GotoCoordinates extends SequentialCommandGroup { addCommands( new TurnDegrees(m_drive, m_currentAngle), new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_hypotDist), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) { + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { m_drive = subsystem; + m_pneumatics = subsystem2; m_xTarget = xTarget; m_yTarget = yTarget; @@ -60,7 +64,7 @@ public class GotoCoordinates extends SequentialCommandGroup { addCommands( new TurnDegrees(m_drive, m_currentAngle), new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_hypotDist), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); }