mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-08 16:28:01 -06:00
delete all the things
This commit is contained in:
aarav18
@@ -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"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
{
|
||||
"CameraPublisher": {
|
||||
"limelight": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
},
|
||||
"NetworkTables Settings": {
|
||||
"mode": "Client"
|
||||
},
|
||||
"limelight": {
|
||||
"open": true
|
||||
}
|
||||
}
|
||||
@@ -77,13 +77,13 @@ public class Robot extends TimedRobot {
|
||||
/* Builds Autos */
|
||||
m_robotContainer.buildAutos();
|
||||
SmartDashboard.putString("Is Auto Start?", "NAH");
|
||||
m_robotContainer.m_robotLime.limeOff();
|
||||
// m_robotContainer.m_robotLime.limeOff();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {
|
||||
m_robotContainer.resetOdometry(new Pose2d());
|
||||
m_robotContainer.idenPath();
|
||||
// m_robotContainer.idenPath();
|
||||
if (m_modeChooser.getSelected() != Mode.get())
|
||||
Mode.set(m_modeChooser.getSelected());
|
||||
}
|
||||
@@ -97,7 +97,7 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
// m_robotContainer.setDriveGearState(true);
|
||||
|
||||
m_initialTime = System.currentTimeMillis();
|
||||
|
||||
@@ -135,9 +135,9 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
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);
|
||||
|
||||
// This makes sure that the autonomous stops running when
|
||||
|
||||
@@ -30,50 +30,50 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Constants.Mode;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.commands.InterruptSubsystem;
|
||||
import frc4388.robot.commands.auto.Barrel;
|
||||
import frc4388.robot.commands.auto.BarrelMany;
|
||||
import frc4388.robot.commands.auto.BarrelStart;
|
||||
import frc4388.robot.commands.auto.Bounce;
|
||||
import frc4388.robot.commands.auto.DriveOffLineBackward;
|
||||
import frc4388.robot.commands.auto.DriveOffLineForward;
|
||||
import frc4388.robot.commands.auto.EightBallAutoMiddle;
|
||||
import frc4388.robot.commands.auto.FiveBallAutoMiddle;
|
||||
import frc4388.robot.commands.auto.GalacticSearch;
|
||||
import frc4388.robot.commands.auto.SequentialTest;
|
||||
import frc4388.robot.commands.auto.SixBallAutoMiddle;
|
||||
import frc4388.robot.commands.auto.Slalom;
|
||||
import frc4388.robot.commands.auto.TenBallAutoMiddle;
|
||||
import frc4388.robot.commands.climber.DisengageRatchet;
|
||||
import frc4388.robot.commands.climber.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.climber.RunLevelerWithJoystick;
|
||||
// import frc4388.robot.commands.InterruptSubsystem;
|
||||
// import frc4388.robot.commands.auto.Barrel;
|
||||
// import frc4388.robot.commands.auto.BarrelMany;
|
||||
// import frc4388.robot.commands.auto.BarrelStart;
|
||||
// import frc4388.robot.commands.auto.Bounce;
|
||||
// import frc4388.robot.commands.auto.DriveOffLineBackward;
|
||||
// import frc4388.robot.commands.auto.DriveOffLineForward;
|
||||
// import frc4388.robot.commands.auto.EightBallAutoMiddle;
|
||||
// import frc4388.robot.commands.auto.FiveBallAutoMiddle;
|
||||
// import frc4388.robot.commands.auto.GalacticSearch;
|
||||
// import frc4388.robot.commands.auto.SequentialTest;
|
||||
// import frc4388.robot.commands.auto.SixBallAutoMiddle;
|
||||
// import frc4388.robot.commands.auto.Slalom;
|
||||
// import frc4388.robot.commands.auto.TenBallAutoMiddle;
|
||||
// import frc4388.robot.commands.climber.DisengageRatchet;
|
||||
// import frc4388.robot.commands.climber.RunClimberWithTriggers;
|
||||
// import frc4388.robot.commands.climber.RunLevelerWithJoystick;
|
||||
import frc4388.robot.commands.drive.DriveWithJoystick;
|
||||
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.intake.RunIntakeWithTriggers;
|
||||
import frc4388.robot.commands.shooter.CalibrateShooter;
|
||||
import frc4388.robot.commands.shooter.RunHoodWithJoystick;
|
||||
import frc4388.robot.commands.shooter.ShootPrepGroup;
|
||||
import frc4388.robot.commands.shooter.ShooterManual;
|
||||
import frc4388.robot.commands.shooter.ShooterTrenchPosition;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
import frc4388.robot.commands.shooter.TrimShooter;
|
||||
import frc4388.robot.commands.storage.ManageStorage;
|
||||
// import frc4388.robot.commands.intake.RunIntakeWithTriggers;
|
||||
// import frc4388.robot.commands.shooter.CalibrateShooter;
|
||||
// import frc4388.robot.commands.shooter.RunHoodWithJoystick;
|
||||
// import frc4388.robot.commands.shooter.ShootPrepGroup;
|
||||
// import frc4388.robot.commands.shooter.ShooterManual;
|
||||
// import frc4388.robot.commands.shooter.ShooterTrenchPosition;
|
||||
// import frc4388.robot.commands.shooter.TrackTarget;
|
||||
// import frc4388.robot.commands.shooter.TrimShooter;
|
||||
// import frc4388.robot.commands.storage.ManageStorage;
|
||||
import frc4388.robot.subsystems.Camera;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
// import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
// import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Leveler;
|
||||
import frc4388.robot.subsystems.LimeLight;
|
||||
// import frc4388.robot.subsystems.Leveler;
|
||||
// import frc4388.robot.subsystems.LimeLight;
|
||||
import frc4388.robot.subsystems.Pneumatics;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.robot.subsystems.ShooterAim;
|
||||
import frc4388.robot.subsystems.ShooterHood;
|
||||
import frc4388.robot.subsystems.Storage;
|
||||
// import frc4388.robot.subsystems.Shooter_1;
|
||||
// import frc4388.robot.subsystems.ShooterAim_1;
|
||||
// import frc4388.robot.subsystems.ShooterHood_1;
|
||||
// import frc4388.robot.subsystems.Storage;
|
||||
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.IHandController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
@@ -87,20 +87,20 @@ import frc4388.utility.controller.XboxController;
|
||||
public class RobotContainer {
|
||||
/* Subsystems */
|
||||
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 Intake m_robotIntake = new Intake();
|
||||
private final Shooter m_robotShooter = new Shooter();
|
||||
private final ShooterAim m_robotShooterAim = new ShooterAim();
|
||||
private final ShooterHood m_robotShooterHood = new ShooterHood();
|
||||
private final Climber m_robotClimber = new Climber();
|
||||
private final Leveler m_robotLeveler = new Leveler();
|
||||
private final Storage m_robotStorage = new Storage();
|
||||
// private final Intake m_robotIntake = new Intake();
|
||||
// private final Shooter_1 m_robotShooter = new Shooter_1();
|
||||
// private final ShooterAim_1 m_robotShooterAim = new ShooterAim_1();
|
||||
// private final ShooterHood_1 m_robotShooterHood = new ShooterHood_1();
|
||||
// private final Climber m_robotClimber = new Climber();
|
||||
// private final Leveler m_robotLeveler = new Leveler();
|
||||
// private final Storage m_robotStorage = new Storage();
|
||||
|
||||
/* Cameras */
|
||||
private final Camera m_robotCameraFront = new Camera("front", 0, 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();
|
||||
|
||||
/* Controllers */
|
||||
@@ -111,22 +111,22 @@ public class RobotContainer {
|
||||
private static XboxController m_manualXbox = new XboxController(3);
|
||||
|
||||
/* Autos */
|
||||
double m_totalTimeAuto;
|
||||
SixBallAutoMiddle m_sixBallAutoMiddle;
|
||||
SixBallAutoMiddle m_sixBallAutoMiddle0;
|
||||
SixBallAutoMiddle m_sixBallAutoMiddle1;
|
||||
EightBallAutoMiddle m_eightBallAutoMiddle;
|
||||
DriveOffLineForward m_driveOffLineForward;
|
||||
DriveOffLineBackward m_driveOffLineBackward;
|
||||
FiveBallAutoMiddle m_fiveBallAutoMiddle;
|
||||
TenBallAutoMiddle m_tenBallAutoMiddle;
|
||||
Slalom m_slalom;
|
||||
Barrel m_barrel;
|
||||
BarrelStart m_barrelStart;
|
||||
BarrelMany m_barrelMany;
|
||||
Bounce m_bounce;
|
||||
SequentialTest m_sequentialTest;
|
||||
GalacticSearch m_galacticSearch;
|
||||
// double m_totalTimeAuto;
|
||||
// SixBallAutoMiddle m_sixBallAutoMiddle;
|
||||
// SixBallAutoMiddle m_sixBallAutoMiddle0;
|
||||
// SixBallAutoMiddle m_sixBallAutoMiddle1;
|
||||
// EightBallAutoMiddle m_eightBallAutoMiddle;
|
||||
// DriveOffLineForward m_driveOffLineForward;
|
||||
// DriveOffLineBackward m_driveOffLineBackward;
|
||||
// FiveBallAutoMiddle m_fiveBallAutoMiddle;
|
||||
// TenBallAutoMiddle m_tenBallAutoMiddle;
|
||||
// Slalom m_slalom;
|
||||
// Barrel m_barrel;
|
||||
// BarrelStart m_barrelStart;
|
||||
// BarrelMany m_barrelMany;
|
||||
// Bounce m_bounce;
|
||||
// SequentialTest m_sequentialTest;
|
||||
// // GalacticSearch m_galacticSearch;
|
||||
|
||||
public static boolean m_isShooterManual = false;
|
||||
|
||||
@@ -140,12 +140,12 @@ public class RobotContainer {
|
||||
public void setControls(Mode mode) {
|
||||
CommandScheduler.getInstance().clearButtons();
|
||||
/* Passing Drive and Pneumatics Subsystems */
|
||||
m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
|
||||
m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter);
|
||||
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
|
||||
m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
|
||||
m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
|
||||
m_robotLeveler.passRequiredSubsystem(m_robotClimber);
|
||||
// m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
|
||||
// m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter);
|
||||
// m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
|
||||
// m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
|
||||
// m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive);
|
||||
// m_robotLeveler.passRequiredSubsystem(m_robotClimber);
|
||||
|
||||
final double drumShooterTargetPIDVelocity;
|
||||
|
||||
@@ -156,9 +156,9 @@ public class RobotContainer {
|
||||
// buildAutos();
|
||||
/* Default Commands */
|
||||
// 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
|
||||
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController()));
|
||||
// m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController()));
|
||||
} else {
|
||||
drumShooterTargetPIDVelocity = 0;
|
||||
configureButtonBindingsCasual();
|
||||
@@ -166,21 +166,21 @@ public class RobotContainer {
|
||||
|
||||
/* Default Commands */
|
||||
// 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()));
|
||||
// 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
|
||||
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
|
||||
m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
|
||||
// m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
|
||||
// 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
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
// runs the storage not
|
||||
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
|
||||
m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime));
|
||||
// m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
|
||||
//m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -192,7 +192,7 @@ public class RobotContainer {
|
||||
private void configureButtonBindingsCompetitive() {
|
||||
/* Test Buttons */
|
||||
// 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
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
|
||||
// Y driver test button
|
||||
@@ -201,37 +201,37 @@ public class RobotContainer {
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand());
|
||||
/* Driver Buttons */
|
||||
// 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
|
||||
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
|
||||
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber));
|
||||
// new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber));
|
||||
|
||||
/* Operator Buttons */
|
||||
// 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,
|
||||
// m_robotStorage), false);
|
||||
// .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
|
||||
// .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
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood,
|
||||
// m_robotStorage), false);
|
||||
// .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
|
||||
// .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
|
||||
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));
|
||||
|
||||
// VOP system testing
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
// .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
|
||||
.whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive));
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
|
||||
// .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)));
|
||||
// // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
|
||||
@@ -240,115 +240,115 @@ public class RobotContainer {
|
||||
|
||||
|
||||
// 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_robotShooter.runDrumShooterVelocityPID(11000)));
|
||||
// .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
|
||||
// .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
|
||||
// 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
|
||||
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
|
||||
|
||||
/* Button Box */
|
||||
// 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
|
||||
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
|
||||
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
|
||||
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
|
||||
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));
|
||||
}
|
||||
|
||||
private void configureButtonBindingsCasual() {
|
||||
/* Driver Buttons */
|
||||
// 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
|
||||
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 */
|
||||
|
||||
// storage
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
|
||||
.whenReleased(new InterruptSubsystem(m_robotStorage));
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage))
|
||||
// .whenReleased(new InterruptSubsystem(m_robotStorage));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
|
||||
.whenReleased(new InterruptSubsystem(m_robotStorage));
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
|
||||
// .whenReleased(new InterruptSubsystem(m_robotStorage));
|
||||
|
||||
// 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)));
|
||||
|
||||
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)));
|
||||
|
||||
// Calibrates turret and hood
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
|
||||
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
|
||||
// // Calibrates turret and hood
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
|
||||
// .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
|
||||
|
||||
// Run drum manual
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000)))
|
||||
.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
|
||||
// // Run drum manual
|
||||
// new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
// .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));
|
||||
}
|
||||
|
||||
public void buildAutos() {
|
||||
// resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
|
||||
String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" };
|
||||
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
|
||||
// String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" };
|
||||
// m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
|
||||
|
||||
String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" };
|
||||
m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
|
||||
// String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" };
|
||||
// m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
|
||||
|
||||
String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" };
|
||||
m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
|
||||
// String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" };
|
||||
// m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
|
||||
|
||||
String[] slalom = new String[] { "Slalom" };
|
||||
m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
|
||||
// String[] slalom = new String[] { "Slalom" };
|
||||
// m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
|
||||
|
||||
String[] barrel = new String[] { "BarrelStart" };
|
||||
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
|
||||
// String[] barrel = new String[] { "BarrelStart" };
|
||||
// m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
|
||||
|
||||
String[] barrelStart = new String[] { "Barrel" };
|
||||
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
|
||||
// String[] barrelStart = new String[] { "Barrel" };
|
||||
// m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
|
||||
|
||||
String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" };
|
||||
m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
|
||||
// String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" };
|
||||
// m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
|
||||
|
||||
String[] barrelMany = new String[] { "BarrelManyWaypoints" };
|
||||
m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
|
||||
// String[] barrelMany = new String[] { "BarrelManyWaypoints" };
|
||||
// m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
|
||||
|
||||
String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" };
|
||||
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
|
||||
// String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" };
|
||||
// m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
|
||||
|
||||
String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" };
|
||||
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
|
||||
// String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" };
|
||||
// m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
|
||||
|
||||
String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" };
|
||||
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
|
||||
// String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" };
|
||||
// m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
|
||||
|
||||
String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" };
|
||||
m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
|
||||
// String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" };
|
||||
// m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
|
||||
|
||||
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" };
|
||||
m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
|
||||
// m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
|
||||
}
|
||||
|
||||
public void idenPath() {
|
||||
m_robotLime.identifyPath();
|
||||
}
|
||||
// public void idenPath() {
|
||||
// m_robotLime.identifyPath();
|
||||
// }
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
@@ -362,7 +362,7 @@ public class RobotContainer {
|
||||
// RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
|
||||
// Run path following command, then stop at the end.
|
||||
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_sixBallAutoMiddle1.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_barrelStart.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) {
|
||||
System.err.println("ERROR");
|
||||
}
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
//TODO
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
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
|
||||
@@ -394,7 +395,7 @@ public class RobotContainer {
|
||||
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
|
||||
double[] times = new double[paths.length];
|
||||
Trajectory initialTrajectory;
|
||||
m_totalTimeAuto = 0;
|
||||
// m_totalTimeAuto = 0;
|
||||
|
||||
try {
|
||||
if (true) {
|
||||
@@ -424,9 +425,9 @@ public class RobotContainer {
|
||||
DriverStation.reportError("Unable to open trajectory", e.getStackTrace());
|
||||
}
|
||||
|
||||
for (int i = 0; i < times.length; i++) {
|
||||
m_totalTimeAuto += times[i];
|
||||
}
|
||||
// for (int i = 0; i < times.length; i++) {
|
||||
// m_totalTimeAuto += times[i];
|
||||
// }
|
||||
return ramseteCommands;
|
||||
}
|
||||
|
||||
@@ -442,13 +443,13 @@ public class RobotContainer {
|
||||
* Sets the gear of the drivetrain
|
||||
* @param state the gearing of the gearbox (true is high, false is low)
|
||||
*/
|
||||
public void setDriveGearState(boolean state) {
|
||||
m_robotPneumatics.setShiftState(state);
|
||||
}
|
||||
// public void setDriveGearState(boolean state) {
|
||||
// m_robotPneumatics.setShiftState(state);
|
||||
// }
|
||||
|
||||
public void shiftClimberRatchet(boolean state) {
|
||||
m_robotClimber.shiftServo(state);
|
||||
}
|
||||
// public void shiftClimberRatchet(boolean state) {
|
||||
// m_robotClimber.shiftServo(state);
|
||||
// }
|
||||
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotDrive.setOdometry(pose);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -9,7 +9,6 @@ package frc4388.robot.commands.drive;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
|
||||
public class PlaySongDrive extends CommandBase {
|
||||
private Drive m_drive;
|
||||
@@ -17,10 +16,10 @@ public class PlaySongDrive extends CommandBase {
|
||||
/**
|
||||
* Creates a new PlaySongDrive.
|
||||
*/
|
||||
public PlaySongDrive(Drive subsystem, Shooter shooter) {
|
||||
public PlaySongDrive(Drive subsystem) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
addRequirements(m_drive, shooter);
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,6 +13,8 @@ import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
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.wpiutil.math.Nat;
|
||||
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.VisionConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.LimeLight;
|
||||
import frc4388.robot.subsystems.ShooterAim;
|
||||
// import frc4388.robot.subsystems.ShooterAim_1;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
|
||||
public class VisionUpdateOdometry extends CommandBase {
|
||||
private Vision m_limeLight;
|
||||
private ShooterAim m_shooterAim;
|
||||
// private ShooterAim_1 m_shooterAim;
|
||||
private Drive m_driveTrain;
|
||||
|
||||
private double xPos;
|
||||
@@ -43,11 +44,11 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
* @param shooterAim replace with Turret 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_shooterAim = shooterAim;
|
||||
// m_shooterAim = shooterAim;
|
||||
m_driveTrain = driveTrain;
|
||||
addRequirements(m_limeLight, m_shooterAim, m_driveTrain);
|
||||
addRequirements(m_limeLight, m_driveTrain);
|
||||
|
||||
// // Turn camera on but leave LEDs off
|
||||
// NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
|
||||
@@ -86,10 +87,11 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
|
||||
double distance = 1.d / Math.tan(viewAngle);
|
||||
distance *= VOPConstants.TARGET_HEIGHT; // replace with VisionConstants for 2022
|
||||
System.out.println("Never gonna give you up: " + distance);
|
||||
|
||||
double[] ypr = new double[3];
|
||||
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]);
|
||||
|
||||
xPos = Math.cos(relativeAngle) * distance;
|
||||
@@ -98,6 +100,9 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
|
||||
Pose2d pose = new Pose2d(translate, rotation);
|
||||
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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -55,7 +55,7 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/* Pneumatics Subsystem */
|
||||
public Pneumatics m_pneumaticsSubsystem;
|
||||
Shooter m_shooter;
|
||||
// Shooter_1 m_shooter;
|
||||
|
||||
/* Low Gear Gains */
|
||||
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
|
||||
@@ -310,10 +310,10 @@ public class Drive extends SubsystemBase {
|
||||
*
|
||||
* @param subsystem Subsystem needed.
|
||||
*/
|
||||
public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
|
||||
public void passRequiredSubsystem(Pneumatics subsystem) {
|
||||
m_pneumaticsSubsystem = subsystem;
|
||||
m_shooter = shooter;
|
||||
m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
|
||||
// m_shooter = shooter;
|
||||
// m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
|
||||
}
|
||||
|
||||
public void updateTime() {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}*/
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -7,52 +7,34 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
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 frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Vision extends SubsystemBase {
|
||||
private PhotonCamera m_camera;
|
||||
|
||||
public Vision() {
|
||||
m_camera = new PhotonCamera(VisionConstants.NAME);
|
||||
// TODO
|
||||
}
|
||||
|
||||
public ArrayList<Point> getTargetPoints() {
|
||||
PhotonPipelineResult result = m_camera.getLatestResult();
|
||||
|
||||
if(!result.hasTargets())
|
||||
if(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) != 1)
|
||||
return null;
|
||||
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
|
||||
for(PhotonTrackedTarget target : result.getTargets()) {
|
||||
List<TargetCorner> corners = target.getCorners();
|
||||
|
||||
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));
|
||||
}
|
||||
double[] corners = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornxy").getDoubleArray(new double[0]);
|
||||
for(int i = 0; i < corners.length; i += 2*2) {
|
||||
points.add(new Point(corners[i], corners[i+1]));
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user