Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

delete all the things

Browse Source
This commit is contained in:
aarav18
2022-02-18 19:16:59 -07:00
parent 816a4c0aea
commit cb1e06b72f
62 changed files with 240 additions and 3888 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.OutlineViewer/outlineviewer-window.json
+35
View File
@@ -0,0 +1,35 @@
{
"MainWindow": {
"GLOBAL": {
"height": "0",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "0",
"xpos": "-32000",
"ypos": "-32000"
}
},
"Window": {
"About": {
"Collapsed": "0",
"Pos": "13,143",
"Size": "574,113"
},
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Entries": {
"Collapsed": "0",
"Pos": "0,0",
"Size": "46,32"
},
"Settings": {
"Collapsed": "0",
"Pos": "173,161",
"Size": "272,123"
}
}
}
.OutlineViewer/outlineviewer.json
+14
View File
@@ -0,0 +1,14 @@
{
"CameraPublisher": {
"limelight": {
"open": true
},
"open": true
},
"NetworkTables Settings": {
"mode": "Client"
},
"limelight": {
"open": true
}
}
src/main/java/frc4388/robot/Robot.java
+5 -5
View File
@@ -77,13 +77,13 @@ public class Robot extends TimedRobot {
/* Builds Autos */ /* Builds Autos */
m_robotContainer.buildAutos(); m_robotContainer.buildAutos();
SmartDashboard.putString("Is Auto Start?", "NAH"); SmartDashboard.putString("Is Auto Start?", "NAH");
m_robotContainer.m_robotLime.limeOff(); // m_robotContainer.m_robotLime.limeOff();
} }
@Override @Override
public void disabledPeriodic() { public void disabledPeriodic() {
m_robotContainer.resetOdometry(new Pose2d()); m_robotContainer.resetOdometry(new Pose2d());
m_robotContainer.idenPath(); // m_robotContainer.idenPath();
if (m_modeChooser.getSelected() != Mode.get()) if (m_modeChooser.getSelected() != Mode.get())
Mode.set(m_modeChooser.getSelected()); Mode.set(m_modeChooser.getSelected());
} }
@@ -97,7 +97,7 @@ public class Robot extends TimedRobot {
m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
m_robotContainer.setDriveGearState(true); // m_robotContainer.setDriveGearState(true);
m_initialTime = System.currentTimeMillis(); m_initialTime = System.currentTimeMillis();
@@ -135,9 +135,9 @@ public class Robot extends TimedRobot {
@Override @Override
public void teleopInit() { public void teleopInit() {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true); // m_robotContainer.setDriveGearState(true);
m_robotContainer.shiftClimberRatchet(false); // m_robotContainer.shiftClimberRatchet(false);
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
// This makes sure that the autonomous stops running when // This makes sure that the autonomous stops running when
src/main/java/frc4388/robot/RobotContainer.java
+160 -159
View File
@@ -30,50 +30,50 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.Mode; import frc4388.robot.Constants.Mode;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.InterruptSubsystem; // import frc4388.robot.commands.InterruptSubsystem;
import frc4388.robot.commands.auto.Barrel; // import frc4388.robot.commands.auto.Barrel;
import frc4388.robot.commands.auto.BarrelMany; // import frc4388.robot.commands.auto.BarrelMany;
import frc4388.robot.commands.auto.BarrelStart; // import frc4388.robot.commands.auto.BarrelStart;
import frc4388.robot.commands.auto.Bounce; // import frc4388.robot.commands.auto.Bounce;
import frc4388.robot.commands.auto.DriveOffLineBackward; // import frc4388.robot.commands.auto.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward; // import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle; // import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.FiveBallAutoMiddle; // import frc4388.robot.commands.auto.FiveBallAutoMiddle;
import frc4388.robot.commands.auto.GalacticSearch; // import frc4388.robot.commands.auto.GalacticSearch;
import frc4388.robot.commands.auto.SequentialTest; // import frc4388.robot.commands.auto.SequentialTest;
import frc4388.robot.commands.auto.SixBallAutoMiddle; // import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Slalom; // import frc4388.robot.commands.auto.Slalom;
import frc4388.robot.commands.auto.TenBallAutoMiddle; // import frc4388.robot.commands.auto.TenBallAutoMiddle;
import frc4388.robot.commands.climber.DisengageRatchet; // import frc4388.robot.commands.climber.DisengageRatchet;
import frc4388.robot.commands.climber.RunClimberWithTriggers; // import frc4388.robot.commands.climber.RunClimberWithTriggers;
import frc4388.robot.commands.climber.RunLevelerWithJoystick; // import frc4388.robot.commands.climber.RunLevelerWithJoystick;
import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.PlaySongDrive; import frc4388.robot.commands.drive.PlaySongDrive;
import frc4388.robot.commands.drive.SetShooterToOdo; // import frc4388.robot.commands.drive.SetShooterToOdo;
import frc4388.robot.commands.drive.VisionUpdateOdometry; import frc4388.robot.commands.drive.VisionUpdateOdometry;
import frc4388.robot.commands.intake.RunIntakeWithTriggers; // import frc4388.robot.commands.intake.RunIntakeWithTriggers;
import frc4388.robot.commands.shooter.CalibrateShooter; // import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.RunHoodWithJoystick; // import frc4388.robot.commands.shooter.RunHoodWithJoystick;
import frc4388.robot.commands.shooter.ShootPrepGroup; // import frc4388.robot.commands.shooter.ShootPrepGroup;
import frc4388.robot.commands.shooter.ShooterManual; // import frc4388.robot.commands.shooter.ShooterManual;
import frc4388.robot.commands.shooter.ShooterTrenchPosition; // import frc4388.robot.commands.shooter.ShooterTrenchPosition;
import frc4388.robot.commands.shooter.TrackTarget; // import frc4388.robot.commands.shooter.TrackTarget;
import frc4388.robot.commands.shooter.TrimShooter; // import frc4388.robot.commands.shooter.TrimShooter;
import frc4388.robot.commands.storage.ManageStorage; // import frc4388.robot.commands.storage.ManageStorage;
import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Camera;
import frc4388.robot.subsystems.Climber; // import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake; // import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Leveler; // import frc4388.robot.subsystems.Leveler;
import frc4388.robot.subsystems.LimeLight; // import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Pneumatics;
import frc4388.robot.subsystems.Shooter; // import frc4388.robot.subsystems.Shooter_1;
import frc4388.robot.subsystems.ShooterAim; // import frc4388.robot.subsystems.ShooterAim_1;
import frc4388.robot.subsystems.ShooterHood; // import frc4388.robot.subsystems.ShooterHood_1;
import frc4388.robot.subsystems.Storage; // import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.Storage.StorageMode; // import frc4388.robot.subsystems.Storage.StorageMode;
import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.ButtonFox;
import frc4388.utility.controller.IHandController; import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxController;
@@ -87,20 +87,20 @@ import frc4388.utility.controller.XboxController;
public class RobotContainer { public class RobotContainer {
/* Subsystems */ /* Subsystems */
private final Drive m_robotDrive = new Drive(); private final Drive m_robotDrive = new Drive();
private final Pneumatics m_robotPneumatics = new Pneumatics(); // private final Pneumatics m_robotPneumatics = new Pneumatics();
private final LED m_robotLED = new LED(); private final LED m_robotLED = new LED();
private final Intake m_robotIntake = new Intake(); // private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter(); // private final Shooter_1 m_robotShooter = new Shooter_1();
private final ShooterAim m_robotShooterAim = new ShooterAim(); // private final ShooterAim_1 m_robotShooterAim = new ShooterAim_1();
private final ShooterHood m_robotShooterHood = new ShooterHood(); // private final ShooterHood_1 m_robotShooterHood = new ShooterHood_1();
private final Climber m_robotClimber = new Climber(); // private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler(); // private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage(); // private final Storage m_robotStorage = new Storage();
/* Cameras */ /* Cameras */
private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
public final LimeLight m_robotLime = new LimeLight(); // public final LimeLight m_robotLime = new LimeLight();
public final Vision m_robotVision = new Vision(); public final Vision m_robotVision = new Vision();
/* Controllers */ /* Controllers */
@@ -111,22 +111,22 @@ public class RobotContainer {
private static XboxController m_manualXbox = new XboxController(3); private static XboxController m_manualXbox = new XboxController(3);
/* Autos */ /* Autos */
double m_totalTimeAuto; // double m_totalTimeAuto;
SixBallAutoMiddle m_sixBallAutoMiddle; // SixBallAutoMiddle m_sixBallAutoMiddle;
SixBallAutoMiddle m_sixBallAutoMiddle0; // SixBallAutoMiddle m_sixBallAutoMiddle0;
SixBallAutoMiddle m_sixBallAutoMiddle1; // SixBallAutoMiddle m_sixBallAutoMiddle1;
EightBallAutoMiddle m_eightBallAutoMiddle; // EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward; // DriveOffLineForward m_driveOffLineForward;
DriveOffLineBackward m_driveOffLineBackward; // DriveOffLineBackward m_driveOffLineBackward;
FiveBallAutoMiddle m_fiveBallAutoMiddle; // FiveBallAutoMiddle m_fiveBallAutoMiddle;
TenBallAutoMiddle m_tenBallAutoMiddle; // TenBallAutoMiddle m_tenBallAutoMiddle;
Slalom m_slalom; // Slalom m_slalom;
Barrel m_barrel; // Barrel m_barrel;
BarrelStart m_barrelStart; // BarrelStart m_barrelStart;
BarrelMany m_barrelMany; // BarrelMany m_barrelMany;
Bounce m_bounce; // Bounce m_bounce;
SequentialTest m_sequentialTest; // SequentialTest m_sequentialTest;
GalacticSearch m_galacticSearch; // // GalacticSearch m_galacticSearch;
public static boolean m_isShooterManual = false; public static boolean m_isShooterManual = false;
@@ -140,12 +140,12 @@ public class RobotContainer {
public void setControls(Mode mode) { public void setControls(Mode mode) {
CommandScheduler.getInstance().clearButtons(); CommandScheduler.getInstance().clearButtons();
/* Passing Drive and Pneumatics Subsystems */ /* Passing Drive and Pneumatics Subsystems */
m_robotPneumatics.passRequiredSubsystem(m_robotDrive); // m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); // m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter);
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); // m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
m_robotShooterHood.passRequiredSubsystem(m_robotShooter); // m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); // m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
m_robotLeveler.passRequiredSubsystem(m_robotClimber); // m_robotLeveler.passRequiredSubsystem(m_robotClimber);
final double drumShooterTargetPIDVelocity; final double drumShooterTargetPIDVelocity;
@@ -156,9 +156,9 @@ public class RobotContainer {
// buildAutos(); // buildAutos();
/* Default Commands */ /* Default Commands */
// drives climber with input from triggers on the opperator controller // drives climber with input from triggers on the opperator controller
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
// drives the leveler with an axis input from the driver controller // drives the leveler with an axis input from the driver controller
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController()));
} else { } else {
drumShooterTargetPIDVelocity = 0; drumShooterTargetPIDVelocity = 0;
configureButtonBindingsCasual(); configureButtonBindingsCasual();
@@ -166,21 +166,21 @@ public class RobotContainer {
/* Default Commands */ /* Default Commands */
// drives the robot with a two-axis input from the driver controller // drives the robot with a two-axis input from the driver controller
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); // m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
// m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); // m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
// drives intake with input from triggers on the opperator controller // drives intake with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// runs the turret with joystick // runs the turret with joystick
m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim));
// runs the hood with joystick // runs the hood with joystick
m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); // m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
// moves the drum not // moves the drum not
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(drumShooterTargetPIDVelocity), m_robotShooter)); // m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(drumShooterTargetPIDVelocity), m_robotShooter));
// continually sends updates to the Blinkin LED controller to keep the lights on // continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
// runs the storage not // runs the storage not
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); // m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime)); //m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime));
} }
/** /**
@@ -192,7 +192,7 @@ public class RobotContainer {
private void configureButtonBindingsCompetitive() { private void configureButtonBindingsCompetitive() {
/* Test Buttons */ /* Test Buttons */
// A driver test button // A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)); // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood));
// B driver test button // B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
// Y driver test button // Y driver test button
@@ -201,37 +201,37 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand()); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand());
/* Driver Buttons */ /* Driver Buttons */
// sets solenoids into high gear // sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
// sets solenoids into low gear // sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); // new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
// Disengages the ratchet to allow for a climb // Disengages the ratchet to allow for a climb
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber)); // new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber));
/* Operator Buttons */ /* Operator Buttons */
// shoots until released // shoots until released
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, // .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood,
// m_robotStorage), false); // m_robotStorage), false);
// .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
// .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); // .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage));
// shoots one ball // shoots one ball
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
// .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, // .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood,
// m_robotStorage), false); // m_robotStorage), false);
// .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
// .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); // .whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage));
// extends or retracts the extender // extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
// VOP system testing // VOP system testing
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); // .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive)); // .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive));
// new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
@@ -240,115 +240,115 @@ public class RobotContainer {
// starts tracking target // starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff)); // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff));
// .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID()));
// .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000)));
// .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) // .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
// .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); // .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
// Trims shooter // Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter)); // new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter));
// Calibrates turret and hood // Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); // new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
// Run drum // Run drum
/* Button Box */ /* Button Box */
// Storage Manual // Storage Manual
new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); // new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET)));
// Meg // Meg
new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); // new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive));
// Shooter Manual // Shooter Manual
new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false)); // new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false));
// Goal Shooter Position // Goal Shooter Position
new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); // new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive));
// Trench Shooter Position // Trench Shooter Position
new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim)); // new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim));
// .whenPressed(new SkipSong(m_robotDrive, 1)); // .whenPressed(new SkipSong(m_robotDrive, 1));
} }
private void configureButtonBindingsCasual() { private void configureButtonBindingsCasual() {
/* Driver Buttons */ /* Driver Buttons */
// sets solenoids into high gear // sets solenoids into high gear
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive));
// sets solenoids into low gear // sets solenoids into low gear
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); // new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive));
/* Operator Buttons */ /* Operator Buttons */
// storage // storage
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
.whenReleased(new InterruptSubsystem(m_robotStorage)); // .whenReleased(new InterruptSubsystem(m_robotStorage));
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
.whenReleased(new InterruptSubsystem(m_robotStorage)); // .whenReleased(new InterruptSubsystem(m_robotStorage));
// extends or retracts the extender // extends or retracts the extender
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) // .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) // .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// Calibrates turret and hood // // Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); // .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
// Run drum manual // // Run drum manual
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000))) // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000)))
.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); // .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
} }
public void buildAutos() { public void buildAutos() {
// resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); // resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" }; // String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" };
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); // m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" }; // String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" };
m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths)); // m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" }; // String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" };
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths)); // m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
String[] slalom = new String[] { "Slalom" }; // String[] slalom = new String[] { "Slalom" };
m_slalom = new Slalom(m_robotDrive, buildPaths(slalom)); // m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
String[] barrel = new String[] { "BarrelStart" }; // String[] barrel = new String[] { "BarrelStart" };
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel)); // m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
String[] barrelStart = new String[] { "Barrel" }; // String[] barrelStart = new String[] { "Barrel" };
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart)); // m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" }; // String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" };
m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce)); // m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
String[] barrelMany = new String[] { "BarrelManyWaypoints" }; // String[] barrelMany = new String[] { "BarrelManyWaypoints" };
m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany)); // m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" }; // String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" };
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); // m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" }; // String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" };
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); // m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" }; // String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" };
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); // m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" }; // String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" };
m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); // m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
String[] tenBallAutoMiddlePaths = new String[] { "SixBallMidComplete", "TenBallMidComplete" }; String[] tenBallAutoMiddlePaths = new String[] { "SixBallMidComplete", "TenBallMidComplete" };
m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); // m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
String[] galacticSearchPaths = new String[] { "GSC_ARED", "GSC_ABLUE", "GSC_BRED", "GSC_BBLUE" }; String[] galacticSearchPaths = new String[] { "GSC_ARED", "GSC_ABLUE", "GSC_BRED", "GSC_BBLUE" };
m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths)); // m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
} }
public void idenPath() { // public void idenPath() {
m_robotLime.identifyPath(); // m_robotLime.identifyPath();
} // }
/** /**
* Use this to pass the autonomous command to the main {@link Robot} class. * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -362,7 +362,7 @@ public class RobotContainer {
// RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
// Run path following command, then stop at the end. // Run path following command, then stop at the end.
try { try {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); // 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_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
// return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
@@ -373,13 +373,14 @@ public class RobotContainer {
// return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); // return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
// return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); // return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
// return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); // return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) { } catch (Exception e) {
System.err.println("ERROR"); System.err.println("ERROR");
} }
return new InstantCommand(); return new InstantCommand();
} }
//TODO
TrajectoryConfig getTrajectoryConfig() { TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
// Add kinematics to ensure max speed is actually obeyed // Add kinematics to ensure max speed is actually obeyed
@@ -394,7 +395,7 @@ public class RobotContainer {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
double[] times = new double[paths.length]; double[] times = new double[paths.length];
Trajectory initialTrajectory; Trajectory initialTrajectory;
m_totalTimeAuto = 0; // m_totalTimeAuto = 0;
try { try {
if (true) { if (true) {
@@ -424,9 +425,9 @@ public class RobotContainer {
DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); DriverStation.reportError("Unable to open trajectory", e.getStackTrace());
} }
for (int i = 0; i < times.length; i++) { // for (int i = 0; i < times.length; i++) {
m_totalTimeAuto += times[i]; // m_totalTimeAuto += times[i];
} // }
return ramseteCommands; return ramseteCommands;
} }
@@ -442,13 +443,13 @@ public class RobotContainer {
* Sets the gear of the drivetrain * Sets the gear of the drivetrain
* @param state the gearing of the gearbox (true is high, false is low) * @param state the gearing of the gearbox (true is high, false is low)
*/ */
public void setDriveGearState(boolean state) { // public void setDriveGearState(boolean state) {
m_robotPneumatics.setShiftState(state); // m_robotPneumatics.setShiftState(state);
} // }
public void shiftClimberRatchet(boolean state) { // public void shiftClimberRatchet(boolean state) {
m_robotClimber.shiftServo(state); // m_robotClimber.shiftServo(state);
} // }
public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
m_robotDrive.setOdometry(pose); m_robotDrive.setOdometry(pose);
src/main/java/frc4388/robot/commands/InterruptSubsystem.java
-42
View File
@@ -1,42 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class InterruptSubsystem extends CommandBase {
/**
* Creates a new InterruptSubsystem.
*/
public InterruptSubsystem(SubsystemBase subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem);
}
// 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() {
return true;
}
}
src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java
-46
View File
@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative;
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, 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 GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 75, 44, -90),
//Start Intake Ball 1
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 12),
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 2
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 3
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
new Wait(m_drive, 0, 2)
//Shoot 3 Balls
);
}
}
src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java
-54
View File
@@ -1,54 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative;
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, 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 GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 77),
//Start Intake Ball 1
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 2
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
//Shoot 5 Balls
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28),
//Start Intake Ball 6 (Ball 1 second round)
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8),
//Move to 7th Ball
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 86.7, -64.11, -180),
//Move to 8th Ball
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 15.31, 90),
//Move to 9th Ball
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 7.11, 24.41, 0),
//Move to 10th Ball
new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 13.30),
//Shoot 5 more Balls (Total 10 Ball Autonomous Path)
new Wait(m_drive, 0, 2)
);
}
}
src/main/java/frc4388/robot/commands/auto/Barrel.java
-28
View File
@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// 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 Barrel extends SequentialCommandGroup {
/**
* Creates a new Barrel.
*/
public Barrel(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/BarrelMany.java
-27
View File
@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// 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 BarrelMany extends SequentialCommandGroup {
/**
* Creates a new BarrelMany.
*/
public BarrelMany(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/BarrelStart.java
-34
View File
@@ -1,34 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// 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 BarrelStart extends SequentialCommandGroup {
/**
* Creates a new BarrelStart.
*/
public BarrelStart(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
//new Wait(drive, 0.01, 1),
new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
);
}
}
src/main/java/frc4388/robot/commands/auto/Bounce.java
-35
View File
@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// 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 Bounce extends SequentialCommandGroup {
/**
* Creates a new Bounce.
*/
public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
paths[1],
paths[2],
paths[3]
);
}
}
src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java
-31
View File
@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.drive.TurnDegrees;
import frc4388.robot.subsystems.Drive;
// 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 DriveOffLineBackward extends SequentialCommandGroup {
/**
* Creates a new DriveOffLineBackward.
*/
public DriveOffLineBackward(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
new TankDriveVelocity(drive, -1, -1, 2)
);
}
}
src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
-34
View File
@@ -1,34 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// 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 DriveOffLineForward extends SequentialCommandGroup {
/**
* Creates a new DriveOffLineForward.
*/
public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
new InstantCommand(() -> drive.switchReversed(true)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java
-29
View File
@@ -1,29 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// 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 EightBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new EightBallAutoMiddle.
*/
public EightBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java
-65
View File
@@ -1,65 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc4388.robot.subsystems.LimeLight;
public class ExecuteCommand extends CommandBase {
/**
* Creates a new ExecuteCommand.
*/
RamseteCommand[] m_paths;
LimeLight m_limeLight;
public ExecuteCommand(LimeLight limeLight, RamseteCommand[] paths) {
// Use addRequirements() here to declare subsystem dependencies.
m_limeLight = limeLight;
m_paths = paths;
}
// 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() {
String gsPath = m_limeLight.galacticSearchPath;
switch (gsPath)
{
case "A_RED":
new RunPath(m_paths[0]);
break;
case "A_BLUE":
new RunPath(m_paths[1]);
break;
case "B_RED":
new RunPath(m_paths[2]);
break;
case "B_BLUE":
new RunPath(m_paths[3]);
break;
case "test":
new RunPath(m_paths[0]);
break;
}
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java
-28
View File
@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// 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 FiveBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new FiveBallAutoMiddle.
*/
public FiveBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Use addRequirements() here to declare subsystem dependencies.
addCommands(
paths[0],
new TankDriveVelocity(drive, -3.2, -0.2, 0.8)
);
}
}
src/main/java/frc4388/robot/commands/auto/GalacticSearch.java
-51
View File
@@ -1,51 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LimeLight;
// 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 GalacticSearch extends SequentialCommandGroup {
/**
* Creates a new GalacticSearch.
*/
public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) {
// addCommands(
// new IdentifyPath(m_limeLight),
// new ExecuteCommand(m_limeLight, paths)
// );
if (m_limeLight.galacticSearchPath == "A_RED")
{
addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "A_BLUE")
{
addCommands(new ParallelCommandGroup(paths[1], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "B_RED")
{
addCommands(new ParallelCommandGroup(paths[2], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "B_BLUE")
{
addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake)));
}
else if (m_limeLight.galacticSearchPath == "test")
{
addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
}
}
}
src/main/java/frc4388/robot/commands/auto/IdentifyPath.java
-163
View File
@@ -1,163 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import javax.lang.model.util.ElementScanner6;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.LimeLight;
public class IdentifyPath extends CommandBase {
LimeLight m_limeLight;
double xAngle;
double yAngle;
double target;
public String path;
boolean closeVisible;
boolean finished;
boolean pathFound;
public IdentifyPath(LimeLight limeLight) {
m_limeLight = limeLight;
addRequirements(m_limeLight);
m_limeLight.limeOff();
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
path = "";
closeVisible = false;
pathFound = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
target = m_limeLight.getV();
xAngle = m_limeLight.getX();
yAngle = m_limeLight.getY();
//m_limeLight.limeOn();
// //Identify which of four paths
// m_limeLight.changePipeline(1);//Dual Targetting Lowest
// if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
// {
// closeVisible = true;
// }
// else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS
// {
// closeVisible = false;
// }
// if (closeVisible)
// {
// m_limeLight.changePipeline(2); //Dual Targetting Highest
// if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
// {
// path = "A_BLUE";
// }
// if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
// {
// path = "B_BLUE";
// }
// }
// else{
// m_limeLight.changePipeline(1); //Dual Targetting Lowest
// if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
// {
// path = "A_RED";
// }
// else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
// {
// path = "B_RED";
// }
// }
SmartDashboard.putBoolean("PathFound", pathFound);
// System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch");
// System.out.println(path);
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.aBlue))
{
pathFound = true;
path = "A_BLUE";
}
m_limeLight.changePipeline(2);
if (withinCoords(VisionConstants.bBlue))
{
pathFound = true;
path = "B_BLUE";
}
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.aRed))
{
pathFound = true;
path = "A_RED";
}
m_limeLight.changePipeline(1);
if (withinCoords(VisionConstants.bRed))
{
pathFound = true;
path = "B_RED";
}
path = "test";
}
public boolean withinError(double angle, double goal)
{
if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
{
return true;
}
else
{
return false;
}
}
public boolean withinCoords(double[] coords)
{
if (withinError(xAngle, coords[0]) && withinError(yAngle, coords[1]))
{
return true;
}
else
{
return false;
}
}
// 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 (path != "")
{
SmartDashboard.putString("GalacticSearchPath", path);
m_limeLight.galacticSearchPath = path;
//m_limeLight.limeOff();
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/auto/RunPath.java
-27
View File
@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// 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 RunPath extends SequentialCommandGroup {
/**
* Creates a new RunPath.
*/
public RunPath(RamseteCommand path) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
path
);
}
}
src/main/java/frc4388/robot/commands/auto/SequentialTest.java
-33
View File
@@ -1,33 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// 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 SequentialTest extends SequentialCommandGroup {
/**
* Creates a new SequentialTest.
*/
public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())),
paths[1]
);
}
}
src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
-35
View File
@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import java.nio.file.Path;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
// 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 SixBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new SixBallAutoMiddle.
*/
public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/Slalom.java
-28
View File
@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// 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 Slalom extends SequentialCommandGroup {
/**
* Creates a new Slalom.
*/
public Slalom(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java
-67
View File
@@ -1,67 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
public class TankDriveVelocity extends CommandBase {
Drive m_drive;
double m_leftTargetVel;
double m_rightTargetVel;
long m_targetTime;
long m_firstTime;
long m_currentTime;
long m_diffTime;
/**
* Creates a new TankDriveVelocity.
*/
public TankDriveVelocity(Drive subsystem, double leftTargetVel, double rightTargetVel, double targetTime) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_leftTargetVel = leftTargetVel;
m_rightTargetVel = rightTargetVel;
m_targetTime = (long) (targetTime * 1000);
addRequirements(subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_firstTime = System.currentTimeMillis();
m_diffTime = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentTime = System.currentTimeMillis();
m_diffTime = m_currentTime - m_firstTime;
if (m_diffTime < m_targetTime) {
m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
}
}
// 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_diffTime >= m_targetTime) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java
-59
View File
@@ -1,59 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.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:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class TenBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new TenBallAutoMiddle.
*/
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(
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]
);
}
}
src/main/java/frc4388/robot/commands/auto/Wait.java
-71
View File
@@ -1,71 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.auto;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Wait extends CommandBase {
long m_startTime;
long m_waitTime;
long m_currentTime;
SubsystemBase m_subsystem;
int m_waitNum;
int counter = 0;
/**
* Creates a new WaitCommand.
*/
public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
// Use addRequirements() here to declare subsystem dependencies.
m_waitTime = (long) (seconds * 1000);
m_subsystem = subsystem;
m_waitNum = waitNum;
addRequirements(m_subsystem);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currentTime = System.currentTimeMillis();
m_startTime = m_currentTime;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
//SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
}
m_currentTime = System.currentTimeMillis();
//SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
counter ++;
}
// 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_currentTime - m_startTime) >= m_waitTime) {
return true;
} else {
return false;
}
}
}
src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java
-48
View File
@@ -1,48 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
public class DisengageRatchet extends CommandBase {
Climber m_climber;
/**
* Creates a new DisengageRatchet command.
*/
public DisengageRatchet(Climber subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_climber = subsystem;
}
// 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() {
if (m_climber.m_climberSafety) {
m_climber.shiftServo(false);
System.err.println("Disengage Rachet");
}
}
// 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() {
return true;
}
}
src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java
-65
View File
@@ -1,65 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Climber;
import frc4388.utility.controller.IHandController;
public class RunClimberWithTriggers extends CommandBase {
private Climber m_climber;
private IHandController m_controller;
/**
* Uses input from opperator triggers to control climber motor
* @param subsystem the climber subsystem
* @param controller the driver controller
*/
public RunClimberWithTriggers(Climber subsystem, IHandController controller) {
m_climber = subsystem;
m_controller = controller;
addRequirements(m_climber);
}
// 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() {
double rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if (rightTrigger < .5) {
if(rightTrigger > leftTrigger) {
output = rightTrigger;
}
if (leftTrigger > rightTrigger) {
output = -leftTrigger;
m_climber.shiftServo(true);
System.err.println("Engage Rachet");
}
} else {
output = rightTrigger;
}
m_climber.runClimber(output);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java
-53
View File
@@ -1,53 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.climber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Leveler;
import frc4388.utility.controller.IHandController;
public class RunLevelerWithJoystick extends CommandBase {
private Leveler m_leveler;
private IHandController m_controller;
/**
* Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunLevelerWithJoystick(Leveler subsystem, IHandController controller) {
m_leveler = subsystem;
m_controller = controller;
addRequirements(m_leveler);
}
// 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() {
double input = m_controller.getRightXAxis();
m_leveler.runLeveler(input);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java
-87
View File
@@ -1,87 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.auto.Wait;
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 GotoCoordinatesRobotRelative extends SequentialCommandGroup {
Drive m_drive;
Pneumatics m_pneumatics;
double m_xTarget;
double m_yTarget;
double m_currentAngle;
double m_hypotDist;
double m_endAngle;
/**
* Creates a new GotoPosition.
*/
public GotoCoordinatesRobotRelative(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;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = endAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
}
public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {
m_drive = subsystem;
m_pneumatics = subsystem2;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = m_currentAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
public boolean isQuadrantThree() {
if ((m_xTarget < 0) && (m_yTarget < 0)) {
return true;
} else {
return false;
}
}
public double calcAngle() {
if (isQuadrantThree()) {
return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
} else {
return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90;
}
}
}
src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java
+2 -3
View File
@@ -9,7 +9,6 @@ package frc4388.robot.commands.drive;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Shooter;
public class PlaySongDrive extends CommandBase { public class PlaySongDrive extends CommandBase {
private Drive m_drive; private Drive m_drive;
@@ -17,10 +16,10 @@ public class PlaySongDrive extends CommandBase {
/** /**
* Creates a new PlaySongDrive. * Creates a new PlaySongDrive.
*/ */
public PlaySongDrive(Drive subsystem, Shooter shooter) { public PlaySongDrive(Drive subsystem) {
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem; m_drive = subsystem;
addRequirements(m_drive, shooter); addRequirements(m_drive);
} }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java
-36
View File
@@ -1,36 +0,0 @@
package frc4388.robot.commands.drive;
import org.opencv.core.Mat;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.ShooterAim;
public class SetShooterToOdo extends CommandBase {
private ShooterAim m_shooterAim;
private Drive m_driveTrain;
private double targetAngle;
public SetShooterToOdo(ShooterAim shooterAim, Drive driveTrain) {
m_shooterAim = shooterAim;
m_driveTrain = driveTrain;
}
@Override
public void initialize() {
double xPos = m_driveTrain.getPose().getX();
double yPos = m_driveTrain.getPose().getY();
targetAngle = Math.atan(yPos / xPos) - m_driveTrain.getPose().getRotation().getDegrees();
}
@Override
public void execute() {
m_shooterAim.runshooterRotatePID(targetAngle);
}
@Override
public boolean isFinished() {
return Math.abs(m_shooterAim.getShooterRotatePosition() - targetAngle) < 5;
}
}
src/main/java/frc4388/robot/commands/drive/SkipSong.java
-56
View File
@@ -1,56 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Drive;
public class SkipSong extends CommandBase {
Drive m_drive;
int m_index;
/**
* Creates a new SkipSong.
*/
public SkipSong(Drive m_robotDrive, int index) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = m_robotDrive;
m_index = index;
}
// 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() {
String[] songs = m_drive.songsStrings;
String song = m_drive.m_currentSong;
for (int i = 0; i < songs.length; i++) {
if (songs[i] == song) {
m_drive.selectSong(songs[i + m_index]);
break;
}
}
}
// 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() {
return true;
}
}
src/main/java/frc4388/robot/commands/drive/TurnDegrees.java
-74
View File
@@ -1,74 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
public class TurnDegrees extends CommandBase {
double m_targetAngle;
Drive m_drive;
double m_currentYawInTicks;
double m_targetAngleTicksIn;
double m_targetAngleTicksOut;
int i;
/**
* Creates a new TurnDeg.
*/
public TurnDegrees(Drive subsystem, double targetAngle) {
// Use addRequirements() here to declare subsystem dependencies.
m_targetAngle = targetAngle;
m_drive = subsystem;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks);
m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
i = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
m_drive.runTurningPID(m_targetAngleTicksIn);
SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
i++;
}
// 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 ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java
+12 -7
View File
@@ -13,6 +13,8 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.Num;
@@ -22,13 +24,12 @@ import edu.wpi.first.wpiutil.math.numbers.N6;
import frc4388.robot.Constants.VOPConstants; import frc4388.robot.Constants.VOPConstants;
import frc4388.robot.Constants.VisionConstants; import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight; // import frc4388.robot.subsystems.ShooterAim_1;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
public class VisionUpdateOdometry extends CommandBase { public class VisionUpdateOdometry extends CommandBase {
private Vision m_limeLight; private Vision m_limeLight;
private ShooterAim m_shooterAim; // private ShooterAim_1 m_shooterAim;
private Drive m_driveTrain; private Drive m_driveTrain;
private double xPos; private double xPos;
@@ -43,11 +44,11 @@ public class VisionUpdateOdometry extends CommandBase {
* @param shooterAim replace with Turret subsystem for integration with 2022 * @param shooterAim replace with Turret subsystem for integration with 2022
* @param driveTrain replace with Swerve subsystem for integration with 2022 * @param driveTrain replace with Swerve subsystem for integration with 2022
*/ */
public VisionUpdateOdometry(Vision limeLight, ShooterAim shooterAim, Drive driveTrain) { public VisionUpdateOdometry(Vision limeLight, Drive driveTrain) {
m_limeLight = limeLight; m_limeLight = limeLight;
m_shooterAim = shooterAim; // m_shooterAim = shooterAim;
m_driveTrain = driveTrain; m_driveTrain = driveTrain;
addRequirements(m_limeLight, m_shooterAim, m_driveTrain); addRequirements(m_limeLight, m_driveTrain);
// // Turn camera on but leave LEDs off // // Turn camera on but leave LEDs off
// NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); // NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
@@ -86,10 +87,11 @@ public class VisionUpdateOdometry extends CommandBase {
double distance = 1.d / Math.tan(viewAngle); double distance = 1.d / Math.tan(viewAngle);
distance *= VOPConstants.TARGET_HEIGHT; // replace with VisionConstants for 2022 distance *= VOPConstants.TARGET_HEIGHT; // replace with VisionConstants for 2022
System.out.println("Never gonna give you up: " + distance);
double[] ypr = new double[3]; double[] ypr = new double[3];
Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022 Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022
double relativeAngle = Math.toDegrees(m_shooterAim.getShooterAngleDegrees() - ypr[0]); double relativeAngle = Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]);
rotation = new Rotation2d(ypr[0]); rotation = new Rotation2d(ypr[0]);
xPos = Math.cos(relativeAngle) * distance; xPos = Math.cos(relativeAngle) * distance;
@@ -98,6 +100,9 @@ public class VisionUpdateOdometry extends CommandBase {
Pose2d pose = new Pose2d(translate, rotation); Pose2d pose = new Pose2d(translate, rotation);
m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter
SmartDashboard.putNumber("x:", pose.getX());
SmartDashboard.putNumber("y:", pose.getY());
m_limeLight.setLEDs(false);
} }
// http://www.lee-mac.com/5pointellipse.html // http://www.lee-mac.com/5pointellipse.html
src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java
-77
View File
@@ -1,77 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.intake;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.Intake;
public class RunExtenderOutIn extends CommandBase {
private Intake m_intake;
CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_extenderForwardLimit;
CANDigitalInput m_extenderReverseLimit;
/**
* Uses input from opperator to run the extender motor.
* The left bumper will run the extender in and out.
* @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunExtenderOutIn(Intake subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
addRequirements(m_intake);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_intake.isExtended = !m_intake.isExtended;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_intake.isExtended){
m_intake.runExtender(0.3);
} else {
m_intake.runExtender(-0.3);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_intake.runExtender(0.0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_intake.isExtended && m_extenderForwardLimit.get() == true){
return true;
}
else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){
return true;
}
else{
return false;
}
}
}
src/main/java/frc4388/robot/commands/intake/RunIntake.java
-46
View File
@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.intake;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.Intake;
public class RunIntake extends CommandBase {
Intake m_intake;
/**
* Creates a new RunIntake.
*/
public RunIntake(Intake subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
addRequirements(m_intake);
}
// 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() {
m_intake.runIntake(IntakeConstants.INTAKE_SPEED);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java
-63
View File
@@ -1,63 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.intake;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.IHandController;
public class RunIntakeWithTriggers extends CommandBase {
private Intake m_intake;
private IHandController m_controller;
/**
* Uses input from opperator triggers to control intake motor.
* The right trigger will run the intake in and the left trigger will run it out.
* @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunIntakeWithTriggers(Intake subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_intake = subsystem;
m_controller = controller;
addRequirements(m_intake);
}
// 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() {
double rightTrigger = m_controller.getRightTriggerAxis();
double leftTrigger = m_controller.getLeftTriggerAxis();
double output = 0;
if(leftTrigger >= rightTrigger) {
output = leftTrigger;
}
if (rightTrigger > leftTrigger) {
output = -rightTrigger;
}
m_intake.runIntake(output);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
-69
View File
@@ -1,69 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
/**
* Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
* @param shootSub The Shooter subsystem
* @param aimSub The ShooterAim subsystem
*/
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) {
m_shooter = shootSub;
m_shooterAim = aimSub;
m_shooterHood = hoodSub;
addRequirements(m_shooter, m_shooterHood, m_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() {
m_shooterHood.m_angleEncoder.setPosition(0);
m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooterAim.m_shooterRotateEncoder.setPosition(0);
m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
m_shooterHood.m_hoodDownLimit.get()) {
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java
-59
View File
@@ -1,59 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.Constants.ShooterConstants;
import frc4388.robot.subsystems.ShooterHood;
public class HoodPositionPID extends CommandBase {
ShooterHood m_shooterHood;
double firingAngle;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(ShooterHood subSystem) {
m_shooterHood = subSystem;
addRequirements(m_shooterHood);
}
// 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() {
//double slope = ShooterConstants.HOOD_CONVERT_SLOPE;
//double b = ShooterConstants.HOOD_CONVERT_B;
//firingAngle = (-slope*m_shooterHood.addFireAngle())+b;
firingAngle = m_shooterHood.addFireAngle();
//SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
//SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooterHood.runAngleAdjustPID(firingAngle);
}
// 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() {
double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
m_shooterHood.m_isHoodReady = true;
} else {
m_shooterHood.m_isHoodReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/PrepChecker.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
}
}
src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java
-54
View File
@@ -1,54 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.Leveler;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class RunHoodWithJoystick extends CommandBase {
private ShooterHood m_hood;
private IHandController m_controller;
/**
* Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller.
* @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) {
m_hood = subsystem;
m_controller = controller;
addRequirements(m_hood);
}
// 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() {
double input = m_controller.getRightYAxis();
m_hood.runHood(input);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java
-34
View File
@@ -1,34 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.ParallelDeadlineGroup;
import frc4388.robot.commands.storage.StoragePrep;
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:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class ShootPrepGroup extends ParallelDeadlineGroup {
/**
* Prepares the Shooter to be fired
* @param m_shooter The Shooter subsytem
* @param m_shooterAim The ShooterAim subsystem
* @param m_storage The Storage subsytem
*/
public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) {
super(
new TrackTarget(m_shooterAim),
new ShooterVelocityControlPID(m_shooter),
new HoodPositionPID(m_shooterHood)
);
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
import frc4388.robot.subsystems.ShooterHood;
public class ShooterGoalPosition extends CommandBase {
Shooter m_shooter;
ShooterHood m_hood;
ShooterAim m_aim;
/**
* Creates a new ShooterGoalPosition.
*/
public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
m_shooter = shooterSub;
m_hood = hoodSub;
m_aim = aimSub;
addRequirements(m_shooter,m_hood,m_aim);
}
// 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() {
m_shooter.runDrumShooterVelocityPID(5000);
m_hood.runAngleAdjustPID(4);
m_aim.runshooterRotatePID(-26.5);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterManual.java
-45
View File
@@ -1,45 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
public class ShooterManual extends CommandBase {
public boolean isManual = false;
/**
* Creates a new ShooterManual.
*/
public ShooterManual(boolean man) {
isManual = man;
}
// 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() {
RobotContainer.m_isShooterManual = isManual;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
RobotContainer.m_isShooterManual = isManual;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
import frc4388.robot.subsystems.ShooterHood;
public class ShooterTrenchPosition extends CommandBase {
Shooter m_shooter;
ShooterHood m_hood;
ShooterAim m_aim;
/**
* Creates a new ShooterTrenchPosition.
*/
public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
m_shooter = shooterSub;
m_hood = hoodSub;
m_aim = aimSub;
addRequirements(m_shooter,m_hood,m_aim);
}
// 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() {
m_shooter.runDrumShooterVelocityPID(5500);
m_hood.runAngleAdjustPID(11);
//m_aim.runshooterRotatePID(-28);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
-61
View File
@@ -1,61 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
/**
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
*/
public ShooterVelocityControlPID(Shooter subsystem) {
m_shooter = subsystem;
addRequirements(m_shooter);
}
// 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() {
//Tells whether the target velocity has been reached
m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition();
m_targetVel = m_shooter.addFireVel();
double error = m_actualVel - m_targetVel;
if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){
m_shooter.m_isDrumReady = true;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
else{
m_shooter.m_isDrumReady = false;
m_shooter.runDrumShooterVelocityPID(m_targetVel);
}
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
-138
View File
@@ -1,138 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class TrackTarget extends CommandBase {
// Setup
ShooterAim m_shooterAim;
Shooter m_shooter;
ShooterHood m_shooterHood;
NetworkTableEntry xEntry;
IHandController m_driverController;
// Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
double target = 0;
public double distance;
public double realDistance;
public static double fireVel;
public static double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
/**
* Uses the Limelight to track the target
* @param shooterSubsystem The Shooter subsystem
* @param aimSubsystem The ShooterAim subsystem
*/
public TrackTarget(ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = m_shooterAim.m_shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
// Vision Processing Mode
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
// Finding Distance
distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
realDistance = (1.09 * distance) - 12.8;
if (target == 1.0) { // If target in view
// Aiming Left/Right
xAngle = xAngle + m_shooter.getCenterDisplacement(realDistance);
turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
turnAmount = 0;
} // Angle Error Zone
// Deadzones
else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE;
} else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE;
}
m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
//m_shooterAim.runshooterRotatePID(targetAngle);
SmartDashboard.putNumber("Distance to Target", realDistance);
SmartDashboard.putNumber("Center Displacement", m_shooter.getCenterDisplacement(realDistance));
//START Equation Code
double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
double xVel = (distance * VisionConstants.GRAV) / (yVel);
//fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
//fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
//END Equation Code
//START CSV Code
fireVel = m_shooter.getVelocity(realDistance);
fireAngle = m_shooter.getHood(realDistance); //Note: Ensure to follow because units are different
//fireAngle = 33;
//END CSV Code
//fireVel = SmartDashboard.getNumber("Velocity Target", 0);
//fireAngle = SmartDashboard.getNumber("Angle Target", 3);
m_shooter.m_fireVel = fireVel;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterAim.m_targetDistance = distance;
}
}
// 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 (xAngle < 0.5 && xAngle > -0.5 && target == 1)
{
m_shooterAim.m_isAimReady = true;
} else {
m_shooterAim.m_isAimReady = false;
}
return false;
}
}
src/main/java/frc4388/robot/commands/shooter/TrimShooter.java
-65
View File
@@ -1,65 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.Constants.OIConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.utility.controller.XboxController;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
public double turretTrim = 0;
public double hoodTrim = 0;
public Shooter m_shooter;
/**
* Trims the shooter based on the D-Pad inputs
* @param shootSub The Shooter subsytems
*/
public TrimShooter(Shooter shootSub) {
m_shooter = shootSub;
}
// 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() {
if(m_operatorXbox.getDPadTop()){
hoodTrim += 1;
}
else if(m_operatorXbox.getDPadBottom()){
hoodTrim -= 1;
}
else if(m_operatorXbox.getDPadRight()){
turretTrim += 1;
}
else if(m_operatorXbox.getDPadLeft()){
turretTrim -= 1;
}
m_shooter.shooterTrims.m_turretTrim = turretTrim;
m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/ManageStorage.java
-147
View File
@@ -1,147 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Storage.StorageMode;
public class ManageStorage extends CommandBase {
Storage m_storage;
/* Timer */
long m_resetStartTime;
/* Keeps track of which beam breaks are pressed */
boolean m_isBallInIntake = false;
boolean m_isBallInStorage = false;
boolean m_isBallInUseless = false;
boolean m_isBallInShooter = false;
/* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
boolean m_isStorageEmpty = true;
/**
* Creates a new ManageStorage.
*/
public ManageStorage(Storage m_robotStorage) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
m_isStorageEmpty = !m_isBallInStorage;
if (m_storage.m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
/// TODO: Delete/Comment these when done
SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storage.m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storage.m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storage.m_storageMode == StorageMode.RESET) {
runReset();
} else if (m_storage.m_storageMode == StorageMode.MANUAL) {
runManual();
}
}
/**
* Intakes a ball.
* Runs until the storage ball has moved past the
* storage sensor and the intake ball has taken its place.
*/
private void runIntake() {
if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty
m_isStorageEmpty = true;
}
if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode
m_isStorageEmpty = false;
m_storage.changeStorageMode(StorageMode.IDLE);
}
} else {
m_storage.changeStorageMode(StorageMode.IDLE);
}
}
/**
* Idles until a ball is in the intake.
* Also updates the status of the storage position
*/
private void runIdle() {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storage.changeStorageMode(StorageMode.INTAKE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Runs Storage out until the Intake Sensor is tripped.
* Then switches into intake mode. This effectively
* resets the position of the balls back to the bottom of the storage.
*/
private void runReset() {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storage.changeStorageMode(StorageMode.INTAKE);
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storage.changeStorageMode(StorageMode.IDLE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Switches Storage to Manual only
*/
private void runManual() {
m_storage.runStorage(0);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storage.changeStorageMode(StorageMode.RESET);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java
-143
View File
@@ -1,143 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Storage.StorageMode;
public class ManageStoragePID extends CommandBase {
Storage m_storage;
/* Timer */
long m_resetStartTime;
/* Start Positions */
double m_intakeStartPos;
/* Keeps track of which beam breaks are pressed */
boolean m_isBallInIntake = false;
boolean m_isBallInStorage = false;
boolean m_isBallInUseless = false;
boolean m_isBallInShooter = false;
/* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */
boolean m_isStorageEmpty = true;
/**
* Creates a new ManageStorage.
*/
public ManageStoragePID(Storage m_robotStorage) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = m_robotStorage;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
m_isStorageEmpty = !m_isBallInStorage;
m_intakeStartPos = m_storage.getEncoderPosInches();
if (m_storage.m_storageMode == StorageMode.RESET) {
m_resetStartTime = System.currentTimeMillis();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_isBallInIntake = !m_storage.getBeamIntake();
m_isBallInStorage = !m_storage.getBeamStorage();
m_isBallInUseless = !m_storage.getBeamUseless();
m_isBallInShooter = !m_storage.getBeamShooter();
/// TODO: Delete/Comment these when done
SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake);
SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage);
SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter);
if (m_storage.m_storageMode == StorageMode.IDLE) {
runIdle();
} else if (m_storage.m_storageMode == StorageMode.INTAKE) {
runIntake();
} else if (m_storage.m_storageMode == StorageMode.RESET) {
runReset();
}
}
/**
* Intakes a ball.
* Runs until the storage ball has moved past the
* storage sensor and the intake ball has taken its place.
*/
private void runIntake() {
if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter
m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL);
double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches();
if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) {
m_storage.changeStorageMode(StorageMode.IDLE);
}
} else {
m_storage.changeStorageMode(StorageMode.IDLE);
}
}
/**
* Idles until a ball is in the intake.
* Also updates the status of the storage position
*/
private void runIdle() {
m_storage.runStorage(0);
if (m_isBallInIntake) {
m_storage.changeStorageMode(StorageMode.INTAKE);
m_intakeStartPos = m_storage.getEncoderPosInches();
}
m_isStorageEmpty = !m_isBallInStorage;
}
/**
* Runs Storage out until the Intake Sensor is tripped.
* Then switches into intake mode. This effectively
* resets the position of the balls back to the bottom of the storage.
*/
private void runReset() {
m_storage.runStorage(-StorageConstants.STORAGE_SPEED);
if (m_isBallInIntake) {
m_storage.changeStorageMode(StorageMode.INTAKE);
m_intakeStartPos = m_storage.getEncoderPosInches();
} else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) {
m_storage.changeStorageMode(StorageMode.IDLE);
}
m_isStorageEmpty = !m_isBallInStorage;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_storage.changeStorageMode(StorageMode.RESET);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/RunStorage.java
-46
View File
@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class RunStorage extends CommandBase {
Storage m_storage;
/**
* Creates a new RunStorage.
*/
public RunStorage(Storage subsystem) {
// Use addRequirements() here to declare subsystem dependencies.
m_storage = subsystem;
addRequirements(m_storage);
}
// 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() {
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/commands/storage/StoragePrep.java
-54
View File
@@ -1,54 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.storage;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.subsystems.Storage;
public class StoragePrep extends CommandBase {
Storage m_storage;
double m_startTime;
/**
* Prepares the Storage for aiming
* @param storeSub The Storage subsystem
*/
public StoragePrep(Storage storeSub) {
m_storage = storeSub;
addRequirements(m_storage);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_startTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) {
m_storage.runStorage(StorageConstants.STORAGE_SPEED);
m_storage.m_isStorageReadyToFire = false;
} else {
m_storage.runStorage(0);
m_storage.m_isStorageReadyToFire = true;
}
}
// 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() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
-93
View File
@@ -1,93 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
Servo m_servo;
//Spark m_spark = new Spark(4);
public boolean m_climberSafety = false;
public boolean m_isRatchetEngaged = true;
/**
* Creates a new Climber.
*/
public Climber() {
m_servo = new Servo(4);
m_climberMotor.restoreFactoryDefaults();
m_climberMotor.setIdleMode(IdleMode.kBrake);
m_climberMotor.setInverted(true);
m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_climberForwardLimit.enableLimitSwitch(true);
m_climberReverseLimit.enableLimitSwitch(true);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putBoolean("Climber Safety", m_climberSafety);
SmartDashboard.putBoolean("Rachet", m_isRatchetEngaged);
}
/**
* Runs climber motor
* @param input the voltage to run motor at
*/
public void runClimber(double input) {
if(m_climberSafety){
input *= 1.0;
m_climberMotor.set(input);
}
else{
m_climberMotor.set(0);
}
}
/* Safety Button for Climber */
public void setSafetyPressed()
{
m_climberSafety = true;
}
/* Safety Button for Climber set back to false */
public void setSafetyNotPressed()
{
m_climberSafety = false;
}
/**
* @param shift true to engage ratchet, false to disengage
*/
public void shiftServo(boolean shift) {
if (shift) {
m_servo.setPosition(0.48);
} else {
m_servo.setPosition(0.56);
}
m_isRatchetEngaged = shift;
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+4 -4
View File
@@ -55,7 +55,7 @@ public class Drive extends SubsystemBase {
/* Pneumatics Subsystem */ /* Pneumatics Subsystem */
public Pneumatics m_pneumaticsSubsystem; public Pneumatics m_pneumaticsSubsystem;
Shooter m_shooter; // Shooter_1 m_shooter;
/* Low Gear Gains */ /* Low Gear Gains */
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
@@ -310,10 +310,10 @@ public class Drive extends SubsystemBase {
* *
* @param subsystem Subsystem needed. * @param subsystem Subsystem needed.
*/ */
public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) { public void passRequiredSubsystem(Pneumatics subsystem) {
m_pneumaticsSubsystem = subsystem; m_pneumaticsSubsystem = subsystem;
m_shooter = shooter; // m_shooter = shooter;
m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); // m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
} }
public void updateTime() { public void updateTime() {
src/main/java/frc4388/robot/subsystems/Intake.java
-86
View File
@@ -1,86 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
public class Intake extends SubsystemBase {
CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_extenderForwardLimit;
CANDigitalInput m_extenderReverseLimit;
public boolean isExtended = false;
/**
* Creates a new Intake.
*/
public Intake() {
m_intakeMotor.restoreFactoryDefaults();
m_extenderMotor.restoreFactoryDefaults();
m_intakeMotor.setIdleMode(IdleMode.kCoast);
m_extenderMotor.setIdleMode(IdleMode.kBrake);
m_intakeMotor.setInverted(false);
m_extenderMotor.setInverted(true);
m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
m_extenderForwardLimit.enableLimitSwitch(true);
m_extenderReverseLimit.enableLimitSwitch(true);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Runs intake motor
* @param input the percent output to run motor at
*/
public void runIntake(double input) {
m_intakeMotor.set(input);
}
public void runExtender(double input){
m_extenderMotor.set(input);
}
/**
* Runs extender motor
* @param input the percent output to run motor at
*/
/*public void runExtender(double input) {
if (m_extenderForwardLimit.get()) {
isExtended = true;
}
else if (m_extenderReverseLimit.get()) {
isExtended = false;
}
else{
m_extenderMotor.set(-input);
}
if (isExtended == false) {
m_extenderMotor.set(input);
}
if (isExtended == true) {
m_extenderMotor.set(-input);
}
}*/
}
src/main/java/frc4388/robot/subsystems/Leveler.java
-57
View File
@@ -1,57 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.LevelerConstants;
public class Leveler extends SubsystemBase {
CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
Climber m_climberSubsystem;
/**
* Creates a new Leveler.
*/
public Leveler() {
m_levelerMotor.restoreFactoryDefaults();
m_levelerMotor.setIdleMode(IdleMode.kBrake);
m_levelerMotor.setInverted(false);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Runs intake motor
* @param input the percent output to run motor at
*/
public void runLeveler(double input) {
if(m_climberSubsystem.m_climberSafety){
m_levelerMotor.set(input);
}
else{
m_levelerMotor.set(0);
}
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Climber subsystem) {
m_climberSubsystem = subsystem;
}
}
src/main/java/frc4388/robot/subsystems/LimeLight.java
-157
View File
@@ -1,157 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
public class LimeLight extends SubsystemBase {
/**
* Creates a new LimeLight.
*/
public String galacticSearchPath;
public LimeLight() {
}
public void limeOff(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
}
public void changePipeline(int pipelineId)
{
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
}
public double getV()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
}
public double getX()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
}
public double getY()
{
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
}
// for VOP
public double[] getVArray()
{
double[] defaultValue = new double[0];
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDoubleArray(defaultValue);
}
public double[] getXArray()
{
double[] defaultValue = new double[0];
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDoubleArray(defaultValue);
}
public double[] getYArray()
{
double[] defaultValue = new double[0];
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDoubleArray(defaultValue);
}
// end VOP
int i = 0;
boolean onceThrough = false;
boolean pathFound = false;
public void identifyPath(){
if (!onceThrough)
{
changePipeline(2);
if (withinCoords(VisionConstants.aBlue))
{
pathFound = true;
galacticSearchPath = "A_BLUE";
}
changePipeline(4);
if (withinCoords(VisionConstants.bBlue))
{
pathFound = true;
galacticSearchPath = "B_BLUE";
}
changePipeline(1);
if (withinCoords(VisionConstants.aRed))
{
pathFound = true;
galacticSearchPath = "A_RED";
}
changePipeline(3);
if (withinCoords(VisionConstants.bRed))
{
pathFound = true;
galacticSearchPath = "B_RED";
}
if (pathFound == false)
{
galacticSearchPath = "PathNotFound";
}
SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
onceThrough = true;
}
else{
i++;
SmartDashboard.putNumber("Counter", i);
}
if (i >= 50)
{
i=0;
pathFound = false;
onceThrough = false;
}
}
public boolean withinError(double angle, double goal)
{
if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
{
return true;
}
else
{
return false;
}
}
public boolean withinCoords(double[] coords)
{
if (withinError(getX(), coords[0]) && withinError(getY(), coords[1]))
{
return true;
}
else
{
return false;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
-200
View File
@@ -1,200 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import java.io.File;
import java.io.IOException;
import java.util.Comparator;
import java.util.Map;
import java.util.Optional;
import java.util.function.Function;
import java.util.regex.Pattern;
import java.util.stream.IntStream;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Robot;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
import frc4388.utility.Trims;
public class Shooter extends SubsystemBase {
public final WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
public final WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
public static class ShooterTableEntry {
public Double distance, hoodExt, drumVelocity, centerDisplacement;
}
private ShooterTableEntry[][] m_shooterTables;
public ShooterTableEntry[] getActiveShooterTable() {
return m_shooterTables[Constants.Mode.get().ordinal()];
}
public boolean m_isDrumReady = false;
public double m_fireVel;
public Trims shooterTrims;
public ShooterHood m_shooterHoodSubsystem;
public ShooterAim m_shooterAimSubsystem;
/*
* Creates a new Shooter subsystem, with the drum shooter and the angle adjuster.
*/
public Shooter() {
// Testing purposes reseting gyros
// resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
// SmartDashboard.putNumber("Velocity Target", 10000);
// SmartDashboard.putNumber("Angle Target", 3);
// LEFT FALCON
m_shooterFalconLeft.configFactoryDefault();
m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
m_shooterFalconLeft.setInverted(true);
m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
// RIGHT FALCON
m_shooterFalconRight.configFactoryDefault();
m_shooterFalconRight.setNeutralMode(NeutralMode.Coast);
m_shooterFalconRight.setInverted(false);
m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
final int closedLoopTimeMs = 1;
// LEFT FALCON
m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
// RIGHT FALCON
// m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
try {
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
@Override
protected String headerSanitizer(final String header) {
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
}
};
m_shooterTables = new ShooterTableEntry[][] {
csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()),
csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances Casual.csv").toPath())
};
new Thread(() -> System.out.println(CSV.ReflectionTable.create(getActiveShooterTable(), RobotBase.isSimulation()))).start();
} catch (final IOException e) {
throw new RuntimeException(e);
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
try {
SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature());
SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
} catch (final Exception e) {
}
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) {
m_shooterHoodSubsystem = subsystem0;
m_shooterAimSubsystem = subsystem1;
}
public double addFireVel() {
return m_fireVel;
}
/**
* Runs drum shooter motor.
* @param speed Speed to set the motor at
*/
public void runDrumShooter(double speed) {
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
m_shooterFalconRight.follow(m_shooterFalconLeft);
// m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed);
}
/**
* Configures gains for shooter PID.
*/
public void setShooterGains() {
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* Runs drum shooter velocity PID.
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel) {
// System.out.println("Target Velocity" + targetVel);
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init PID
m_shooterFalconRight.follow(m_shooterFalconLeft);
}
public Double getCenterDisplacement(final Double distance) {
return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.centerDisplacement).doubleValue();
}
public Double getVelocity(final Double distance) {
return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
public Double getHood(final Double distance) {
return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
final E closestRecord = closestEntry.getValue();
final int closestRecordIndex = closestEntry.getKey();
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
}
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
}
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
-177
View File
@@ -1,177 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
public class ShooterAim extends SubsystemBase {
public Shooter m_shooterSubsystem;
public Drive m_driveSubsystem;
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
public GyroBase m_turretGyro;
public double m_targetDistance = 0;
public boolean m_isAimReady = false;
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a subsytem for the turret aiming
*/
public ShooterAim() {
//resetGyroShooterRotate();
m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
m_turretGyro = getGyroInterface();
m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_shooterRightLimit.enableLimitSwitch(true);
m_shooterLeftLimit.enableLimitSwitch(true);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT);
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
m_shooterRotateMotor.setInverted(false);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition());
SmartDashboard.putData("Turret Angle", m_turretGyro);
SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady);
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Shooter subsystem0, Drive subsystem1) {
m_shooterSubsystem = subsystem0;
m_driveSubsystem = subsystem1;
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
/* Rotate Shooter PID Control */
public void runshooterRotatePID(double targetAngle)
{
// Set PID Coefficients
m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput);
// Convert input angle in degrees to rotations of the motor
targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
public double getShooterRotatePosition()
{
return m_shooterRotateEncoder.getPosition();
}
public double getShooterAngleDegrees() {
return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
}
/**
* Gets the angle of the Shooter relative to the target.
*/
public double getTargetAngleDegrees() {
return m_driveSubsystem.getHeading() + getShooterAngleDegrees();
}
public double getTargetXDisplacement() {
return m_targetDistance * Math.cos(getTargetAngleDegrees());
}
public double getTargetYDisplacement() {
return m_targetDistance * Math.sin(getTargetAngleDegrees());
}
/**
* Gets the angle of the Shooter relative to the inner port.
* Use for tuning the Shooter Displacement
*/
public double getInnerPortAngleDegrees() {
return Math.atan( getTargetYDisplacement() / (getTargetXDisplacement() + 29.25) );
}
public GyroBase getGyroInterface() {
return new GyroBase(){
@Override
public void close() throws Exception {
// TODO Auto-generated method stub
}
@Override
public void reset() {
// TODO Auto-generated method stub
}
@Override
public double getRate() {
// TODO Auto-generated method stub
return 0;
}
@Override
public double getAngle() {
// TODO Auto-generated method stub
return getShooterAngleDegrees();
}
@Override
public void calibrate() {
// TODO Auto-generated method stub
}
};
}
}
src/main/java/frc4388/robot/subsystems/ShooterHood.java
-107
View File
@@ -1,107 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
public class ShooterHood extends SubsystemBase {
public Shooter m_shooterSubsystem;
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public boolean m_isHoodReady = false;
public double m_fireAngle;
public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
/**
* Creates a new ShooterHood.
*/
public ShooterHood() {
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
//m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
//m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle);
SmartDashboard.putNumber("Hood Angle Raw", getAnglePositionDegrees());
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Shooter subsystem) {
m_shooterSubsystem = subsystem;
}
public double addFireAngle() {
return m_fireAngle;
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double targetAngle)
{
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
public void runHood(double input)
{
m_angleAdjustMotor.set(input);
}
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
return m_angleEncoder.getPosition();
}
public double getAnglePositionDegrees(){
return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
-144
View File
@@ -1,144 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.subsystems;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.StorageConstants;
import frc4388.utility.Gains;
public class Storage extends SubsystemBase {
public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS);
private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE);
private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
CANPIDController m_storagePIDController = m_storageMotor.getPIDController();
CANEncoder m_encoder = m_storageMotor.getEncoder();
Gains storageGains = StorageConstants.STORAGE_GAINS;
Intake m_intake;
public boolean m_isStorageReadyToFire = false;
public enum StorageMode{IDLE, INTAKE, RESET, MANUAL};
public StorageMode m_storageMode = StorageMode.IDLE;
/**
* Creates a new Storage.
*/
public Storage() {
resetEncoder();
// Set PID Coefficients
m_storagePIDController.setP(storageGains.m_kP);
m_storagePIDController.setI(storageGains.m_kI);
m_storagePIDController.setD(storageGains.m_kD);
m_storagePIDController.setIZone(storageGains.m_kIzone);
m_storagePIDController.setFF(storageGains.m_kF);
m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput);
}
@Override
public void periodic() {
SmartDashboard.putBoolean("Intake Beam", getBeamIntake());
SmartDashboard.putBoolean("Storage Beam", getBeamStorage());
SmartDashboard.putBoolean("Upper Beam", getBeamUseless());
SmartDashboard.putBoolean("Shooter Beam", getBeamShooter());
}
/**
* Runs storage motor
*
* @param input the percent output to run motor at
*/
public void runStorage(double input) {
m_storageMotor.set(input);
}
public void resetEncoder(){
m_encoder.setPosition(0);
}
/**
* Runs Storage to a particular position
* @param targetPos in inches
*/
public void runStoragePositionPID(double targetPos){
//SmartDashboard.putNumber("Storage Position PID Target", targetPos);
//SmartDashboard.putNumber("Storage Position Pos", getEncoderPos());
targetPos = InchesToMotorRots(targetPos);
m_storagePIDController.setReference(targetPos, ControlType.kPosition);
}
/**
* Runs Storage to a particular position
* @param position in motor rotations
*/
public void setStoragePID(double position){
m_storagePIDController.setReference(position, ControlType.kPosition);
}
public double getEncoderPos(){
return m_encoder.getPosition();
}
public double getEncoderPosInches(){
return motorRotsToInches(getEncoderPos());
}
public double getEncoderVel(){
return m_encoder.getVelocity();
}
/**
* @param motorRots
* @return inches
*/
public double motorRotsToInches(double motorRots) {
return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT);
}
/**
* @param inches
* @return motorRots
*/
public double InchesToMotorRots(double inches) {
return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT);
}
public boolean getBeamShooter(){
return m_beamShooter.get();
}
public boolean getBeamUseless(){
return m_beamUseless.get();
}
public boolean getBeamStorage(){
return m_beamStorage.get();
}
public boolean getBeamIntake(){
return m_beamIntake.get();
}
public void changeStorageMode(StorageMode storageMode){
m_storageMode = storageMode;
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+8 -26
View File
@@ -7,52 +7,34 @@
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Point; import org.opencv.core.Point;
import org.photonvision.PhotonCamera;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
public class Vision extends SubsystemBase { public class Vision extends SubsystemBase {
private PhotonCamera m_camera;
public Vision() { public Vision() {
m_camera = new PhotonCamera(VisionConstants.NAME); // TODO
} }
public ArrayList<Point> getTargetPoints() { public ArrayList<Point> getTargetPoints() {
PhotonPipelineResult result = m_camera.getLatestResult(); if(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) != 1)
if(!result.hasTargets())
return null; return null;
ArrayList<Point> points = new ArrayList<>(); ArrayList<Point> points = new ArrayList<>();
for(PhotonTrackedTarget target : result.getTargets()) { double[] corners = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornxy").getDoubleArray(new double[0]);
List<TargetCorner> corners = target.getCorners(); for(int i = 0; i < corners.length; i += 2*2) {
points.add(new Point(corners[i], corners[i+1]));
double centerY = 0;
for(TargetCorner corner : corners) {
centerY += corner.y;
}
centerY /= corners.size();
for(TargetCorner corner : corners) {
if(corner.y <= centerY)
points.add(new Point(corner.x, corner.y));
}
} }
return points; return points;
} }
public void setLEDs(boolean on) { public void setLEDs(boolean on) {
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(on ? 0 : 1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(on ? 3 : 1);
} }
} }