It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - /** Organizes all the non-changing values we use in the code */ - public static final int leftFrontPort = 10; - public static final int leftBackPort = 11; - public static final int rightFrontPort = 12; - public static final int rightBackPort = 13; - -} diff --git a/NewDriveTest/src/main/java/frc/robot/Robot.java b/NewDriveTest/src/main/java/frc/robot/Robot.java deleted file mode 100644 index 885590d..0000000 --- a/NewDriveTest/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,119 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.DriveTrain; - -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends TimedRobot { - /** You can leave this in if you want, it only changes anything if you are using the autonomous part of the code, - * if you do remove it, just delete any errors it creates */ - private Command m_autonomousCommand; - - /**Creates an object for our DriveTrain class so we can call things from the class - * make sure to include any variables needed for the parameters - */ - public static DriveTrain m_driveTrain = new DriveTrain(Constants.leftFrontPort, Constants.leftBackPort, Constants.rightFrontPort, Constants.rightBackPort); - /** Similar to the previous line, creates a RobotContainer object so we can call things from the robotcontainer class, - * what you name this is up to you */ - private RobotContainer m_robotContainer; - - - /** The values that we will use from the joystick */ - double xAxis; - double yAxis; - /** Values that will go in the parameters that are needed for the RobotDrive method */ - double leftSpeed; - double rightSpeed; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - } - - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *
This runs after the mode specific periodic functions, but before LiveWindow and
- * SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
- // commands, running already-scheduled commands, removing finished or interrupted commands,
- // and running subsystem periodic() methods. This must be called from the robot's periodic
- // block in order for anything in the Command-based framework to work.
- CommandScheduler.getInstance().run();
- }
-
- /** This function is called once each time the robot enters Disabled mode. */
- @Override
- public void disabledInit() {}
-
- @Override
- public void disabledPeriodic() {}
-
- /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
- @Override
- public void autonomousInit() {
-
- }
-
- /** This function is called periodically during autonomous. */
- @Override
- public void autonomousPeriodic() {}
-
- @Override
- public void teleopInit() {
- // This makes sure that the autonomous stops running when
- // teleop starts running. If you want the autonomous to
- // continue until interrupted by another command, remove
- // this line or comment it out.
- if (m_autonomousCommand != null) {
- m_autonomousCommand.cancel();
- }
- }
-
- /** This function is called periodically during operator control. */
- @Override
- public void teleopPeriodic() {
- /** Defining the XAxis and yAxis objects as the x and y values from our joysticks on the controller */
- xAxis = -m_robotContainer.xboxController.getRawAxis(1);
- yAxis = m_robotContainer.xboxController.getRawAxis(4);
- /** The math required to make the robot turn left and right properly according to the joystick configuration. */
- leftSpeed = yAxis + xAxis;
- rightSpeed = yAxis - xAxis;
- /** Calling back the robotDrive method from the DriveTrain class so that it runs fifty times a second
- * uses the parameters leftSpeed and rightSpeed that we just defined
- */
- m_driveTrain.robotDrive(leftSpeed, rightSpeed);
-
- }
-
- @Override
- public void testInit() {
- // Cancels all running commands at the start of test mode.
- CommandScheduler.getInstance().cancelAll();
- }
-
- /** This function is called periodically during test mode. */
- @Override
- public void testPeriodic() {}
-}
diff --git a/NewDriveTest/src/main/java/frc/robot/RobotContainer.java b/NewDriveTest/src/main/java/frc/robot/RobotContainer.java
deleted file mode 100644
index 44489ff..0000000
--- a/NewDriveTest/src/main/java/frc/robot/RobotContainer.java
+++ /dev/null
@@ -1,44 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.XboxController;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
- * subsystems, commands, and button mappings) should be declared here.
- */
-public class RobotContainer {
- // The robot's subsystems and commands are defined here...
- /** Declaring and defining our joystick object */
- public static Joystick xboxController = new Joystick(1);
-
- /** The container for the robot. Contains subsystems, OI devices, and commands. */
- public RobotContainer() {
- // Configure the button bindings
- configureButtonBindings();
- }
-
- /**
- * Use this method to define your button->command mappings. Buttons can be created by
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
- * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
- */
- private void configureButtonBindings() {}
-
- /**
- * Use this to pass the autonomous command to the main {@link Robot} class.
- *
- * @return the command to run in autonomous
- */
-
-}
diff --git a/NewDriveTest/src/main/java/frc/robot/subsystems/DriveTrain.java b/NewDriveTest/src/main/java/frc/robot/subsystems/DriveTrain.java
deleted file mode 100644
index ba233bf..0000000
--- a/NewDriveTest/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ /dev/null
@@ -1,56 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems;
-
-/** YOU DO NOT HAVE TO WORRY ABOUT THIS SECTION, USING TAB WILL ADD ANYTHING NEEDED HERE AUTOMATICALLY */
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-
-
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class DriveTrain extends SubsystemBase {
- /** Creates a new DriveTrain. */
- /** Creates a differential drive which allows the two speed controller groups to work together */
- DifferentialDrive diffDrive;
- /**Creates a speed controller group which allows you to run multiple motors with one object */
- SpeedControllerGroup leftSide;
- SpeedControllerGroup rightSide;
- /**Creates the objects for each motor controller */
- CANSparkMax leftFront;
- CANSparkMax leftBack;
- CANSparkMax rightFront;
- CANSparkMax rightBack;
-
-
- public DriveTrain(int leftFrontPort, int leftBackPort, int rightFrontPort, int rightBackPort) {
- /** Defining all the motor controllers with the CANSparkMax(port, motor type) method */
- leftFront = new CANSparkMax(leftFrontPort, MotorType.kBrushless);
- leftBack = new CANSparkMax(leftBackPort, MotorType.kBrushless);
- rightBack = new CANSparkMax(rightBackPort, MotorType.kBrushless);
- rightFront = new CANSparkMax(rightFrontPort, MotorType.kBrushless);
- /**Defining the two speed controller groups using the SpeedControllerGroup(motor, motor) method */
- leftSide = new SpeedControllerGroup(leftFront, leftBack);
- rightSide = new SpeedControllerGroup(rightFront, rightBack);
-
- /**Defining the differential drive object using the DifferentialDrive(speedcontrollergroup, speedcontrollergroup) method */
- diffDrive = new DifferentialDrive(leftSide, rightSide);
- }
-
- /**Method we will use to actually control the robot, which need the parameters leftSpeed and rightSpeed, which will be defined in robot */
- public void robotDrive(double leftSpeed, double rightSpeed){
- /** A method tied to the DifferentialDrive object in the Wpilib libraries, basically lets us control the motors in a tank drive fashion */
- diffDrive.tankDrive(leftSpeed, rightSpeed);
- /** Creates a minimum required output for the joystick to run the motor */
- diffDrive.setDeadband(0.1);
- }
-
- @Override
- public void periodic() {
- // This method will be called once per scheduler run (50 times a second)
- }
-}
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..f5306f1
--- /dev/null
+++ b/README.md
@@ -0,0 +1 @@
+# FRC Team 696's code for the 2020 season.
diff --git a/Version and Port Tracking.xlsx b/Version and Port Tracking.xlsx
new file mode 100644
index 0000000000000000000000000000000000000000..8aa5ebc95d96ea53ce1ab76deae9a4a4908f260a
GIT binary patch
literal 9757
zcmd^F1z40@x29W41cn+K6bT)=TjJ0NN_R7K_s|F`r9*d0cPk(gN{O^E14BrM5<_0_
z-2Wbr|2gMA_kaHX#&g#^vuA(%nZ3Tf-@U%I_Ikgqu8fXBhJu5GgW{g0qmJ@}P+rx&
z9k?xAO}%Wv)^2~R+~e|gbjTPn1b6WeVDH}CBz!n;t4DcLhRkS0m4^_{0;Z0CHb9v7
zIdD{o-_{S6&hfnja%T|i-f%2ycGI6}y@nLi_g3AWArs+_Q*AQ4AY{@cv{&BXc@Oxs
z4!~DF+*ZW6LlO|%t*4|TCJet%@MWy-#u9yDf^Z4$kfNu~(5rR1eMy{Dzqd_U
ztc&ZN=&9(4uNNwlVGVqEM^YIyCxQ%N
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final int CANtimeout = 10;
+
+ public static final int timeBetweenPockets = 50;
+
+ public static final class SpindexerConstants {
+ public static final int nPockets = 5;
+ public static final int MotorCANID = 20;
+ public static final int EncoderPort = 0;
+ public static final double P = 0.08;
+ public static final double closedLoopRampRate = 0.5;
+ public static final double openLoopRampRate = 0.25;
+ public static final int RevTBEMinMicroseconds = 1;
+ public static final int RevTBEMaxMicroseconds = 1024;
+ // public static final double[] PocketPositions = {0.9335, 0.1398, 0.3431,
+ // 0.5386, 0.7351};
+ // public static final double[] PocketPositions = {0.9689, 0.1689, 0.3689,
+ // 0.5689, 0.7689};
+ public static final double[] PocketPositions = { 0.964, 0.173, 0.375, 0.580, 0.775 };
+ public static final double PositionTolerance = 0.005; // Used to calcuate isOnTarget
+ public static final double VelocityTolerance = 0.01; // used to calculate isMoving
+ public static final double MotorTurnsPerSpindexerTurn = 62.5;
+ public static final byte ColorSensorsMuxAddress = 0x70;
+ public static final byte ColorSensor1MuxChannel = 0;
+ public static final byte ColorSensor2MuxChannel = 1;
+ public static final byte ColorSensor3MuxChannel = 2;
+ public static final byte ColorSensor4MuxChannel = 3;
+ public static final byte ColorSensor5MuxChannel = 7;
+
+ public static final double HueMax = 1;
+ public static final double HueMin = 0;
+ public static final double SatMax = 1;
+ public static final double SatMin = 0;
+ public static final double ValueMax = 1;
+ public static final double ValueMin = 0;
+
+ public static final int KickMotorCANID = 21;
+ public static final double KickMotorSpeed = 1.;
+ public static final double KickMotorReverseSpeed = -0.1;
+
+ public static final double loadingDrumPower = 0.22;
+ public static final double continuousShootDrumPower = 0.55;
+ public static final double stopDrumPower = 0;
+
+ public static final double timeBetweenIndex = 1;
+
+ public static final double indexingJamDetectionCurrent = 15;
+ public static final double loadingJamDetectionCurrent = 10;
+ public static final double antiJamTimeout = 50;
+ }
+
+ public static final class ShooterConstants {
+ public static final int leftShooterHoodServo = 2;
+ public static final int rightShooterHoodServo = 3;
+
+ public static final int leftShooterPort = 42;
+ public static final boolean leftShooterInverted = false;
+ public static final boolean leftShooterSensorPhase = false;
+ public static final int rightShooterPort = 43;
+ public static final boolean rightShooterInverted = true;
+ public static final boolean rightShooterSensorPhase = false;
+ public static final int pidSlot = 0;
+ public static double shooterkP = 0.6;
+ public static double shooterkI = 0.0;
+ public static double shooterkD = 0.0;
+ public static double shooterkF = 0.06;
+ public static final int allowableShooterError = 10;
+
+ // TODO: run simple motor characterization routine to deterine feedforward gains
+ public static final double shooterkSGain = 0;
+ public static final double shooterkVGain = 0;
+ public static final double shooterkAGain = 0;
+
+ public static final double shooterHoodAngleMaxLimit = 115;
+ public static final double shooterHoodAngleMinLimit = 175;
+
+ public static final int acceleratorPort = 41;
+ public static final double acceleratorPower = 0.3;
+ public static double acceleratorkP = 0;
+ public static double acceleratorkI = 0;
+ public static double acceleratorkD = 0;
+ public static double acceleratorkF = 0;
+ public static final int allowableAcceleratorError = 100;
+ public static final boolean acceleratorInverted = true;
+ public static final boolean acceleratorSensorPhase = false;
+
+ public static final double shooterGearRatio = 2.5;
+
+ public static final double autoLineTargetRPM = 3900;
+ public static final double autoLineTargetAngle = 30;
+
+ public static final double trenchRunTargetRPM = 5742.27;
+ public static final double trenchRunTargetAngle = 18.6;
+
+ }
+
+ public static final class TurretConstants {
+ public static final int turretMotorPort = 40;
+ // public static final boolean turret
+
+ public static final boolean turretMotorInverted = false;
+ public static final boolean turretMotorSensorPhase = true;
+
+ public static final int forwardSoftLimit = -370;
+ public static final int reverseSoftLimit = -570;
+
+ public static final int turretCenterPos = -540;
+
+ public static final double positionTolerance = 0.6;
+
+ public static final double peakOutput = 0.75;
+
+ public static final double kP = 0.05;
+ public static final double kI = 0.0;
+ public static final double kD = 0.001;
+ }
+
+
+ public static final class LimelightConstants {
+ public static final int camTranPipeline = 1;
+ public static final int loadingpipeline = 1;
+ }
+
+ public static final class OperatorConstants {
+ public static final int operatorPanelPort = 0;
+ public static final int driverJoystickPort = 1;
+
+ // Driver gamepad mappings
+ public static final int driveModeButton = 4;
+
+ // Operator panel mappings
+ public static final int ATCRevButton = 1;
+ public static final int intakeOnButton = 3;
+ public static final int intakeOffButton = 4;
+ public static final int intakeOutButton = 5;
+ public static final int fitUnderTrenchButton = 6;
+ public static final int intakeInButton = 6;
+ public static final int ATCForButton = 7;
+ public static final int fireButton = 8;
+ public static final int hoodAutoButton = 9;
+ public static final int shooterAutoButton = 10;
+ public static final int colorWheelPositionControl = 11;
+ public static final int climberUpButton = 12;
+ public static final int colorWheelDeploy = 13;
+ public static final int turretAutoButton = 14;
+ public static final int shooterManualButton = 15;
+ public static final int spinUpButton = 16;
+ public static final int climberDownButton = 17;
+ public static final int colorWheelRotationControl = 18;
+ public static final int driveAssistButton = 19;
+
+ public static final int shooterManualAxis = 3;
+ public static final int turretManualAxis = 2;
+ public static final int hoodManualAxis = 1;
+
+ public static final int ATCRevLED = 0;
+ public static final int limelightLockLED = 1;
+ public static final int limelightCaptureLED = 2;
+ public static final int ATCForLED = 3;
+ public static final int fireLED = 4;
+ public static final int realsenseCaptureLED = 5;
+ public static final int realsenseLockLED = 6;
+ public static final int carabinerLatchLED = 7;
+ public static final int climberUpLimLED = 8;
+ public static final int climberDownLimLED = 9;
+ public static final int colorWheelDeployLED = 10;
+ }
+
+ public static final class DrivetrainConstants {
+ public static final int leftFrontCANID = 10;
+ public static final int leftRearCANID = 11;
+ public static final int rightFrontCANID = 12;
+ public static final int rightRearCANID = 13;
+
+ public static final boolean leftFrontIsInverted = true;
+ public static final boolean leftRearIsInverted = true;
+ public static final boolean rightFrontIsInverted = false;
+ public static final boolean rightRearIsInverted = false;
+
+ public static final double wheelDiameter = 6.; // In inches. Used for calculating speed.
+ public static final double driveGearboxReduction = (60. / 10.) * (30. / 18.);
+
+ public static final int IMUCalTime = 8;
+ public static final double yawkP = 1.;
+ public static final double yawkI = 0.;
+ public static final double yawkD = 0.;
+
+ public static final double maxSpeed = 184.6; // In in/s
+ public static final double maxTurnRate = 5; // In degrees/iteration
+
+ public static final int stallCurrentLimit = 80;
+ public static final int freeCurrentLimit = 80;
+ public static final int stallThresholdRPM = 1000;
+
+ public static final double openLoopRampRate = 0.5;
+
+ public static final double driveControllerDeadband = 0.05;
+
+ public static final int slowTasksSpeed = 50;
+ }
+
+ public static final class IntakeConstants {
+ public static final int IntakeMotorPort = 50;
+ public static final boolean IntakeInverted = false;
+
+ public static final double intakePower = 0.7;
+ public static final double outtakePower = -1;
+ public static final double stopIntakePower = 0;
+ }
+}
diff --git a/NewDriveTest/src/main/java/frc/robot/Main.java b/src/main/java/org/team696/robot/Main.java
similarity index 50%
rename from NewDriveTest/src/main/java/frc/robot/Main.java
rename to src/main/java/org/team696/robot/Main.java
index 8776e5d..a961ae0 100644
--- a/NewDriveTest/src/main/java/frc/robot/Main.java
+++ b/src/main/java/org/team696/robot/Main.java
@@ -1,8 +1,11 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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 frc.robot;
+package org.team696.robot;
import edu.wpi.first.wpilibj.RobotBase;
@@ -12,7 +15,8 @@
* call.
*/
public final class Main {
- private Main() {}
+ private Main() {
+ }
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/src/main/java/org/team696/robot/Robot.java b/src/main/java/org/team696/robot/Robot.java
new file mode 100644
index 0000000..f869600
--- /dev/null
+++ b/src/main/java/org/team696/robot/Robot.java
@@ -0,0 +1,202 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 org.team696.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+import org.team696.robot.Constants.OperatorConstants;
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.ShooterHood;
+
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private static final Logger logger = LogManager.getLogger(Robot.class);
+ public static RobotContainer m_robotContainer;
+ public static OperatorConstants m_robotConstants;
+ private Command m_autonomousCommand;
+ double last_hood_axis = -69.23; //arbitrary value for initialization later
+
+ // public static double shootAngle;
+
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ logger.info("Starting up...");
+ m_robotContainer = new RobotContainer();
+ m_robotConstants = new OperatorConstants();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+
+ RobotContainer.limelight.pipeline(1);
+ SmartDashboard.putBoolean("isOnTarget", m_robotContainer.turretSubsystem.onTarget());
+ SmartDashboard.putNumber("Current RPM", m_robotContainer.shooter.getRPM());
+ SmartDashboard.putNumber("Shoot Angle", m_robotContainer.shooterHood.shootAngleToServoAngle(m_robotContainer.shooterHood.servoAngle()));
+ SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128));
+ SmartDashboard.putNumber("Shoot Angle", m_robotContainer.shooterHood.servoAngleToShootAngle(m_robotContainer.shooterHood.servoAngle()));
+ SmartDashboard.putBoolean("Up to Speed", m_robotContainer.shooter.isUpToSpeed());
+ SmartDashboard.putNumber("Spindexer Current", m_robotContainer.spindexer.getCurrent());
+ SmartDashboard.putNumber("Hood Angle", m_robotContainer.shooterHood.servoAngle());
+ SmartDashboard.putNumber("Accelerator Velocity", m_robotContainer.shooter.getAcceleratorVelocity());
+
+
+ // SmartDashboard.putNumber("spin pos", m_robotContainer.spindexer.getEncoderPosition());
+ // SmartDashboard.putNumber("servo angle", m_robotContainer.shooterHood.servoAngle());
+ // SmartDashboard.putNumber("Number of Balls", m_robotContainer.spindexer.getNumberOfFilledPockets());
+ // SmartDashboard.putNumber("distance", Limelight.distanceFromTarget());
+ // SmartDashboard.putNumber("shooter power", m_robotContainer.shooter.getShooterPower());
+ // SmartDashboard.putNumber("index", Limelight.indexForDistance());
+ // SmartDashboard.putNumber("AUTORPM", m_robotContainer.shooter.getAutoRPM());
+ // SmartDashboard.putNumber("AUTOANGLE", m_robotContainer.shooter.getAutoAngle());
+ // SmartDashboard.putNumber("SERVOAUTOANGLE", m_robotContainer.shooterHood.shootAngleToServoAngle(m_robotContainer.shooter.getAutoAngle()));
+ // SmartDashboard.putNumber("AngleDistance", Limelight.angleDistance());
+ // SmartDashboard.putNumber("Intake Current", m_robotContainer.intake.intakeCurrent());
+ // RobotContainer.limelight.setLights(3);
+ // SmartDashboard.putNumber("Roll", m_robotContainer.drivetrain.roll());
+
+ }
+
+
+ /**
+ * This function is called once each time the robot enters Disabled mode.
+ */
+ @Override
+ public void disabledInit() {
+ logger.info("Transitioning to disabled.");
+ m_robotContainer.spindexer.setBrake(false);
+ // RobotContainer.limelight.setLights(3);
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ Limelight.setLights(1);
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ logger.info("Transitioning to autonomous.");
+ m_robotContainer.spindexer.setBrake(true);
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.schedule();
+ }
+
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+
+
+ }
+
+ @Override
+ public void teleopInit() {
+ logger.info("Transitioning to teleop.");
+ m_robotContainer.spindexer.setBrake(true);
+ // SmartDashboard.putNumber("Target RPM", 3900);
+ // SmartDashboard.putNumber("Target Angle", 30);
+
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+
+
+ @Override
+ public void teleopPeriodic() {
+ if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.hoodAutoButton)){
+ //Previous comp: m_robotContainer.shooterHood.moveServoAngle(51);
+ m_robotContainer.shooterHood.moveServoAngle(51);
+
+
+
+ }
+ else{
+ double hood_axis = -Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128*1.5);
+ if (last_hood_axis == -69.23){
+ last_hood_axis = hood_axis;
+ }
+ double delta = hood_axis - last_hood_axis;
+
+ SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128));
+
+ if (delta + m_robotContainer.shooterHood.servoAngle() <= 51 && delta + m_robotContainer.shooterHood.servoAngle() >= 10){
+ m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
+
+ /*
+ if (m_robotContainer.shooterHood.servoAngle() >= 51 && delta < 0){
+ m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
+ }
+ else if (m_robotContainer.shooterHood.servoAngle() <= 10 && delta > 0){
+ m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
+ }
+
+ if (m_robotContainer.shooterHood.servoAngle() < 51 && m_robotContainer.shooterHood.servoAngle() > 10){
+ m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
+ }
+ */
+ }
+ last_hood_axis = hood_axis;
+
+
+ }
+ // }
+
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/src/main/java/org/team696/robot/RobotContainer.java b/src/main/java/org/team696/robot/RobotContainer.java
new file mode 100644
index 0000000..7375cd7
--- /dev/null
+++ b/src/main/java/org/team696/robot/RobotContainer.java
@@ -0,0 +1,229 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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 org.team696.robot;
+
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import org.team696.MathUtils.MathUtils;
+import org.team696.robot.Constants.IntakeConstants;
+import org.team696.robot.Constants.OperatorConstants;
+import org.team696.robot.Constants.SpindexerConstants;
+import org.team696.robot.commands.ATCForCommand;
+import org.team696.robot.commands.ATCRevCommand;
+import org.team696.robot.commands.AutoIndexKickUp;
+import org.team696.robot.commands.Drive;
+import org.team696.robot.commands.DriveTimer;
+import org.team696.robot.commands.FireCommand;
+import org.team696.robot.commands.IntakeTimerCommand;
+import org.team696.robot.commands.OmniKickUp;
+import org.team696.robot.commands.OmniKickUpTimer;
+import org.team696.robot.commands.ShooterCommand;
+import org.team696.robot.commands.ShooterHoodCommand;
+import org.team696.robot.commands.ShooterPowerCommand;
+import org.team696.robot.commands.ShooterPrep;
+import org.team696.robot.commands.SpinToPocket;
+import org.team696.robot.commands.SpindexerLoading;
+import org.team696.robot.commands.TurretLockOn;
+import org.team696.robot.commands.TurretManual;
+import org.team696.robot.subsystems.Drivetrain;
+import org.team696.robot.subsystems.Intake;
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.Shooter;
+import org.team696.robot.subsystems.ShooterHood;
+import org.team696.robot.subsystems.Spindexer;
+import org.team696.robot.subsystems.TurretSubsystem;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // Subystem declarations
+ private static final Logger logger = LogManager.getLogger(RobotContainer.class);
+ public final Drivetrain drivetrain = new Drivetrain();
+ public final Spindexer spindexer = new Spindexer();
+ public final Shooter shooter = new Shooter();
+ public final ShooterHood shooterHood = new ShooterHood();
+ public final TurretSubsystem turretSubsystem = new TurretSubsystem();
+ public static final Limelight limelight = new Limelight();
+ public final Intake intake = new Intake();
+
+ // Joysticks
+ public final XboxController driverController = new XboxController(OperatorConstants.driverJoystickPort);
+ public static final Joystick operatorPanel = new Joystick(OperatorConstants.operatorPanelPort);
+
+ // Buttons
+
+ // make hood/shooter auto/man to change from 2 distances
+ // make intake in/out be for color wheel height
+ private final JoystickButton driveMode = new JoystickButton(driverController, OperatorConstants.driveModeButton);
+
+ private final JoystickButton spinUpButton = new JoystickButton(operatorPanel, OperatorConstants.spinUpButton);
+ private final JoystickButton shooterAutoButton = new JoystickButton(operatorPanel, OperatorConstants.shooterAutoButton);
+ private final JoystickButton shooterManualButton = new JoystickButton(operatorPanel, OperatorConstants.shooterManualButton);
+ private final JoystickButton fireButton = new JoystickButton(operatorPanel, OperatorConstants.fireButton);
+ private final JoystickButton ATCForButton = new JoystickButton(operatorPanel, OperatorConstants.ATCForButton);
+ private final JoystickButton ATCRevButton = new JoystickButton(operatorPanel, OperatorConstants.ATCRevButton);
+
+ private final JoystickButton turretAutoButton = new JoystickButton(operatorPanel, OperatorConstants.turretAutoButton);
+ private final JoystickButton hoodAutoButton = new JoystickButton(operatorPanel, OperatorConstants.hoodAutoButton);
+
+ private final JoystickButton intakeOnButton = new JoystickButton(operatorPanel, OperatorConstants.intakeOnButton);
+ private final JoystickButton intakeOffButton = new JoystickButton(operatorPanel, OperatorConstants.intakeOffButton);
+
+ private final JoystickButton fitUnderTrenchButton = new JoystickButton(operatorPanel, OperatorConstants.fitUnderTrenchButton);
+
+ private final JoystickButton continuousButton = new JoystickButton(operatorPanel, OperatorConstants.colorWheelDeploy);
+ private final JoystickButton omniReverse = new JoystickButton(operatorPanel, OperatorConstants.colorWheelRotationControl);
+ private final JoystickButton automaticShootPrep = new JoystickButton(operatorPanel, OperatorConstants.driveAssistButton);
+ private final JoystickButton automaticIndex = new JoystickButton(operatorPanel, OperatorConstants.colorWheelPositionControl);
+
+ public double getDriveSpeed() {
+ // Math to correct for nonlinearities in the controller should happen here.
+ double jsVal = -driverController.getY(Hand.kLeft);
+ if (jsVal > 0) {
+ return MathUtils.deadBand(jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
+ } else {
+ return -MathUtils.deadBand(-jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
+ }
+ }
+
+ public double getDriveTurn() {
+ // Math to correct for nonlinearities in the controller should happen here.
+ double jsVal = -driverController.getX(Hand.kRight);
+ if (jsVal > 0) {
+ return MathUtils.deadBand(jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
+ } else {
+ return -MathUtils.deadBand(-jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
+ }
+ }
+
+ public double turretManual() {
+ return operatorPanel.getRawAxis(1);
+ }
+
+ public RobotContainer() {
+ Command driveCommand = new Drive(() -> getDriveSpeed(), () -> getDriveTurn(), drivetrain);
+ drivetrain.setDefaultCommand(driveCommand);
+ configureButtonBindings();
+ }
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ private void configureButtonBindings() {
+ // rpm is 3900
+ spinUpButton.whenPressed(new ShooterCommand(shooter, shooter.shootRPM, true));
+ spinUpButton.whenReleased(new ShooterPowerCommand(shooter, 0, false));
+
+ turretAutoButton.whileHeld(new TurretLockOn(turretSubsystem, limelight));
+ turretAutoButton.whenReleased(new TurretManual(turretSubsystem, limelight));
+
+ SpindexerLoading loadingOff = new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.stopIntakePower);
+ intakeOnButton.whileHeld(
+ new SpindexerLoading(spindexer, intake, SpindexerConstants.loadingDrumPower, IntakeConstants.intakePower));
+ intakeOnButton.whenReleased(loadingOff);
+ intakeOffButton.whileHeld(
+ new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.outtakePower));
+ intakeOffButton.whenReleased(loadingOff);
+
+ // maybe have this button cancel the human player loading sequence
+ continuousButton.whileHeld(new SpindexerLoading(spindexer, intake, SpindexerConstants.continuousShootDrumPower,
+ IntakeConstants.stopIntakePower));
+ continuousButton.whenReleased(loadingOff);
+
+ automaticShootPrep.whenPressed(new ShooterPrep(shooter, turretSubsystem));
+ automaticShootPrep.whenReleased(new ShooterPowerCommand(shooter, 0, false));
+ automaticShootPrep.whenReleased(new TurretManual(turretSubsystem, limelight));
+
+ automaticIndex.whenPressed(new AutoIndexKickUp(spindexer, intake, SpindexerConstants.continuousShootDrumPower, 0));
+ automaticIndex.whenReleased(loadingOff);
+
+ fireButton.whenPressed(new FireCommand(shooter, spindexer, SpindexerConstants.KickMotorSpeed));
+ fireButton.whenReleased(new FireCommand(shooter, spindexer, SpindexerConstants.stopDrumPower));
+
+ OmniKickUp reverseOmni = new OmniKickUp(spindexer, SpindexerConstants.KickMotorReverseSpeed);
+ omniReverse.whenPressed(reverseOmni);
+ omniReverse.whenReleased(new OmniKickUp(spindexer, SpindexerConstants.stopDrumPower));
+
+ ATCForCommand ATCfor = new ATCForCommand(spindexer);
+ ATCRevCommand ATCrev = new ATCRevCommand(spindexer);
+
+ ATCForButton.whenPressed(ATCfor);
+ ATCForButton.cancelWhenPressed(ATCrev);
+
+ ATCRevButton.whenPressed(ATCrev);
+ ATCRevButton.cancelWhenPressed(ATCfor);
+
+ intakeOnButton.whenReleased(ATCfor);
+
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ @SuppressWarnings("checkstyle:magicnumber")
+ public Command getAutonomousCommand() {
+
+ return new SequentialCommandGroup(
+
+ new ParallelCommandGroup(
+ new TurretLockOn(turretSubsystem, limelight),
+ new ShooterCommand(shooter, 3900, true),
+
+ new SequentialCommandGroup(
+ new ShooterHoodCommand(shooterHood, 51),
+ // new IntakeTimerCommand(intake, 50);
+ new SpinToPocket(spindexer, 1),
+ new OmniKickUpTimer(spindexer, true, 25),
+ new SpinToPocket(spindexer, 2),
+ new OmniKickUpTimer(spindexer, true, 25),
+ new SpinToPocket(spindexer, 3),
+ new OmniKickUpTimer(spindexer, true, 25)
+ )
+ ),
+
+ new DriveTimer(drivetrain, -0.5, 0, 50),
+ new DriveTimer(drivetrain, 0, 0.7, 40),
+ new DriveTimer(drivetrain, 0.5, 0, 25),
+ new ParallelCommandGroup(
+ new DriveTimer(drivetrain, 0.5, 0, 35),
+ new IntakeTimerCommand(intake, 35)
+ )
+
+
+ );
+
+ }
+
+ public boolean getDriveMode() {
+ return driveMode.get();
+ }
+
+ public void setLimelightCaptureLED(boolean state) {
+ operatorPanel.setOutput(OperatorConstants.limelightCaptureLED, state);
+ }
+
+ public void setLimelightLockLED(boolean state) {
+ operatorPanel.setOutput(OperatorConstants.limelightLockLED, state);
+ }
+}
diff --git a/src/main/java/org/team696/robot/TrajectoryTable.java b/src/main/java/org/team696/robot/TrajectoryTable.java
new file mode 100644
index 0000000..213a3ca
--- /dev/null
+++ b/src/main/java/org/team696/robot/TrajectoryTable.java
@@ -0,0 +1,349 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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 org.team696.robot;
+
+/**
+ * Add your docs here.
+ */
+public class TrajectoryTable {
+
+ /**
+ * First line is 120 inches away
+ * First column is angle that ball is shot
+ * Second column is velcity of the ball in RPM
+ */
+public static final double[][] TrajectoryTable = {
+
+{0.0,0.0},
+{51.2,2354.45},
+{51.0,2362.16},
+{50.8,2369.90},
+{50.6,2377.69},
+{50.3,2385.51},
+{50.1,2393.37},
+{49.9,2401.27},
+{49.7,2409.20},
+{49.4,2417.17},
+{49.2,2425.17},
+{49.0,2433.22},
+{48.8,2441.29},
+{48.6,2449.40},
+{48.3,2457.55},
+{48.1,2465.73},
+{47.9,2473.94},
+{47.7,2482.19},
+{47.5,2490.47},
+{47.3,2498.79},
+{47.1,2507.13},
+{46.9,2515.51},
+{46.7,2523.92},
+{46.5,2532.37},
+{46.3,2540.84},
+{46.1,2549.35},
+{45.9,2557.88},
+{45.7,2566.45},
+{45.5,2575.04},
+{45.3,2583.67},
+{45.1,2592.32},
+{44.9,2601.01},
+{44.7,2609.72},
+{44.5,2618.46},
+{44.3,2627.23},
+{44.2,2636.03},
+{44.0,2644.86},
+{43.8,2653.71},
+{43.6,2662.59},
+{43.4,2671.50},
+{43.2,2680.44},
+{43.1,2689.40},
+{42.9,2698.38},
+{42.7,2707.40},
+{42.5,2716.43},
+{42.4,2725.50},
+{42.2,2734.59},
+{42.0,2743.70},
+{41.8,2752.84},
+{41.7,2762.00},
+{41.5,2771.19},
+{41.3,2780.40},
+{41.2,2789.63},
+{41.0,2798.89},
+{40.8,2808.17},
+{40.7,2817.48},
+{40.5,2826.80},
+{40.3,2836.15},
+{40.2,2845.53},
+{40.0,2854.92},
+{39.9,2864.34},
+{39.7,2873.77},
+{39.6,2883.23},
+{39.4,2892.71},
+{39.2,2902.21},
+{39.1,2911.73},
+{38.9,2921.28},
+{38.8,2930.84},
+{38.6,2940.42},
+{38.5,2950.02},
+{38.3,2959.65},
+{38.2,2969.29},
+{38.1,2978.95},
+{37.9,2988.63},
+{37.8,2998.33},
+{37.6,3008.05},
+{37.5,3017.79},
+{37.3,3027.54},
+{37.2,3037.32},
+{37.1,3047.11},
+{36.9,3056.92},
+{36.8,3066.75},
+{36.6,3076.60},
+{36.5,3086.46},
+{36.4,3096.34},
+{36.2,3106.24},
+{36.1,3116.15},
+{36.0,3126.08},
+{35.8,3136.03},
+{35.7,3146.00},
+{35.6,3155.98},
+{35.4,3165.98},
+{35.3,3175.99},
+{35.2,3186.02},
+{35.1,3196.06},
+{34.9,3206.12},
+{34.8,3216.20},
+{34.7,3226.29},
+{34.6,3236.40},
+{34.4,3246.52},
+{34.3,3256.65},
+{34.2,3266.81},
+{34.1,3276.97},
+{34.0,3287.15},
+{33.8,3297.35},
+{33.7,3307.55},
+{33.6,3317.78},
+{33.5,3328.01},
+{33.4,3338.26},
+{33.3,3348.53},
+{33.1,3358.80},
+{33.0,3369.10},
+{32.9,3379.40},
+{32.8,3389.72},
+{32.7,3400.05},
+{32.6,3410.39},
+{32.5,3420.75},
+{32.4,3431.12},
+{32.2,3441.50},
+{32.1,3451.89},
+{32.0,3462.30},
+{31.9,3472.72},
+{31.8,3483.15},
+{31.7,3493.59},
+{31.6,3504.04},
+{31.5,3514.51},
+{31.4,3524.99},
+{31.3,3535.48},
+{31.2,3545.98},
+{31.1,3556.49},
+{31.0,3567.02},
+{30.9,3577.55},
+{30.8,3588.10},
+{30.7,3598.66},
+{30.6,3609.22},
+{30.5,3619.80},
+{30.4,3630.39},
+{30.3,3640.99},
+{30.2,3651.60},
+{30.1,3662.22},
+{30.0,3672.86},
+{29.9,3683.50},
+{29.8,3694.15},
+{29.7,3704.81},
+{29.6,3715.49},
+{29.5,3726.17},
+{29.4,3736.86},
+{29.3,3747.56},
+{29.2,3758.27},
+{29.2,3768.99},
+{29.1,3779.72},
+{29.0,3790.46},
+{28.9,3801.21},
+{28.8,3811.97},
+{28.7,3822.74},
+{28.6,3833.52},
+{28.5,3844.30},
+{28.4,3855.10},
+{28.4,3865.90},
+{28.3,3876.71},
+{28.2,3887.53},
+{28.1,3898.36},
+{28.0,3909.20},
+{27.9,3920.05},
+{27.8,3930.90},
+{27.8,3941.77},
+{27.7,3952.64},
+{27.6,3963.52},
+{27.5,3974.41},
+{27.4,3985.31},
+{27.4,3996.21},
+{27.3,4007.12},
+{27.2,4018.04},
+{27.1,4028.97},
+{27.0,4039.91},
+{27.0,4050.85},
+{26.9,4061.80},
+{26.8,4072.76},
+{26.7,4083.73},
+{26.6,4094.70},
+{26.6,4105.68},
+{26.5,4116.67},
+{26.4,4127.67},
+{26.3,4138.67},
+{26.3,4149.68},
+{26.2,4160.70},
+{26.1,4171.72},
+{26.0,4182.76},
+{26.0,4193.80},
+{25.9,4204.84},
+{25.8,4215.89},
+{25.7,4226.95},
+{25.7,4238.02},
+{25.6,4249.09},
+{25.5,4260.17},
+{25.5,4271.26},
+{25.4,4282.35},
+{25.3,4293.45},
+{25.2,4304.55},
+{25.2,4315.66},
+{25.1,4326.78},
+{25.0,4337.91},
+{25.0,4349.04},
+{24.9,4360.17},
+{24.8,4371.31},
+{24.8,4382.46},
+{24.7,4393.62},
+{24.6,4404.78},
+{24.6,4415.95},
+{24.5,4427.12},
+{24.4,4438.30},
+{24.4,4449.48},
+{24.3,4460.67},
+{24.2,4471.87},
+{24.2,4483.07},
+{24.1,4494.28},
+{24.0,4505.49},
+{24.0,4516.71},
+{23.9,4527.93},
+{23.9,4539.16},
+{23.8,4550.40},
+{23.7,4561.64},
+{23.7,4572.88},
+{23.6,4584.13},
+{23.6,4595.39},
+{23.5,4606.65},
+{23.4,4617.92},
+{23.4,4629.19},
+{23.3,4640.46},
+{23.2,4651.75},
+{23.2,4663.03},
+{23.1,4674.33},
+{23.1,4685.62},
+{23.0,4696.92},
+{23.0,4708.23},
+{22.9,4719.54},
+{22.8,4730.86},
+{22.8,4742.18},
+{22.7,4753.51},
+{22.7,4764.84},
+{22.6,4776.17},
+{22.6,4787.51},
+{22.5,4798.86},
+{22.4,4810.21},
+{22.4,4821.56},
+{22.3,4832.92},
+{22.3,4844.28},
+{22.2,4855.65},
+{22.2,4867.02},
+{22.1,4878.40},
+{22.1,4889.78},
+{22.0,4901.17},
+{21.9,4912.56},
+{21.9,4923.95},
+{21.8,4935.35},
+{21.8,4946.75},
+{21.7,4958.16},
+{21.7,4969.57},
+{21.6,4980.98},
+{21.6,4992.40},
+{21.5,5003.82},
+{21.5,5015.25},
+{21.4,5026.68},
+{21.4,5038.12},
+{21.3,5049.56},
+{21.3,5061.00},
+{21.2,5072.44},
+{21.2,5083.90},
+{21.1,5095.35},
+{21.1,5106.81},
+{21.0,5118.27},
+{21.0,5129.74},
+{20.9,5141.21},
+{20.9,5152.68},
+{20.8,5164.16},
+{20.8,5175.64},
+{20.7,5187.12},
+{20.7,5198.61},
+{20.6,5210.10},
+{20.6,5221.60},
+{20.5,5233.10},
+{20.5,5244.60},
+{20.4,5256.11},
+{20.4,5267.62},
+{20.4,5279.13},
+{20.3,5290.65},
+{20.3,5302.17},
+{20.2,5313.69},
+{20.2,5325.22},
+{20.1,5336.75},
+{20.1,5348.28},
+{20.0,5359.82},
+{20.0,5371.36},
+{19.9,5382.90},
+{19.9,5394.45},
+{19.9,5406.00},
+{19.8,5417.55},
+{19.8,5429.11},
+{19.7,5440.67},
+{19.7,5452.23},
+{19.6,5463.80},
+{19.6,5475.37},
+{19.6,5486.94},
+{19.5,5498.51},
+{19.5,5510.09},
+{19.4,5521.67},
+{19.4,5533.26},
+{19.3,5544.84},
+{19.3,5556.44},
+{19.3,5568.03},
+{19.2,5579.62},
+{19.2,5591.22},
+{19.1,5602.83},
+{19.1,5614.43},
+{19.0,5626.04},
+{19.0,5637.65},
+{19.0,5649.26},
+{18.9,5660.88},
+{18.9,5672.50},
+{18.8,5684.12},
+{18.8,5695.74},
+{18.8,5707.37},
+{18.7,5719.00},
+{18.7,5730.64},
+{18.6,5742.27}
+};
+
+}
\ No newline at end of file
diff --git a/src/main/java/org/team696/robot/commands/ATCForCommand.java b/src/main/java/org/team696/robot/commands/ATCForCommand.java
new file mode 100644
index 0000000..d7d6ed1
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ATCForCommand.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import org.team696.robot.Constants;
+import org.team696.robot.subsystems.Spindexer;
+
+public class ATCForCommand extends CommandBase {
+ /**
+ * Creates a new SpinToIndex.
+ */
+ private static final Logger logger = LogManager.getLogger(ATCForCommand.class);
+ Spindexer spindexer;
+
+ int oldSpindexerPocket = 0;
+
+ public ATCForCommand(Spindexer spindexer) {
+ this.spindexer = spindexer;
+ addRequirements(spindexer);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ oldSpindexerPocket = spindexer.getTargetPocket();
+ int targetPocket;
+ if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) {
+ targetPocket = 1;
+ } else {
+ targetPocket = spindexer.getTargetPocket() + 1;
+ }
+ logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
+ spindexer.setTargetPocket(targetPocket);
+ }
+
+ // 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) {
+ if(interrupted){
+ logger.info("Spindexer command interrupted.");
+ } else {
+ logger.info("Spindexer on target.");
+ }
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return spindexer.isOnTarget();
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ATCRevCommand.java b/src/main/java/org/team696/robot/commands/ATCRevCommand.java
new file mode 100644
index 0000000..beb0e54
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ATCRevCommand.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import org.team696.robot.Constants;
+import org.team696.robot.subsystems.Spindexer;
+
+public class ATCRevCommand extends CommandBase {
+ private static final Logger logger = LogManager.getLogger(ATCRevCommand.class);
+ Spindexer spindexer;
+ int oldSpindexerPocket;
+
+ public ATCRevCommand(Spindexer spindexer) {
+ this.spindexer = spindexer;
+ addRequirements(spindexer);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ oldSpindexerPocket = spindexer.getTargetPocket();
+ int targetPocket;
+ if (spindexer.getTargetPocket() == 1) {
+ targetPocket = Constants.SpindexerConstants.nPockets;
+ } else {
+ targetPocket = spindexer.getTargetPocket() - 1;
+ }
+ logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
+ spindexer.setTargetPocket(targetPocket);
+ }
+
+ // 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) {
+ if(interrupted){
+ logger.info("Spindexer command interrupted.");
+ } else {
+ logger.info("Spindexer on target.");
+ }
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return spindexer.isOnTarget();
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java b/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java
new file mode 100644
index 0000000..9a2f3b2
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Intake;
+import org.team696.robot.subsystems.Spindexer;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+// 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 AutoIndexKickUp extends SequentialCommandGroup {
+ /**
+ * Creates a new AutoIndexKickUp.
+ */
+ public AutoIndexKickUp(Spindexer spindexer, Intake intake, double drumPower, double omniPower) {
+ // Add your commands in the super() call, e.g.
+ // super(new FooCommand(), new BarCommand());
+ super(
+ // new ATCForCommand(spindexer),
+ // new OmniKickUp(spindexer, omniPower),
+ new ContinuousShoot(spindexer, drumPower, true)
+
+ );
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ClimberLift.java b/src/main/java/org/team696/robot/commands/ClimberLift.java
new file mode 100644
index 0000000..18d54c8
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ClimberLift.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+
+// import org.team696.robot.subsystems.ClimberSubsystem;
+// import org.team696.robot.RobotContainer;
+// import org.team696.robot.Constants.OperatorConstants;
+// import org.team696.robot.subsystems.ClimberServos;
+
+// import edu.wpi.first.wpilibj2.command.CommandBase;
+
+// public class ClimberLift extends CommandBase {
+// private ClimberSubsystem climberSubsystem;
+// private ClimberServos climberServos;
+// /**
+// * Creates a new ClimberPullUp.
+// */
+// public ClimberLift(ClimberSubsystem climberSubsystem, ClimberServos climberServos) {
+// this.climberSubsystem = climberSubsystem;
+// this.climberServos = climberServos;
+// addRequirements(climberSubsystem);
+// addRequirements(climberServos);
+// }
+
+// // 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() {
+// // servoSubsystem.closeServo();
+// // if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)){
+
+// climberSubsystem.setClimberPower();
+// // System.out.println(climberSubsystem.climberTargetPower);
+
+// // }
+// // else{
+// // climberSubsystem.setClimberPower(0);
+// // // System.out.println(climberSubsystem.climberTargetPower);
+
+// // }
+
+
+// }
+
+
+
+// // Called once the command ends or is interrupted.
+// @Override
+// public void end(boolean interrupted) {
+// // servoSubsystem.CloseServo();
+
+// }
+
+// // Returns true when the command should end.
+// @Override
+// public boolean isFinished() {
+// return false;
+// }
+// }
diff --git a/src/main/java/org/team696/robot/commands/ClimberReach.java b/src/main/java/org/team696/robot/commands/ClimberReach.java
new file mode 100644
index 0000000..b32183f
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ClimberReach.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+// import org.team696.robot.subsystems.ClimberSubsystem;
+// import org.team696.robot.RobotContainer;
+// import org.team696.robot.Constants.OperatorConstants;
+// import org.team696.robot.subsystems.ClimberServos;
+
+// import edu.wpi.first.wpilibj2.command.CommandBase;
+
+// public class ClimberReach extends CommandBase {
+// private ClimberSubsystem climberSubsystem;
+// private ClimberServos servoSubsystem;
+// /**
+// * Creates a new ClimberPushDown.
+// */
+
+// boolean servoIsOpen = false;
+
+// public ClimberReach(ClimberSubsystem climberSubsystem, ClimberServos servoSubsystem) {
+// this.climberSubsystem = climberSubsystem;
+// this.servoSubsystem = servoSubsystem;
+// addRequirements(climberSubsystem);
+// addRequirements(servoSubsystem);
+// // Use addRequirements() here to declare subsystem dependencies.
+// }
+
+// // 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() {
+
+// // servoSubsystem.moveClimberServos(180);
+
+// if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton)){
+// climberSubsystem.setClimberPower();
+
+// }
+// else{
+// climberSubsystem.setClimberPower(0);
+// }
+
+// }
+
+// // Called once the command ends or is interrupted.
+// @Override
+// public void end(boolean interrupted) {
+// //ClimberSubsystem.climberState = ClimberStates.AT_HEIGHT;
+// }
+
+// // Returns true when the command should end.
+// @Override
+// public boolean isFinished() {
+// //if sensor trips return tru
+// // return climberSubsystem.isLatchedOn();
+// return false;
+// }
+// }
diff --git a/src/main/java/org/team696/robot/commands/ContinuousShoot.java b/src/main/java/org/team696/robot/commands/ContinuousShoot.java
new file mode 100644
index 0000000..e131fcd
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ContinuousShoot.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Spindexer;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ContinuousShoot extends CommandBase {
+ /**
+ * Creates a new ContinuousShoot.
+ */
+ Spindexer spindexer;
+ boolean kickUpState;
+ double drumSpeed;
+ int timer;
+ public ContinuousShoot(Spindexer spindexer, double drumSpeed, boolean kickUpState) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.spindexer = spindexer;
+ this.kickUpState = kickUpState;
+ this.drumSpeed = drumSpeed;
+ addRequirements(spindexer);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ timer = 0;
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ timer++;
+ if(timer>6){
+ spindexer.spindexerLoadingAntiJam(drumSpeed);
+
+ }
+ spindexer.setKickMotor(1);
+
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ spindexer.setKickMotor(0);
+ timer = 0;
+
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/Drive.java b/src/main/java/org/team696/robot/commands/Drive.java
new file mode 100644
index 0000000..8efafb3
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/Drive.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import java.util.function.DoubleSupplier;
+
+import org.team696.robot.subsystems.Drivetrain;
+import org.team696.robot.subsystems.Shooter;
+
+public class Drive extends CommandBase {
+ private final Drivetrain drivetrain;
+ private final DoubleSupplier speedSupplier;
+ private final DoubleSupplier turnSupplier;
+
+ /**
+ * Creates a new Drive.
+ */
+ double currentServoPosition;
+
+ public Drive(DoubleSupplier speed, DoubleSupplier turn, Drivetrain drivetrain) {
+ this.drivetrain = drivetrain;
+ this.speedSupplier = speed;
+ this.turnSupplier = turn;
+
+ addRequirements(drivetrain);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ // currentServoPosition = shooter.servoAngle();
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ drivetrain.arcadeDrive(speedSupplier.getAsDouble(), turnSupplier.getAsDouble());
+
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ drivetrain.arcadeDrive(0, 0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/DriveTimer.java b/src/main/java/org/team696/robot/commands/DriveTimer.java
new file mode 100644
index 0000000..705801d
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/DriveTimer.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Drivetrain;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveTimer extends CommandBase {
+ private Drivetrain drivetrain;
+ /**
+ * Creates a new DriveTimer.
+ */
+ int timer;
+ int x = 0;
+ double speed;
+ double turn;
+ public DriveTimer(Drivetrain drivetrain, double speed, double turn, int timer) {
+ this.drivetrain = drivetrain;
+ this.timer = timer;
+ this.speed = speed;
+ this.turn = turn;
+ addRequirements(drivetrain);
+ }
+
+ // 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() {
+ drivetrain.arcadeDrive(speed, turn);
+ x++;
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ drivetrain.arcadeDrive(0, 0);
+ x = 0;
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return x>timer;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/FireCommand.java b/src/main/java/org/team696/robot/commands/FireCommand.java
new file mode 100644
index 0000000..00e140e
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/FireCommand.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Shooter;
+import org.team696.robot.subsystems.Spindexer;
+
+public class FireCommand extends CommandBase {
+ /**
+ * Creates a new FireCommand.
+ */
+ private Shooter shooter;
+ private Spindexer spindexer;
+ private double power;
+
+ public FireCommand(Shooter shooter, Spindexer spindexer, double kickMotorPower) {
+
+ this.shooter = shooter;
+ this.spindexer = spindexer;
+ this.power = kickMotorPower;
+ addRequirements(shooter);
+ addRequirements(spindexer);
+ }
+
+ // 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(shooter.isUpToSpeed()){
+ // shooter.setAcceleratorPower(true);
+ spindexer.setKickMotor(power);
+ // spindexer.spindexerLoadingAntiJam(0.5, 20);
+
+
+ }
+ else{
+ spindexer.setKickMotor(0);
+
+ }
+
+ }
+
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/FullShoot.java b/src/main/java/org/team696/robot/commands/FullShoot.java
new file mode 100644
index 0000000..1c02161
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/FullShoot.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.ShooterHood;
+import org.team696.robot.subsystems.Spindexer;
+import org.team696.robot.subsystems.TurretSubsystem;
+
+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 FullShoot extends SequentialCommandGroup {
+ /**
+ * Creates a new FullShoot.
+ */
+ public FullShoot(ShooterHood shooterHood, Spindexer spindexer, TurretSubsystem turretSubsystem, Limelight limelight) {
+ // Add your commands in the super() call, e.g.
+ // super(new FooCommand(), new BarCommand());
+ super(
+ new ShooterHoodCommand(shooterHood, 51),
+ new SpinToPocket(spindexer, 1),
+ new TurretLockOn(turretSubsystem, limelight),
+ new OmniKickUpTimer(spindexer, true, 50),
+ new SpinToPocket(spindexer, 2),
+ new OmniKickUpTimer(spindexer, true, 50),
+ new SpinToPocket(spindexer, 3),
+ new OmniKickUpTimer(spindexer, true, 50),
+ new SpinToPocket(spindexer, 4),
+ new OmniKickUpTimer(spindexer, true, 50),
+ new SpinToPocket(spindexer, 5),
+ new OmniKickUpTimer(spindexer, true, 50),
+ new ShooterHoodCommand(shooterHood, 10)
+
+ );
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/IntakeCommand.java b/src/main/java/org/team696/robot/commands/IntakeCommand.java
new file mode 100644
index 0000000..c5781c2
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/IntakeCommand.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Intake;
+
+public class IntakeCommand extends CommandBase {
+ /**
+ * Creates a new IntakeCom
+ * mand.
+ */
+ private Intake intake;
+
+ private double intakePower;
+ public IntakeCommand(Intake intake, double intakePower) {
+ // Use addRequirements() here to declare subsystem dependencies.
+
+ this.intake = intake;
+ this.intakePower = intakePower;
+ addRequirements(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() {
+ intake.runIntake(intakePower);
+ // spindexer.spindexerLoadingAntiJam(power, current);
+ // System.out.println("loading");
+ }
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java b/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java
new file mode 100644
index 0000000..336c665
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class IntakeTimerCommand extends CommandBase {
+ /**
+ * Creates a new IntakeTimerCommand.
+ */
+ Intake intake;
+ int x = 0;
+ double timer;
+ public IntakeTimerCommand(Intake intake, double timer) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.intake = intake;
+ this.timer = timer;
+ addRequirements(intake);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ x = 0;
+
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ intake.runIntake(0.7);
+ x++;
+ // System.out.println(timer);
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ // timer = 0;
+ intake.runIntake(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return x>timer;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/OmniKickUp.java b/src/main/java/org/team696/robot/commands/OmniKickUp.java
new file mode 100644
index 0000000..49ed7c8
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/OmniKickUp.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Spindexer;
+
+public class OmniKickUp extends CommandBase {
+ /**
+ * Creates a new OmniKickUp.
+ */
+ Spindexer spindexer;
+ double power;
+ public OmniKickUp(Spindexer spindexer, double power) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.spindexer = spindexer;
+ this.power = power;
+ addRequirements(spindexer);
+
+ }
+
+ // 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() {
+ spindexer.setKickMotor(power);
+
+ }
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java b/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java
new file mode 100644
index 0000000..4d0c7a2
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Spindexer;
+
+public class OmniKickUpTimer extends CommandBase {
+ /**
+ * Creates a new OmniKickUp.
+ */
+ Spindexer spindexer;
+ boolean state;
+ int x = 0;
+ double timer;
+
+ public OmniKickUpTimer(Spindexer spindexer, boolean state, double timer) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.spindexer = spindexer;
+ this.state = state;
+
+ this.timer = timer;
+ addRequirements(spindexer);
+
+
+ }
+
+ // 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() {
+ spindexer.setKickMotor(1);
+ System.out.println(x);
+ x++;
+
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ x = 0;
+ System.out.println("done");
+ spindexer.setKickMotor(0);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return x>timer ;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ShooterCommand.java b/src/main/java/org/team696/robot/commands/ShooterCommand.java
new file mode 100644
index 0000000..de47a1b
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ShooterCommand.java
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+import org.team696.robot.RobotContainer;
+import org.team696.robot.Constants.OperatorConstants;
+import org.team696.robot.Constants.ShooterConstants;
+import org.team696.robot.subsystems.Shooter;
+
+public class ShooterCommand extends CommandBase {
+ /**
+ * Creates a new ShooterCommand.
+ */
+ private static final Logger logger = LogManager.getLogger(ShooterCommand.class);
+ private Shooter shooter;
+
+ double RPM;
+ boolean state;
+ public ShooterCommand(Shooter shooter, double RPM, boolean state) {
+
+ this.shooter = shooter;
+ this.state = state;
+ this.RPM = RPM;
+ addRequirements(shooter);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ logger.info(String.format("Spinning up shooter to %f RPM", RPM));
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterAutoButton)){
+ shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(shooter.getShootRPM()));
+ // }
+ // else if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterManualButton)){
+ // // shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM+1000*RobotContainer.operatorPanel.getRawAxis(OperatorConstants.shooterManualAxis)));
+ // shooter.setShooterPower(shooter.shooterRPMToTalonVelocity(ShooterConstants.trenchRunTargetRPM));
+ // }
+ // else{
+ // shooter.setShooterPower(0);
+ // }
+
+ shooter.setAcceleratorPower(state);
+
+ //TODO: make another command to run off of feedforward gains
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ logger.info("Done with shooter command.");
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java b/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java
new file mode 100644
index 0000000..bc2101c
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.RobotContainer;
+import org.team696.robot.Constants.OperatorConstants;
+import org.team696.robot.subsystems.Shooter;
+import org.team696.robot.subsystems.ShooterHood;
+
+public class ShooterHoodCommand extends CommandBase {
+ /**
+ * Creates a new ShooterHoodCommand.
+ */
+ private ShooterHood shooterHood;
+ private double angle;
+
+ private double timer;
+ public ShooterHoodCommand(ShooterHood shooterHood, double angle) {
+
+ this.shooterHood = shooterHood;
+ this.angle = angle;
+ addRequirements(shooterHood);
+
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ timer = 0;
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+
+ timer++;
+ System.out.println("adjusting hood");
+ // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterAutoButton)){
+ shooterHood.moveServoAngle(angle);
+ // }
+ // else if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterManualButton)){
+ // // shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM+1000*RobotContainer.operatorPanel.getRawAxis(OperatorConstants.shooterManualAxis)));
+ // shooterHood.moveServoAngle(shooterHood.shootAngleToServoAngle(18));
+
+ // }
+ // else{
+ // // shooter.setShooterPower(0);
+ // shooterHood.moveServoAngle(shooterHood.shootAngleToServoAngle(40));
+
+ // }
+
+ }
+
+ // 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 timer>150;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java b/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java
new file mode 100644
index 0000000..41fc4c5
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Shooter;
+
+public class ShooterPowerCommand extends CommandBase {
+ /**
+ * Creates a new ShooterPowerCommand.
+ */
+ private Shooter shooter;
+ private double power;
+ private boolean state;
+ public ShooterPowerCommand(Shooter shooter, double power, boolean state) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.shooter = shooter;
+ this.state = state;
+ this.power = power;
+ addRequirements(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() {
+ shooter.setShooterPower(power);
+ shooter.setAcceleratorPower(state);
+ }
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ShooterPrep.java b/src/main/java/org/team696/robot/commands/ShooterPrep.java
new file mode 100644
index 0000000..333bffd
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ShooterPrep.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.Shooter;
+import org.team696.robot.subsystems.TurretSubsystem;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ShooterPrep extends CommandBase {
+
+ //maybe add omni to this command or make it part of the spindexer drum one
+ TurretSubsystem turretSubsystem;
+ Shooter shooter;
+ public ShooterPrep(Shooter shooter, TurretSubsystem turretSubsystem) {
+ this.turretSubsystem = turretSubsystem;
+ this.shooter = shooter;
+ addRequirements(shooter);
+ addRequirements(turretSubsystem);
+ }
+
+ // 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() {
+ Limelight.setLights(3);
+ turretSubsystem.moveToTarget();
+ shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(shooter.getShootRPM()));
+ shooter.setAcceleratorPower(true);
+ //maybe add shooter hood
+ }
+
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java b/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java
new file mode 100644
index 0000000..2dea15f
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Shooter;
+
+public class ShooterTimerCommand extends CommandBase {
+ /**
+ * Creates a new ShooterCommand.
+ */
+ private Shooter shooter;
+
+ double RPM;
+ boolean state;
+ int x = 0;
+ public ShooterTimerCommand(Shooter shooter, double RPM, boolean state) {
+
+ this.shooter = shooter;
+ this.state = state;
+ this.RPM = RPM;
+ addRequirements(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() {
+ shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM));
+ shooter.setAcceleratorPower(state);
+ System.out.println("shooting");
+ x++;
+
+ //TODO: make another command to run off of feedforward gains
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ x = 0;
+ shooter.setShooterPower(0);
+
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return x>150;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/SpinToPocket.java b/src/main/java/org/team696/robot/commands/SpinToPocket.java
new file mode 100644
index 0000000..5ff11af
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/SpinToPocket.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import org.team696.robot.Constants;
+import org.team696.robot.subsystems.Spindexer;
+
+public class SpinToPocket extends CommandBase {
+ /**
+ * Creates a new SpinToIndex.
+ */
+ private static final Logger logger = LogManager.getLogger(ATCForCommand.class);
+ Spindexer spindexer;
+
+ int targetPocket;
+
+ public SpinToPocket(Spindexer spindexer, int targetPocket) {
+ this.spindexer = spindexer;
+ this.targetPocket = targetPocket;
+ addRequirements(spindexer);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ // oldSpindexerPocket = spindexer.getTargetPocket();
+ // int targetPocket;
+ // if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) {
+ // targetPocket = 1;
+ // } else {
+ // targetPocket = spindexer.getTargetPocket() + 1;
+ // }
+ // logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
+ // spindexer.setTargetPocket(targetPocket);
+ spindexer.setTargetPocket(targetPocket);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ //System.out.println("spinning to index stuff");
+ //System.out.println(spindexer.getCurrentPocket());
+
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ if(interrupted){
+ logger.info("Spindexer command interrupted.");
+ } else {
+ logger.info("Spindexer on target.");
+ }
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return spindexer.getCurrentPocket()==targetPocket;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/SpindexerLoading.java b/src/main/java/org/team696/robot/commands/SpindexerLoading.java
new file mode 100644
index 0000000..bad50e9
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/SpindexerLoading.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Intake;
+import org.team696.robot.subsystems.Spindexer;
+
+public class SpindexerLoading extends CommandBase {
+ /**
+ * Creates a new SpindexerLoading.
+ */
+ Spindexer spindexer;
+ Intake intake;
+ double spindexerPower;
+ double intakePower;
+ public SpindexerLoading(Spindexer spindexer, Intake intake, double spindexerPower, double intakePower) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.spindexerPower = spindexerPower;
+ this.intakePower = intakePower;
+ this.spindexer = spindexer;
+ this.intake = intake;
+ addRequirements(spindexer);
+ addRequirements(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() {
+ spindexer.spindexerLoadingAntiJam(spindexerPower);
+ intake.runIntake(intakePower);
+ // System.out.println("running the loading stuff");
+ }
+
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/StopTurret.java b/src/main/java/org/team696/robot/commands/StopTurret.java
new file mode 100644
index 0000000..ef0a34a
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/StopTurret.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.TurretSubsystem;
+
+public class StopTurret extends CommandBase {
+ /**
+ * Creates a new StopTurret.
+ */
+ private TurretSubsystem turretSubsystem;
+
+ public StopTurret(TurretSubsystem turretSubsystem) {
+ // Use addRequirements() here to declare subsystem dependencies.
+ this.turretSubsystem = turretSubsystem;
+ addRequirements(turretSubsystem);
+ }
+
+ // 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() {
+ turretSubsystem.openLoop(0);
+ }
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/TurretLockOn.java b/src/main/java/org/team696/robot/commands/TurretLockOn.java
new file mode 100644
index 0000000..abd8a31
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/TurretLockOn.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.TurretSubsystem;
+
+public class TurretLockOn extends CommandBase {
+ /**
+ * Creates a new TurretLockOn.
+ */
+
+ TurretSubsystem turretSubsystem;
+ Limelight limelight;
+ public TurretLockOn(TurretSubsystem turretSubsystem, Limelight limelight) {
+ this.turretSubsystem = turretSubsystem;
+ this.limelight = limelight;
+ addRequirements(turretSubsystem);
+ addRequirements(limelight);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ limelight.setLights(3);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ // turretSubsystem.openLoop(0.05);
+ turretSubsystem.moveToTarget();
+ }
+
+ // 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 turretSubsystem.onTarget();
+ return false;
+ }
+}
diff --git a/src/main/java/org/team696/robot/commands/TurretManual.java b/src/main/java/org/team696/robot/commands/TurretManual.java
new file mode 100644
index 0000000..a9cbf90
--- /dev/null
+++ b/src/main/java/org/team696/robot/commands/TurretManual.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.team696.robot.commands;
+
+import org.team696.robot.RobotContainer;
+import org.team696.robot.Constants.TurretConstants;
+import org.team696.robot.subsystems.Limelight;
+import org.team696.robot.subsystems.TurretSubsystem;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TurretManual extends CommandBase {
+ /**
+ * Creates a new TurretManual.
+ */
+ TurretSubsystem turretSubsystem;
+ Limelight limelight;
+ double position;
+ public TurretManual(TurretSubsystem turretSubsystem, Limelight limelight) {
+ // Use addRequirements() here to declare subsystem dependencies.\
+ this.turretSubsystem = turretSubsystem;
+ this.limelight = limelight;
+ this.position = position;
+ addRequirements(turretSubsystem);
+ addRequirements(limelight);
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ // limelight.setLights(1);
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ // turretSubsystem.moveToPosition(TurretConstants.turretCenterPos+position*100);
+ // System.out.println("turret manual");
+ // System.out.println(position);
+ if(RobotContainer.operatorPanel.getRawAxis(0)>0.05){
+ limelight.setLights(1);
+ }
+ else{
+ limelight.setLights(1);
+ }
+ turretSubsystem.openLoop(-RobotContainer.operatorPanel.getRawAxis(0));
+ }
+
+ // 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;
+ }
+}
diff --git a/src/main/java/org/team696/robot/subsystems/BallSensors.java b/src/main/java/org/team696/robot/subsystems/BallSensors.java
new file mode 100644
index 0000000..018d21c
--- /dev/null
+++ b/src/main/java/org/team696/robot/subsystems/BallSensors.java
@@ -0,0 +1,138 @@
+package org.team696.robot.subsystems;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+
+import org.apache.logging.log4j.Logger;
+import org.apache.logging.log4j.LogManager;
+
+import org.team696.TCA9548ADriver.TCA9548A;
+import org.team696.TCA9548ADriver.TCA9548ADevice;
+import org.team696.TCS34725.Gain;
+import org.team696.TCS34725.IntegrationTime;
+import org.team696.TCS34725.TCS34725;
+import org.team696.robot.Constants.SpindexerConstants;
+import org.team696.utils.ColorUtils.Color;
+
+public class BallSensors {
+ private int num_sensors;
+ private TCS34725[] sensors;
+ private static final Logger logger = LogManager.getLogger(TCA9548ADevice.class);
+
+ /**
+ * Constructor that assumes a mux with the address given in Constants and sensors connected in order.
+ */
+ public BallSensors() {
+ num_sensors = SpindexerConstants.nPockets;
+ sensors = new TCS34725[num_sensors];
+ byte muxaddr = SpindexerConstants.ColorSensorsMuxAddress;
+ TCA9548A mux = new TCA9548A(Port.kOnboard, muxaddr);
+ int i;
+ for (i = 0; i < num_sensors; i++) {
+ TCA9548ADevice thisMuxChannel = new TCA9548ADevice(mux, TCS34725.I2C_constants.TCS34725_I2C_ADDR, i);
+ TCS34725 thisSensor = new TCS34725(thisMuxChannel);
+ sensors[i] = thisSensor;
+ }
+ }
+
+ /**
+ * Constructor that takes premade I2C channels.
+ * @param channels I2C channels (should be num_sensors long)
+ */
+ public BallSensors(I2C[] channels) {
+ num_sensors = SpindexerConstants.nPockets;
+ sensors = new TCS34725[num_sensors];
+ int i;
+ for (i = 0; i < num_sensors; i++) {
+ TCS34725 thisSensor = new TCS34725(channels[i]);
+ sensors[i] = thisSensor;
+ }
+ }
+
+ /**
+ * Constructor that takes a mux and the indices of the mux channels going to the sensors.
+ * @param mux A TCA9548A mux object
+ * @param muxChannels The mux channels to use (should be num_sensors long)
+ */
+ public BallSensors(TCA9548A mux, byte[] muxChannels) {
+ num_sensors = SpindexerConstants.nPockets;
+ sensors = new TCS34725[num_sensors];
+ int i;
+ for (i = 0; i < num_sensors; i++) {
+ TCA9548ADevice thisMuxChannel = new TCA9548ADevice(mux, TCS34725.I2C_constants.TCS34725_I2C_ADDR, muxChannels[i]);
+ TCS34725 thisSensor = new TCS34725(thisMuxChannel);
+ thisSensor.setName(String.format("Pocket %d", i));
+ sensors[i] = thisSensor;
+ }
+ }
+
+ /**
+ * Calls init() on all the sensors.
+ */
+ public void initSensors() {
+ for (TCS34725 sensor : sensors) {
+ sensor.init(IntegrationTime.IT_24MS, Gain.X1);
+ }
+ }
+
+
+ public double[] getColor(int idx){
+ double[] retval = new double[3];
+ TCS34725 sensor = sensors[idx];
+ if(sensor.readColors() == -1){
+ sensor.init(IntegrationTime.IT_24MS, Gain.X1);
+ }
+ retval[0] = sensor.getRedVal();
+ retval[1] = sensor.getGreenVal();
+ retval[2] = sensor.getBlueVal();
+ return retval;
+ }
+ /**
+ * Gets whether each sensor sees a ball.
+ * For each sensor, commands a reading, then compares the red, green,
+ * blue, and clear values to the limits in Constants to see if a ball
+ * is there.
+ * @return An array of num_sensors booleans, each indicating if that sensor sees a ball
+ */
+ public boolean[] getResults() {
+ boolean[] results = new boolean[num_sensors];
+ int i;
+ for (i = 0; i < num_sensors; i++) {
+ TCS34725 sensor = sensors[i];
+ if(sensor.readColors() == -1){
+ sensor.init(IntegrationTime.IT_24MS, Gain.X1);
+ }
+ //TODO: figure out how to scale colors
+ double red = sensor.getRedVal();
+ double green = sensor.getGreenVal();
+ double blue = sensor.getBlueVal();
+ Color color = new Color(red, green, blue);
+ double[] hsv = color.getHSV();
+ boolean hasBall = true;
+ if (hsv[0] < SpindexerConstants.HueMin || hsv[0] > SpindexerConstants.HueMax) {
+ hasBall = false;
+ }
+ if (hsv[1] < SpindexerConstants.SatMin || hsv[1] > SpindexerConstants.SatMax) {
+ hasBall = false;
+ }
+ if (hsv[2] < SpindexerConstants.ValueMin || hsv[2] > SpindexerConstants.ValueMax) {
+ hasBall = false;
+ }
+ results[i] = hasBall;
+ }
+
+ return results;
+ }
+
+ public void setLEDs(boolean state){
+ int i;
+ for(i=0; i It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ /** Organizes all the non-changing values we use in the code */
+ public static final int leftFrontPort = 10;
+ public static final int leftBackPort = 11;
+ public static final int rightFrontPort = 12;
+ public static final int rightBackPort = 13;
+
+}
diff --git a/src/main/java/org/team696/robot/Main.java b/src/main/java/frc/robot/Main.java
similarity index 50%
rename from src/main/java/org/team696/robot/Main.java
rename to src/main/java/frc/robot/Main.java
index a961ae0..8776e5d 100644
--- a/src/main/java/org/team696/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-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. */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
-package org.team696.robot;
+package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
@@ -15,8 +12,7 @@
* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
new file mode 100644
index 0000000..885590d
--- /dev/null
+++ b/src/main/java/frc/robot/Robot.java
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.DriveTrain;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ /** You can leave this in if you want, it only changes anything if you are using the autonomous part of the code,
+ * if you do remove it, just delete any errors it creates */
+ private Command m_autonomousCommand;
+
+ /**Creates an object for our DriveTrain class so we can call things from the class
+ * make sure to include any variables needed for the parameters
+ */
+ public static DriveTrain m_driveTrain = new DriveTrain(Constants.leftFrontPort, Constants.leftBackPort, Constants.rightFrontPort, Constants.rightBackPort);
+ /** Similar to the previous line, creates a RobotContainer object so we can call things from the robotcontainer class,
+ * what you name this is up to you */
+ private RobotContainer m_robotContainer;
+
+
+ /** The values that we will use from the joystick */
+ double xAxis;
+ double yAxis;
+ /** Values that will go in the parameters that are needed for the RobotDrive method */
+ double leftSpeed;
+ double rightSpeed;
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // autonomous chooser on the dashboard.
+ m_robotContainer = new RobotContainer();
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * This runs after the mode specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+ CommandScheduler.getInstance().run();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {}
+
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ @Override
+ public void teleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {
+ /** Defining the XAxis and yAxis objects as the x and y values from our joysticks on the controller */
+ xAxis = -m_robotContainer.xboxController.getRawAxis(1);
+ yAxis = m_robotContainer.xboxController.getRawAxis(4);
+ /** The math required to make the robot turn left and right properly according to the joystick configuration. */
+ leftSpeed = yAxis + xAxis;
+ rightSpeed = yAxis - xAxis;
+ /** Calling back the robotDrive method from the DriveTrain class so that it runs fifty times a second
+ * uses the parameters leftSpeed and rightSpeed that we just defined
+ */
+ m_driveTrain.robotDrive(leftSpeed, rightSpeed);
+
+ }
+
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
new file mode 100644
index 0000000..44489ff
--- /dev/null
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ // The robot's subsystems and commands are defined here...
+ /** Declaring and defining our joystick object */
+ public static Joystick xboxController = new Joystick(1);
+
+ /** The container for the robot. Contains subsystems, OI devices, and commands. */
+ public RobotContainer() {
+ // Configure the button bindings
+ configureButtonBindings();
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be created by
+ * instantiating a {@link GenericHID} or one of its subclasses ({@link
+ * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+ * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {}
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+
+}
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
new file mode 100644
index 0000000..ba233bf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+/** YOU DO NOT HAVE TO WORRY ABOUT THIS SECTION, USING TAB WILL ADD ANYTHING NEEDED HERE AUTOMATICALLY */
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveTrain extends SubsystemBase {
+ /** Creates a new DriveTrain. */
+ /** Creates a differential drive which allows the two speed controller groups to work together */
+ DifferentialDrive diffDrive;
+ /**Creates a speed controller group which allows you to run multiple motors with one object */
+ SpeedControllerGroup leftSide;
+ SpeedControllerGroup rightSide;
+ /**Creates the objects for each motor controller */
+ CANSparkMax leftFront;
+ CANSparkMax leftBack;
+ CANSparkMax rightFront;
+ CANSparkMax rightBack;
+
+
+ public DriveTrain(int leftFrontPort, int leftBackPort, int rightFrontPort, int rightBackPort) {
+ /** Defining all the motor controllers with the CANSparkMax(port, motor type) method */
+ leftFront = new CANSparkMax(leftFrontPort, MotorType.kBrushless);
+ leftBack = new CANSparkMax(leftBackPort, MotorType.kBrushless);
+ rightBack = new CANSparkMax(rightBackPort, MotorType.kBrushless);
+ rightFront = new CANSparkMax(rightFrontPort, MotorType.kBrushless);
+ /**Defining the two speed controller groups using the SpeedControllerGroup(motor, motor) method */
+ leftSide = new SpeedControllerGroup(leftFront, leftBack);
+ rightSide = new SpeedControllerGroup(rightFront, rightBack);
+
+ /**Defining the differential drive object using the DifferentialDrive(speedcontrollergroup, speedcontrollergroup) method */
+ diffDrive = new DifferentialDrive(leftSide, rightSide);
+ }
+
+ /**Method we will use to actually control the robot, which need the parameters leftSpeed and rightSpeed, which will be defined in robot */
+ public void robotDrive(double leftSpeed, double rightSpeed){
+ /** A method tied to the DifferentialDrive object in the Wpilib libraries, basically lets us control the motors in a tank drive fashion */
+ diffDrive.tankDrive(leftSpeed, rightSpeed);
+ /** Creates a minimum required output for the joystick to run the motor */
+ diffDrive.setDeadband(0.1);
+ }
+
+ @Override
+ public void periodic() {
+ // This method will be called once per scheduler run (50 times a second)
+ }
+}
diff --git a/src/main/java/org/team696/robot/Constants.java b/src/main/java/org/team696/robot/Constants.java
deleted file mode 100644
index 4016734..0000000
--- a/src/main/java/org/team696/robot/Constants.java
+++ /dev/null
@@ -1,229 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-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 org.team696.robot;
-
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide
- * numerical or boolean constants. This class should not be used for any other
- * purpose. All constants should be declared globally (i.e. public static). Do
- * not put anything functional in this class.
- *
- *
- * It is advised to statically import this class (or one of its inner classes)
- * wherever the constants are needed, to reduce verbosity.
- */
-public final class Constants {
- public static final int CANtimeout = 10;
-
- public static final int timeBetweenPockets = 50;
-
- public static final class SpindexerConstants {
- public static final int nPockets = 5;
- public static final int MotorCANID = 20;
- public static final int EncoderPort = 0;
- public static final double P = 0.08;
- public static final double closedLoopRampRate = 0.5;
- public static final double openLoopRampRate = 0.25;
- public static final int RevTBEMinMicroseconds = 1;
- public static final int RevTBEMaxMicroseconds = 1024;
- // public static final double[] PocketPositions = {0.9335, 0.1398, 0.3431,
- // 0.5386, 0.7351};
- // public static final double[] PocketPositions = {0.9689, 0.1689, 0.3689,
- // 0.5689, 0.7689};
- public static final double[] PocketPositions = { 0.964, 0.173, 0.375, 0.580, 0.775 };
- public static final double PositionTolerance = 0.005; // Used to calcuate isOnTarget
- public static final double VelocityTolerance = 0.01; // used to calculate isMoving
- public static final double MotorTurnsPerSpindexerTurn = 62.5;
- public static final byte ColorSensorsMuxAddress = 0x70;
- public static final byte ColorSensor1MuxChannel = 0;
- public static final byte ColorSensor2MuxChannel = 1;
- public static final byte ColorSensor3MuxChannel = 2;
- public static final byte ColorSensor4MuxChannel = 3;
- public static final byte ColorSensor5MuxChannel = 7;
-
- public static final double HueMax = 1;
- public static final double HueMin = 0;
- public static final double SatMax = 1;
- public static final double SatMin = 0;
- public static final double ValueMax = 1;
- public static final double ValueMin = 0;
-
- public static final int KickMotorCANID = 21;
- public static final double KickMotorSpeed = 1.;
- public static final double KickMotorReverseSpeed = -0.1;
-
- public static final double loadingDrumPower = 0.22;
- public static final double continuousShootDrumPower = 0.55;
- public static final double stopDrumPower = 0;
-
- public static final double timeBetweenIndex = 1;
-
- public static final double indexingJamDetectionCurrent = 15;
- public static final double loadingJamDetectionCurrent = 10;
- public static final double antiJamTimeout = 50;
- }
-
- public static final class ShooterConstants {
- public static final int leftShooterHoodServo = 2;
- public static final int rightShooterHoodServo = 3;
-
- public static final int leftShooterPort = 42;
- public static final boolean leftShooterInverted = false;
- public static final boolean leftShooterSensorPhase = false;
- public static final int rightShooterPort = 43;
- public static final boolean rightShooterInverted = true;
- public static final boolean rightShooterSensorPhase = false;
- public static final int pidSlot = 0;
- public static double shooterkP = 0.6;
- public static double shooterkI = 0.0;
- public static double shooterkD = 0.0;
- public static double shooterkF = 0.06;
- public static final int allowableShooterError = 10;
-
- // TODO: run simple motor characterization routine to deterine feedforward gains
- public static final double shooterkSGain = 0;
- public static final double shooterkVGain = 0;
- public static final double shooterkAGain = 0;
-
- public static final double shooterHoodAngleMaxLimit = 115;
- public static final double shooterHoodAngleMinLimit = 175;
-
- public static final int acceleratorPort = 41;
- public static final double acceleratorPower = 0.3;
- public static double acceleratorkP = 0;
- public static double acceleratorkI = 0;
- public static double acceleratorkD = 0;
- public static double acceleratorkF = 0;
- public static final int allowableAcceleratorError = 100;
- public static final boolean acceleratorInverted = true;
- public static final boolean acceleratorSensorPhase = false;
-
- public static final double shooterGearRatio = 2.5;
-
- public static final double autoLineTargetRPM = 3900;
- public static final double autoLineTargetAngle = 30;
-
- public static final double trenchRunTargetRPM = 5742.27;
- public static final double trenchRunTargetAngle = 18.6;
-
- }
-
- public static final class TurretConstants {
- public static final int turretMotorPort = 40;
- // public static final boolean turret
-
- public static final boolean turretMotorInverted = false;
- public static final boolean turretMotorSensorPhase = true;
-
- public static final int forwardSoftLimit = -370;
- public static final int reverseSoftLimit = -570;
-
- public static final int turretCenterPos = -540;
-
- public static final double positionTolerance = 0.6;
-
- public static final double peakOutput = 0.75;
-
- public static final double kP = 0.05;
- public static final double kI = 0.0;
- public static final double kD = 0.001;
- }
-
-
- public static final class LimelightConstants {
- public static final int camTranPipeline = 1;
- public static final int loadingpipeline = 1;
- }
-
- public static final class OperatorConstants {
- public static final int operatorPanelPort = 0;
- public static final int driverJoystickPort = 1;
-
- // Driver gamepad mappings
- public static final int driveModeButton = 4;
-
- // Operator panel mappings
- public static final int ATCRevButton = 1;
- public static final int intakeOnButton = 3;
- public static final int intakeOffButton = 4;
- public static final int intakeOutButton = 5;
- public static final int fitUnderTrenchButton = 6;
- public static final int intakeInButton = 6;
- public static final int ATCForButton = 7;
- public static final int fireButton = 8;
- public static final int hoodAutoButton = 9;
- public static final int shooterAutoButton = 10;
- public static final int colorWheelPositionControl = 11;
- public static final int climberUpButton = 12;
- public static final int colorWheelDeploy = 13;
- public static final int turretAutoButton = 14;
- public static final int shooterManualButton = 15;
- public static final int spinUpButton = 16;
- public static final int climberDownButton = 17;
- public static final int colorWheelRotationControl = 18;
- public static final int driveAssistButton = 19;
-
- public static final int shooterManualAxis = 3;
- public static final int turretManualAxis = 2;
- public static final int hoodManualAxis = 1;
-
- public static final int ATCRevLED = 0;
- public static final int limelightLockLED = 1;
- public static final int limelightCaptureLED = 2;
- public static final int ATCForLED = 3;
- public static final int fireLED = 4;
- public static final int realsenseCaptureLED = 5;
- public static final int realsenseLockLED = 6;
- public static final int carabinerLatchLED = 7;
- public static final int climberUpLimLED = 8;
- public static final int climberDownLimLED = 9;
- public static final int colorWheelDeployLED = 10;
- }
-
- public static final class DrivetrainConstants {
- public static final int leftFrontCANID = 10;
- public static final int leftRearCANID = 11;
- public static final int rightFrontCANID = 12;
- public static final int rightRearCANID = 13;
-
- public static final boolean leftFrontIsInverted = true;
- public static final boolean leftRearIsInverted = true;
- public static final boolean rightFrontIsInverted = false;
- public static final boolean rightRearIsInverted = false;
-
- public static final double wheelDiameter = 6.; // In inches. Used for calculating speed.
- public static final double driveGearboxReduction = (60. / 10.) * (30. / 18.);
-
- public static final int IMUCalTime = 8;
- public static final double yawkP = 1.;
- public static final double yawkI = 0.;
- public static final double yawkD = 0.;
-
- public static final double maxSpeed = 184.6; // In in/s
- public static final double maxTurnRate = 5; // In degrees/iteration
-
- public static final int stallCurrentLimit = 80;
- public static final int freeCurrentLimit = 80;
- public static final int stallThresholdRPM = 1000;
-
- public static final double openLoopRampRate = 0.5;
-
- public static final double driveControllerDeadband = 0.05;
-
- public static final int slowTasksSpeed = 50;
- }
-
- public static final class IntakeConstants {
- public static final int IntakeMotorPort = 50;
- public static final boolean IntakeInverted = false;
-
- public static final double intakePower = 0.7;
- public static final double outtakePower = -1;
- public static final double stopIntakePower = 0;
- }
-}
diff --git a/src/main/java/org/team696/robot/Robot.java b/src/main/java/org/team696/robot/Robot.java
deleted file mode 100644
index f869600..0000000
--- a/src/main/java/org/team696/robot/Robot.java
+++ /dev/null
@@ -1,202 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-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 org.team696.robot;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-import org.team696.robot.Constants.OperatorConstants;
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.ShooterHood;
-
-
-/**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
- * project.
- */
-public class Robot extends TimedRobot {
- private static final Logger logger = LogManager.getLogger(Robot.class);
- public static RobotContainer m_robotContainer;
- public static OperatorConstants m_robotConstants;
- private Command m_autonomousCommand;
- double last_hood_axis = -69.23; //arbitrary value for initialization later
-
- // public static double shootAngle;
-
-
- /**
- * This function is run when the robot is first started up and should be used for any
- * initialization code.
- */
- @Override
- public void robotInit() {
- logger.info("Starting up...");
- m_robotContainer = new RobotContainer();
- m_robotConstants = new OperatorConstants();
- }
-
- /**
- * This function is called every robot packet, no matter the mode. Use this for items like
- * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
- *
- * This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
- // commands, running already-scheduled commands, removing finished or interrupted commands,
- // and running subsystem periodic() methods. This must be called from the robot's periodic
- // block in order for anything in the Command-based framework to work.
- CommandScheduler.getInstance().run();
-
- RobotContainer.limelight.pipeline(1);
- SmartDashboard.putBoolean("isOnTarget", m_robotContainer.turretSubsystem.onTarget());
- SmartDashboard.putNumber("Current RPM", m_robotContainer.shooter.getRPM());
- SmartDashboard.putNumber("Shoot Angle", m_robotContainer.shooterHood.shootAngleToServoAngle(m_robotContainer.shooterHood.servoAngle()));
- SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128));
- SmartDashboard.putNumber("Shoot Angle", m_robotContainer.shooterHood.servoAngleToShootAngle(m_robotContainer.shooterHood.servoAngle()));
- SmartDashboard.putBoolean("Up to Speed", m_robotContainer.shooter.isUpToSpeed());
- SmartDashboard.putNumber("Spindexer Current", m_robotContainer.spindexer.getCurrent());
- SmartDashboard.putNumber("Hood Angle", m_robotContainer.shooterHood.servoAngle());
- SmartDashboard.putNumber("Accelerator Velocity", m_robotContainer.shooter.getAcceleratorVelocity());
-
-
- // SmartDashboard.putNumber("spin pos", m_robotContainer.spindexer.getEncoderPosition());
- // SmartDashboard.putNumber("servo angle", m_robotContainer.shooterHood.servoAngle());
- // SmartDashboard.putNumber("Number of Balls", m_robotContainer.spindexer.getNumberOfFilledPockets());
- // SmartDashboard.putNumber("distance", Limelight.distanceFromTarget());
- // SmartDashboard.putNumber("shooter power", m_robotContainer.shooter.getShooterPower());
- // SmartDashboard.putNumber("index", Limelight.indexForDistance());
- // SmartDashboard.putNumber("AUTORPM", m_robotContainer.shooter.getAutoRPM());
- // SmartDashboard.putNumber("AUTOANGLE", m_robotContainer.shooter.getAutoAngle());
- // SmartDashboard.putNumber("SERVOAUTOANGLE", m_robotContainer.shooterHood.shootAngleToServoAngle(m_robotContainer.shooter.getAutoAngle()));
- // SmartDashboard.putNumber("AngleDistance", Limelight.angleDistance());
- // SmartDashboard.putNumber("Intake Current", m_robotContainer.intake.intakeCurrent());
- // RobotContainer.limelight.setLights(3);
- // SmartDashboard.putNumber("Roll", m_robotContainer.drivetrain.roll());
-
- }
-
-
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
- @Override
- public void disabledInit() {
- logger.info("Transitioning to disabled.");
- m_robotContainer.spindexer.setBrake(false);
- // RobotContainer.limelight.setLights(3);
- }
-
- @Override
- public void disabledPeriodic() {
- Limelight.setLights(1);
- }
-
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
- @Override
- public void autonomousInit() {
- logger.info("Transitioning to autonomous.");
- m_robotContainer.spindexer.setBrake(true);
- m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
- // schedule the autonomous command (example)
- if (m_autonomousCommand != null) {
- m_autonomousCommand.schedule();
- }
-
- }
-
- /**
- * This function is called periodically during autonomous.
- */
- @Override
- public void autonomousPeriodic() {
-
-
- }
-
- @Override
- public void teleopInit() {
- logger.info("Transitioning to teleop.");
- m_robotContainer.spindexer.setBrake(true);
- // SmartDashboard.putNumber("Target RPM", 3900);
- // SmartDashboard.putNumber("Target Angle", 30);
-
- }
-
- /**
- * This function is called periodically during operator control.
- */
-
-
- @Override
- public void teleopPeriodic() {
- if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.hoodAutoButton)){
- //Previous comp: m_robotContainer.shooterHood.moveServoAngle(51);
- m_robotContainer.shooterHood.moveServoAngle(51);
-
-
-
- }
- else{
- double hood_axis = -Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128*1.5);
- if (last_hood_axis == -69.23){
- last_hood_axis = hood_axis;
- }
- double delta = hood_axis - last_hood_axis;
-
- SmartDashboard.putNumber("Hood Axis", Math.round(m_robotContainer.operatorPanel.getRawAxis(1)*128));
-
- if (delta + m_robotContainer.shooterHood.servoAngle() <= 51 && delta + m_robotContainer.shooterHood.servoAngle() >= 10){
- m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
-
- /*
- if (m_robotContainer.shooterHood.servoAngle() >= 51 && delta < 0){
- m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
- }
- else if (m_robotContainer.shooterHood.servoAngle() <= 10 && delta > 0){
- m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
- }
-
- if (m_robotContainer.shooterHood.servoAngle() < 51 && m_robotContainer.shooterHood.servoAngle() > 10){
- m_robotContainer.shooterHood.moveServoAngle(m_robotContainer.shooterHood.servoAngle()+delta);
- }
- */
- }
- last_hood_axis = hood_axis;
-
-
- }
- // }
-
-
- }
-
- @Override
- public void testInit() {
- // Cancels all running commands at the start of test mode.
- CommandScheduler.getInstance().cancelAll();
- }
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- }
-}
diff --git a/src/main/java/org/team696/robot/RobotContainer.java b/src/main/java/org/team696/robot/RobotContainer.java
deleted file mode 100644
index 7375cd7..0000000
--- a/src/main/java/org/team696/robot/RobotContainer.java
+++ /dev/null
@@ -1,229 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-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 org.team696.robot;
-
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-
-import org.team696.MathUtils.MathUtils;
-import org.team696.robot.Constants.IntakeConstants;
-import org.team696.robot.Constants.OperatorConstants;
-import org.team696.robot.Constants.SpindexerConstants;
-import org.team696.robot.commands.ATCForCommand;
-import org.team696.robot.commands.ATCRevCommand;
-import org.team696.robot.commands.AutoIndexKickUp;
-import org.team696.robot.commands.Drive;
-import org.team696.robot.commands.DriveTimer;
-import org.team696.robot.commands.FireCommand;
-import org.team696.robot.commands.IntakeTimerCommand;
-import org.team696.robot.commands.OmniKickUp;
-import org.team696.robot.commands.OmniKickUpTimer;
-import org.team696.robot.commands.ShooterCommand;
-import org.team696.robot.commands.ShooterHoodCommand;
-import org.team696.robot.commands.ShooterPowerCommand;
-import org.team696.robot.commands.ShooterPrep;
-import org.team696.robot.commands.SpinToPocket;
-import org.team696.robot.commands.SpindexerLoading;
-import org.team696.robot.commands.TurretLockOn;
-import org.team696.robot.commands.TurretManual;
-import org.team696.robot.subsystems.Drivetrain;
-import org.team696.robot.subsystems.Intake;
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.Shooter;
-import org.team696.robot.subsystems.ShooterHood;
-import org.team696.robot.subsystems.Spindexer;
-import org.team696.robot.subsystems.TurretSubsystem;
-
-/**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a "declarative" paradigm, very little robot logic should
- * actually be handled in the {@link Robot} periodic methods (other than the
- * scheduler calls). Instead, the structure of the robot (including subsystems,
- * commands, and button mappings) should be declared here.
- */
-public class RobotContainer {
- // Subystem declarations
- private static final Logger logger = LogManager.getLogger(RobotContainer.class);
- public final Drivetrain drivetrain = new Drivetrain();
- public final Spindexer spindexer = new Spindexer();
- public final Shooter shooter = new Shooter();
- public final ShooterHood shooterHood = new ShooterHood();
- public final TurretSubsystem turretSubsystem = new TurretSubsystem();
- public static final Limelight limelight = new Limelight();
- public final Intake intake = new Intake();
-
- // Joysticks
- public final XboxController driverController = new XboxController(OperatorConstants.driverJoystickPort);
- public static final Joystick operatorPanel = new Joystick(OperatorConstants.operatorPanelPort);
-
- // Buttons
-
- // make hood/shooter auto/man to change from 2 distances
- // make intake in/out be for color wheel height
- private final JoystickButton driveMode = new JoystickButton(driverController, OperatorConstants.driveModeButton);
-
- private final JoystickButton spinUpButton = new JoystickButton(operatorPanel, OperatorConstants.spinUpButton);
- private final JoystickButton shooterAutoButton = new JoystickButton(operatorPanel, OperatorConstants.shooterAutoButton);
- private final JoystickButton shooterManualButton = new JoystickButton(operatorPanel, OperatorConstants.shooterManualButton);
- private final JoystickButton fireButton = new JoystickButton(operatorPanel, OperatorConstants.fireButton);
- private final JoystickButton ATCForButton = new JoystickButton(operatorPanel, OperatorConstants.ATCForButton);
- private final JoystickButton ATCRevButton = new JoystickButton(operatorPanel, OperatorConstants.ATCRevButton);
-
- private final JoystickButton turretAutoButton = new JoystickButton(operatorPanel, OperatorConstants.turretAutoButton);
- private final JoystickButton hoodAutoButton = new JoystickButton(operatorPanel, OperatorConstants.hoodAutoButton);
-
- private final JoystickButton intakeOnButton = new JoystickButton(operatorPanel, OperatorConstants.intakeOnButton);
- private final JoystickButton intakeOffButton = new JoystickButton(operatorPanel, OperatorConstants.intakeOffButton);
-
- private final JoystickButton fitUnderTrenchButton = new JoystickButton(operatorPanel, OperatorConstants.fitUnderTrenchButton);
-
- private final JoystickButton continuousButton = new JoystickButton(operatorPanel, OperatorConstants.colorWheelDeploy);
- private final JoystickButton omniReverse = new JoystickButton(operatorPanel, OperatorConstants.colorWheelRotationControl);
- private final JoystickButton automaticShootPrep = new JoystickButton(operatorPanel, OperatorConstants.driveAssistButton);
- private final JoystickButton automaticIndex = new JoystickButton(operatorPanel, OperatorConstants.colorWheelPositionControl);
-
- public double getDriveSpeed() {
- // Math to correct for nonlinearities in the controller should happen here.
- double jsVal = -driverController.getY(Hand.kLeft);
- if (jsVal > 0) {
- return MathUtils.deadBand(jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
- } else {
- return -MathUtils.deadBand(-jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
- }
- }
-
- public double getDriveTurn() {
- // Math to correct for nonlinearities in the controller should happen here.
- double jsVal = -driverController.getX(Hand.kRight);
- if (jsVal > 0) {
- return MathUtils.deadBand(jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
- } else {
- return -MathUtils.deadBand(-jsVal, Constants.DrivetrainConstants.driveControllerDeadband);
- }
- }
-
- public double turretManual() {
- return operatorPanel.getRawAxis(1);
- }
-
- public RobotContainer() {
- Command driveCommand = new Drive(() -> getDriveSpeed(), () -> getDriveTurn(), drivetrain);
- drivetrain.setDefaultCommand(driveCommand);
- configureButtonBindings();
- }
-
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
- private void configureButtonBindings() {
- // rpm is 3900
- spinUpButton.whenPressed(new ShooterCommand(shooter, shooter.shootRPM, true));
- spinUpButton.whenReleased(new ShooterPowerCommand(shooter, 0, false));
-
- turretAutoButton.whileHeld(new TurretLockOn(turretSubsystem, limelight));
- turretAutoButton.whenReleased(new TurretManual(turretSubsystem, limelight));
-
- SpindexerLoading loadingOff = new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.stopIntakePower);
- intakeOnButton.whileHeld(
- new SpindexerLoading(spindexer, intake, SpindexerConstants.loadingDrumPower, IntakeConstants.intakePower));
- intakeOnButton.whenReleased(loadingOff);
- intakeOffButton.whileHeld(
- new SpindexerLoading(spindexer, intake, SpindexerConstants.stopDrumPower, IntakeConstants.outtakePower));
- intakeOffButton.whenReleased(loadingOff);
-
- // maybe have this button cancel the human player loading sequence
- continuousButton.whileHeld(new SpindexerLoading(spindexer, intake, SpindexerConstants.continuousShootDrumPower,
- IntakeConstants.stopIntakePower));
- continuousButton.whenReleased(loadingOff);
-
- automaticShootPrep.whenPressed(new ShooterPrep(shooter, turretSubsystem));
- automaticShootPrep.whenReleased(new ShooterPowerCommand(shooter, 0, false));
- automaticShootPrep.whenReleased(new TurretManual(turretSubsystem, limelight));
-
- automaticIndex.whenPressed(new AutoIndexKickUp(spindexer, intake, SpindexerConstants.continuousShootDrumPower, 0));
- automaticIndex.whenReleased(loadingOff);
-
- fireButton.whenPressed(new FireCommand(shooter, spindexer, SpindexerConstants.KickMotorSpeed));
- fireButton.whenReleased(new FireCommand(shooter, spindexer, SpindexerConstants.stopDrumPower));
-
- OmniKickUp reverseOmni = new OmniKickUp(spindexer, SpindexerConstants.KickMotorReverseSpeed);
- omniReverse.whenPressed(reverseOmni);
- omniReverse.whenReleased(new OmniKickUp(spindexer, SpindexerConstants.stopDrumPower));
-
- ATCForCommand ATCfor = new ATCForCommand(spindexer);
- ATCRevCommand ATCrev = new ATCRevCommand(spindexer);
-
- ATCForButton.whenPressed(ATCfor);
- ATCForButton.cancelWhenPressed(ATCrev);
-
- ATCRevButton.whenPressed(ATCrev);
- ATCRevButton.cancelWhenPressed(ATCfor);
-
- intakeOnButton.whenReleased(ATCfor);
-
- }
-
- /**
- * Use this to pass the autonomous command to the main {@link Robot} class.
- *
- * @return the command to run in autonomous
- */
- @SuppressWarnings("checkstyle:magicnumber")
- public Command getAutonomousCommand() {
-
- return new SequentialCommandGroup(
-
- new ParallelCommandGroup(
- new TurretLockOn(turretSubsystem, limelight),
- new ShooterCommand(shooter, 3900, true),
-
- new SequentialCommandGroup(
- new ShooterHoodCommand(shooterHood, 51),
- // new IntakeTimerCommand(intake, 50);
- new SpinToPocket(spindexer, 1),
- new OmniKickUpTimer(spindexer, true, 25),
- new SpinToPocket(spindexer, 2),
- new OmniKickUpTimer(spindexer, true, 25),
- new SpinToPocket(spindexer, 3),
- new OmniKickUpTimer(spindexer, true, 25)
- )
- ),
-
- new DriveTimer(drivetrain, -0.5, 0, 50),
- new DriveTimer(drivetrain, 0, 0.7, 40),
- new DriveTimer(drivetrain, 0.5, 0, 25),
- new ParallelCommandGroup(
- new DriveTimer(drivetrain, 0.5, 0, 35),
- new IntakeTimerCommand(intake, 35)
- )
-
-
- );
-
- }
-
- public boolean getDriveMode() {
- return driveMode.get();
- }
-
- public void setLimelightCaptureLED(boolean state) {
- operatorPanel.setOutput(OperatorConstants.limelightCaptureLED, state);
- }
-
- public void setLimelightLockLED(boolean state) {
- operatorPanel.setOutput(OperatorConstants.limelightLockLED, state);
- }
-}
diff --git a/src/main/java/org/team696/robot/TrajectoryTable.java b/src/main/java/org/team696/robot/TrajectoryTable.java
deleted file mode 100644
index 213a3ca..0000000
--- a/src/main/java/org/team696/robot/TrajectoryTable.java
+++ /dev/null
@@ -1,349 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-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 org.team696.robot;
-
-/**
- * Add your docs here.
- */
-public class TrajectoryTable {
-
- /**
- * First line is 120 inches away
- * First column is angle that ball is shot
- * Second column is velcity of the ball in RPM
- */
-public static final double[][] TrajectoryTable = {
-
-{0.0,0.0},
-{51.2,2354.45},
-{51.0,2362.16},
-{50.8,2369.90},
-{50.6,2377.69},
-{50.3,2385.51},
-{50.1,2393.37},
-{49.9,2401.27},
-{49.7,2409.20},
-{49.4,2417.17},
-{49.2,2425.17},
-{49.0,2433.22},
-{48.8,2441.29},
-{48.6,2449.40},
-{48.3,2457.55},
-{48.1,2465.73},
-{47.9,2473.94},
-{47.7,2482.19},
-{47.5,2490.47},
-{47.3,2498.79},
-{47.1,2507.13},
-{46.9,2515.51},
-{46.7,2523.92},
-{46.5,2532.37},
-{46.3,2540.84},
-{46.1,2549.35},
-{45.9,2557.88},
-{45.7,2566.45},
-{45.5,2575.04},
-{45.3,2583.67},
-{45.1,2592.32},
-{44.9,2601.01},
-{44.7,2609.72},
-{44.5,2618.46},
-{44.3,2627.23},
-{44.2,2636.03},
-{44.0,2644.86},
-{43.8,2653.71},
-{43.6,2662.59},
-{43.4,2671.50},
-{43.2,2680.44},
-{43.1,2689.40},
-{42.9,2698.38},
-{42.7,2707.40},
-{42.5,2716.43},
-{42.4,2725.50},
-{42.2,2734.59},
-{42.0,2743.70},
-{41.8,2752.84},
-{41.7,2762.00},
-{41.5,2771.19},
-{41.3,2780.40},
-{41.2,2789.63},
-{41.0,2798.89},
-{40.8,2808.17},
-{40.7,2817.48},
-{40.5,2826.80},
-{40.3,2836.15},
-{40.2,2845.53},
-{40.0,2854.92},
-{39.9,2864.34},
-{39.7,2873.77},
-{39.6,2883.23},
-{39.4,2892.71},
-{39.2,2902.21},
-{39.1,2911.73},
-{38.9,2921.28},
-{38.8,2930.84},
-{38.6,2940.42},
-{38.5,2950.02},
-{38.3,2959.65},
-{38.2,2969.29},
-{38.1,2978.95},
-{37.9,2988.63},
-{37.8,2998.33},
-{37.6,3008.05},
-{37.5,3017.79},
-{37.3,3027.54},
-{37.2,3037.32},
-{37.1,3047.11},
-{36.9,3056.92},
-{36.8,3066.75},
-{36.6,3076.60},
-{36.5,3086.46},
-{36.4,3096.34},
-{36.2,3106.24},
-{36.1,3116.15},
-{36.0,3126.08},
-{35.8,3136.03},
-{35.7,3146.00},
-{35.6,3155.98},
-{35.4,3165.98},
-{35.3,3175.99},
-{35.2,3186.02},
-{35.1,3196.06},
-{34.9,3206.12},
-{34.8,3216.20},
-{34.7,3226.29},
-{34.6,3236.40},
-{34.4,3246.52},
-{34.3,3256.65},
-{34.2,3266.81},
-{34.1,3276.97},
-{34.0,3287.15},
-{33.8,3297.35},
-{33.7,3307.55},
-{33.6,3317.78},
-{33.5,3328.01},
-{33.4,3338.26},
-{33.3,3348.53},
-{33.1,3358.80},
-{33.0,3369.10},
-{32.9,3379.40},
-{32.8,3389.72},
-{32.7,3400.05},
-{32.6,3410.39},
-{32.5,3420.75},
-{32.4,3431.12},
-{32.2,3441.50},
-{32.1,3451.89},
-{32.0,3462.30},
-{31.9,3472.72},
-{31.8,3483.15},
-{31.7,3493.59},
-{31.6,3504.04},
-{31.5,3514.51},
-{31.4,3524.99},
-{31.3,3535.48},
-{31.2,3545.98},
-{31.1,3556.49},
-{31.0,3567.02},
-{30.9,3577.55},
-{30.8,3588.10},
-{30.7,3598.66},
-{30.6,3609.22},
-{30.5,3619.80},
-{30.4,3630.39},
-{30.3,3640.99},
-{30.2,3651.60},
-{30.1,3662.22},
-{30.0,3672.86},
-{29.9,3683.50},
-{29.8,3694.15},
-{29.7,3704.81},
-{29.6,3715.49},
-{29.5,3726.17},
-{29.4,3736.86},
-{29.3,3747.56},
-{29.2,3758.27},
-{29.2,3768.99},
-{29.1,3779.72},
-{29.0,3790.46},
-{28.9,3801.21},
-{28.8,3811.97},
-{28.7,3822.74},
-{28.6,3833.52},
-{28.5,3844.30},
-{28.4,3855.10},
-{28.4,3865.90},
-{28.3,3876.71},
-{28.2,3887.53},
-{28.1,3898.36},
-{28.0,3909.20},
-{27.9,3920.05},
-{27.8,3930.90},
-{27.8,3941.77},
-{27.7,3952.64},
-{27.6,3963.52},
-{27.5,3974.41},
-{27.4,3985.31},
-{27.4,3996.21},
-{27.3,4007.12},
-{27.2,4018.04},
-{27.1,4028.97},
-{27.0,4039.91},
-{27.0,4050.85},
-{26.9,4061.80},
-{26.8,4072.76},
-{26.7,4083.73},
-{26.6,4094.70},
-{26.6,4105.68},
-{26.5,4116.67},
-{26.4,4127.67},
-{26.3,4138.67},
-{26.3,4149.68},
-{26.2,4160.70},
-{26.1,4171.72},
-{26.0,4182.76},
-{26.0,4193.80},
-{25.9,4204.84},
-{25.8,4215.89},
-{25.7,4226.95},
-{25.7,4238.02},
-{25.6,4249.09},
-{25.5,4260.17},
-{25.5,4271.26},
-{25.4,4282.35},
-{25.3,4293.45},
-{25.2,4304.55},
-{25.2,4315.66},
-{25.1,4326.78},
-{25.0,4337.91},
-{25.0,4349.04},
-{24.9,4360.17},
-{24.8,4371.31},
-{24.8,4382.46},
-{24.7,4393.62},
-{24.6,4404.78},
-{24.6,4415.95},
-{24.5,4427.12},
-{24.4,4438.30},
-{24.4,4449.48},
-{24.3,4460.67},
-{24.2,4471.87},
-{24.2,4483.07},
-{24.1,4494.28},
-{24.0,4505.49},
-{24.0,4516.71},
-{23.9,4527.93},
-{23.9,4539.16},
-{23.8,4550.40},
-{23.7,4561.64},
-{23.7,4572.88},
-{23.6,4584.13},
-{23.6,4595.39},
-{23.5,4606.65},
-{23.4,4617.92},
-{23.4,4629.19},
-{23.3,4640.46},
-{23.2,4651.75},
-{23.2,4663.03},
-{23.1,4674.33},
-{23.1,4685.62},
-{23.0,4696.92},
-{23.0,4708.23},
-{22.9,4719.54},
-{22.8,4730.86},
-{22.8,4742.18},
-{22.7,4753.51},
-{22.7,4764.84},
-{22.6,4776.17},
-{22.6,4787.51},
-{22.5,4798.86},
-{22.4,4810.21},
-{22.4,4821.56},
-{22.3,4832.92},
-{22.3,4844.28},
-{22.2,4855.65},
-{22.2,4867.02},
-{22.1,4878.40},
-{22.1,4889.78},
-{22.0,4901.17},
-{21.9,4912.56},
-{21.9,4923.95},
-{21.8,4935.35},
-{21.8,4946.75},
-{21.7,4958.16},
-{21.7,4969.57},
-{21.6,4980.98},
-{21.6,4992.40},
-{21.5,5003.82},
-{21.5,5015.25},
-{21.4,5026.68},
-{21.4,5038.12},
-{21.3,5049.56},
-{21.3,5061.00},
-{21.2,5072.44},
-{21.2,5083.90},
-{21.1,5095.35},
-{21.1,5106.81},
-{21.0,5118.27},
-{21.0,5129.74},
-{20.9,5141.21},
-{20.9,5152.68},
-{20.8,5164.16},
-{20.8,5175.64},
-{20.7,5187.12},
-{20.7,5198.61},
-{20.6,5210.10},
-{20.6,5221.60},
-{20.5,5233.10},
-{20.5,5244.60},
-{20.4,5256.11},
-{20.4,5267.62},
-{20.4,5279.13},
-{20.3,5290.65},
-{20.3,5302.17},
-{20.2,5313.69},
-{20.2,5325.22},
-{20.1,5336.75},
-{20.1,5348.28},
-{20.0,5359.82},
-{20.0,5371.36},
-{19.9,5382.90},
-{19.9,5394.45},
-{19.9,5406.00},
-{19.8,5417.55},
-{19.8,5429.11},
-{19.7,5440.67},
-{19.7,5452.23},
-{19.6,5463.80},
-{19.6,5475.37},
-{19.6,5486.94},
-{19.5,5498.51},
-{19.5,5510.09},
-{19.4,5521.67},
-{19.4,5533.26},
-{19.3,5544.84},
-{19.3,5556.44},
-{19.3,5568.03},
-{19.2,5579.62},
-{19.2,5591.22},
-{19.1,5602.83},
-{19.1,5614.43},
-{19.0,5626.04},
-{19.0,5637.65},
-{19.0,5649.26},
-{18.9,5660.88},
-{18.9,5672.50},
-{18.8,5684.12},
-{18.8,5695.74},
-{18.8,5707.37},
-{18.7,5719.00},
-{18.7,5730.64},
-{18.6,5742.27}
-};
-
-}
\ No newline at end of file
diff --git a/src/main/java/org/team696/robot/commands/ATCForCommand.java b/src/main/java/org/team696/robot/commands/ATCForCommand.java
deleted file mode 100644
index d7d6ed1..0000000
--- a/src/main/java/org/team696/robot/commands/ATCForCommand.java
+++ /dev/null
@@ -1,70 +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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-
-import org.team696.robot.Constants;
-import org.team696.robot.subsystems.Spindexer;
-
-public class ATCForCommand extends CommandBase {
- /**
- * Creates a new SpinToIndex.
- */
- private static final Logger logger = LogManager.getLogger(ATCForCommand.class);
- Spindexer spindexer;
-
- int oldSpindexerPocket = 0;
-
- public ATCForCommand(Spindexer spindexer) {
- this.spindexer = spindexer;
- addRequirements(spindexer);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- oldSpindexerPocket = spindexer.getTargetPocket();
- int targetPocket;
- if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) {
- targetPocket = 1;
- } else {
- targetPocket = spindexer.getTargetPocket() + 1;
- }
- logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
- spindexer.setTargetPocket(targetPocket);
- }
-
- // 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) {
- if(interrupted){
- logger.info("Spindexer command interrupted.");
- } else {
- logger.info("Spindexer on target.");
- }
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return spindexer.isOnTarget();
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ATCRevCommand.java b/src/main/java/org/team696/robot/commands/ATCRevCommand.java
deleted file mode 100644
index beb0e54..0000000
--- a/src/main/java/org/team696/robot/commands/ATCRevCommand.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-
-import org.team696.robot.Constants;
-import org.team696.robot.subsystems.Spindexer;
-
-public class ATCRevCommand extends CommandBase {
- private static final Logger logger = LogManager.getLogger(ATCRevCommand.class);
- Spindexer spindexer;
- int oldSpindexerPocket;
-
- public ATCRevCommand(Spindexer spindexer) {
- this.spindexer = spindexer;
- addRequirements(spindexer);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- oldSpindexerPocket = spindexer.getTargetPocket();
- int targetPocket;
- if (spindexer.getTargetPocket() == 1) {
- targetPocket = Constants.SpindexerConstants.nPockets;
- } else {
- targetPocket = spindexer.getTargetPocket() - 1;
- }
- logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
- spindexer.setTargetPocket(targetPocket);
- }
-
- // 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) {
- if(interrupted){
- logger.info("Spindexer command interrupted.");
- } else {
- logger.info("Spindexer on target.");
- }
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return spindexer.isOnTarget();
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java b/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java
deleted file mode 100644
index 9a2f3b2..0000000
--- a/src/main/java/org/team696/robot/commands/AutoIndexKickUp.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Intake;
-import org.team696.robot.subsystems.Spindexer;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-
-// 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 AutoIndexKickUp extends SequentialCommandGroup {
- /**
- * Creates a new AutoIndexKickUp.
- */
- public AutoIndexKickUp(Spindexer spindexer, Intake intake, double drumPower, double omniPower) {
- // Add your commands in the super() call, e.g.
- // super(new FooCommand(), new BarCommand());
- super(
- // new ATCForCommand(spindexer),
- // new OmniKickUp(spindexer, omniPower),
- new ContinuousShoot(spindexer, drumPower, true)
-
- );
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ClimberLift.java b/src/main/java/org/team696/robot/commands/ClimberLift.java
deleted file mode 100644
index 18d54c8..0000000
--- a/src/main/java/org/team696/robot/commands/ClimberLift.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-
-// import org.team696.robot.subsystems.ClimberSubsystem;
-// import org.team696.robot.RobotContainer;
-// import org.team696.robot.Constants.OperatorConstants;
-// import org.team696.robot.subsystems.ClimberServos;
-
-// import edu.wpi.first.wpilibj2.command.CommandBase;
-
-// public class ClimberLift extends CommandBase {
-// private ClimberSubsystem climberSubsystem;
-// private ClimberServos climberServos;
-// /**
-// * Creates a new ClimberPullUp.
-// */
-// public ClimberLift(ClimberSubsystem climberSubsystem, ClimberServos climberServos) {
-// this.climberSubsystem = climberSubsystem;
-// this.climberServos = climberServos;
-// addRequirements(climberSubsystem);
-// addRequirements(climberServos);
-// }
-
-// // 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() {
-// // servoSubsystem.closeServo();
-// // if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberUpButton)){
-
-// climberSubsystem.setClimberPower();
-// // System.out.println(climberSubsystem.climberTargetPower);
-
-// // }
-// // else{
-// // climberSubsystem.setClimberPower(0);
-// // // System.out.println(climberSubsystem.climberTargetPower);
-
-// // }
-
-
-// }
-
-
-
-// // Called once the command ends or is interrupted.
-// @Override
-// public void end(boolean interrupted) {
-// // servoSubsystem.CloseServo();
-
-// }
-
-// // Returns true when the command should end.
-// @Override
-// public boolean isFinished() {
-// return false;
-// }
-// }
diff --git a/src/main/java/org/team696/robot/commands/ClimberReach.java b/src/main/java/org/team696/robot/commands/ClimberReach.java
deleted file mode 100644
index b32183f..0000000
--- a/src/main/java/org/team696/robot/commands/ClimberReach.java
+++ /dev/null
@@ -1,68 +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 org.team696.robot.commands;
-
-// import org.team696.robot.subsystems.ClimberSubsystem;
-// import org.team696.robot.RobotContainer;
-// import org.team696.robot.Constants.OperatorConstants;
-// import org.team696.robot.subsystems.ClimberServos;
-
-// import edu.wpi.first.wpilibj2.command.CommandBase;
-
-// public class ClimberReach extends CommandBase {
-// private ClimberSubsystem climberSubsystem;
-// private ClimberServos servoSubsystem;
-// /**
-// * Creates a new ClimberPushDown.
-// */
-
-// boolean servoIsOpen = false;
-
-// public ClimberReach(ClimberSubsystem climberSubsystem, ClimberServos servoSubsystem) {
-// this.climberSubsystem = climberSubsystem;
-// this.servoSubsystem = servoSubsystem;
-// addRequirements(climberSubsystem);
-// addRequirements(servoSubsystem);
-// // Use addRequirements() here to declare subsystem dependencies.
-// }
-
-// // 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() {
-
-// // servoSubsystem.moveClimberServos(180);
-
-// if (RobotContainer.operatorPanel.getRawButton(OperatorConstants.climberDownButton)){
-// climberSubsystem.setClimberPower();
-
-// }
-// else{
-// climberSubsystem.setClimberPower(0);
-// }
-
-// }
-
-// // Called once the command ends or is interrupted.
-// @Override
-// public void end(boolean interrupted) {
-// //ClimberSubsystem.climberState = ClimberStates.AT_HEIGHT;
-// }
-
-// // Returns true when the command should end.
-// @Override
-// public boolean isFinished() {
-// //if sensor trips return tru
-// // return climberSubsystem.isLatchedOn();
-// return false;
-// }
-// }
diff --git a/src/main/java/org/team696/robot/commands/ContinuousShoot.java b/src/main/java/org/team696/robot/commands/ContinuousShoot.java
deleted file mode 100644
index e131fcd..0000000
--- a/src/main/java/org/team696/robot/commands/ContinuousShoot.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Spindexer;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class ContinuousShoot extends CommandBase {
- /**
- * Creates a new ContinuousShoot.
- */
- Spindexer spindexer;
- boolean kickUpState;
- double drumSpeed;
- int timer;
- public ContinuousShoot(Spindexer spindexer, double drumSpeed, boolean kickUpState) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.spindexer = spindexer;
- this.kickUpState = kickUpState;
- this.drumSpeed = drumSpeed;
- addRequirements(spindexer);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- timer = 0;
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- timer++;
- if(timer>6){
- spindexer.spindexerLoadingAntiJam(drumSpeed);
-
- }
- spindexer.setKickMotor(1);
-
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- spindexer.setKickMotor(0);
- timer = 0;
-
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/Drive.java b/src/main/java/org/team696/robot/commands/Drive.java
deleted file mode 100644
index 8efafb3..0000000
--- a/src/main/java/org/team696/robot/commands/Drive.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import java.util.function.DoubleSupplier;
-
-import org.team696.robot.subsystems.Drivetrain;
-import org.team696.robot.subsystems.Shooter;
-
-public class Drive extends CommandBase {
- private final Drivetrain drivetrain;
- private final DoubleSupplier speedSupplier;
- private final DoubleSupplier turnSupplier;
-
- /**
- * Creates a new Drive.
- */
- double currentServoPosition;
-
- public Drive(DoubleSupplier speed, DoubleSupplier turn, Drivetrain drivetrain) {
- this.drivetrain = drivetrain;
- this.speedSupplier = speed;
- this.turnSupplier = turn;
-
- addRequirements(drivetrain);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- // currentServoPosition = shooter.servoAngle();
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- drivetrain.arcadeDrive(speedSupplier.getAsDouble(), turnSupplier.getAsDouble());
-
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- drivetrain.arcadeDrive(0, 0);
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/DriveTimer.java b/src/main/java/org/team696/robot/commands/DriveTimer.java
deleted file mode 100644
index 705801d..0000000
--- a/src/main/java/org/team696/robot/commands/DriveTimer.java
+++ /dev/null
@@ -1,55 +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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Drivetrain;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class DriveTimer extends CommandBase {
- private Drivetrain drivetrain;
- /**
- * Creates a new DriveTimer.
- */
- int timer;
- int x = 0;
- double speed;
- double turn;
- public DriveTimer(Drivetrain drivetrain, double speed, double turn, int timer) {
- this.drivetrain = drivetrain;
- this.timer = timer;
- this.speed = speed;
- this.turn = turn;
- addRequirements(drivetrain);
- }
-
- // 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() {
- drivetrain.arcadeDrive(speed, turn);
- x++;
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- drivetrain.arcadeDrive(0, 0);
- x = 0;
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return x>timer;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/FireCommand.java b/src/main/java/org/team696/robot/commands/FireCommand.java
deleted file mode 100644
index 00e140e..0000000
--- a/src/main/java/org/team696/robot/commands/FireCommand.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Shooter;
-import org.team696.robot.subsystems.Spindexer;
-
-public class FireCommand extends CommandBase {
- /**
- * Creates a new FireCommand.
- */
- private Shooter shooter;
- private Spindexer spindexer;
- private double power;
-
- public FireCommand(Shooter shooter, Spindexer spindexer, double kickMotorPower) {
-
- this.shooter = shooter;
- this.spindexer = spindexer;
- this.power = kickMotorPower;
- addRequirements(shooter);
- addRequirements(spindexer);
- }
-
- // 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(shooter.isUpToSpeed()){
- // shooter.setAcceleratorPower(true);
- spindexer.setKickMotor(power);
- // spindexer.spindexerLoadingAntiJam(0.5, 20);
-
-
- }
- else{
- spindexer.setKickMotor(0);
-
- }
-
- }
-
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/FullShoot.java b/src/main/java/org/team696/robot/commands/FullShoot.java
deleted file mode 100644
index 1c02161..0000000
--- a/src/main/java/org/team696/robot/commands/FullShoot.java
+++ /dev/null
@@ -1,44 +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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.ShooterHood;
-import org.team696.robot.subsystems.Spindexer;
-import org.team696.robot.subsystems.TurretSubsystem;
-
-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 FullShoot extends SequentialCommandGroup {
- /**
- * Creates a new FullShoot.
- */
- public FullShoot(ShooterHood shooterHood, Spindexer spindexer, TurretSubsystem turretSubsystem, Limelight limelight) {
- // Add your commands in the super() call, e.g.
- // super(new FooCommand(), new BarCommand());
- super(
- new ShooterHoodCommand(shooterHood, 51),
- new SpinToPocket(spindexer, 1),
- new TurretLockOn(turretSubsystem, limelight),
- new OmniKickUpTimer(spindexer, true, 50),
- new SpinToPocket(spindexer, 2),
- new OmniKickUpTimer(spindexer, true, 50),
- new SpinToPocket(spindexer, 3),
- new OmniKickUpTimer(spindexer, true, 50),
- new SpinToPocket(spindexer, 4),
- new OmniKickUpTimer(spindexer, true, 50),
- new SpinToPocket(spindexer, 5),
- new OmniKickUpTimer(spindexer, true, 50),
- new ShooterHoodCommand(shooterHood, 10)
-
- );
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/IntakeCommand.java b/src/main/java/org/team696/robot/commands/IntakeCommand.java
deleted file mode 100644
index c5781c2..0000000
--- a/src/main/java/org/team696/robot/commands/IntakeCommand.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Intake;
-
-public class IntakeCommand extends CommandBase {
- /**
- * Creates a new IntakeCom
- * mand.
- */
- private Intake intake;
-
- private double intakePower;
- public IntakeCommand(Intake intake, double intakePower) {
- // Use addRequirements() here to declare subsystem dependencies.
-
- this.intake = intake;
- this.intakePower = intakePower;
- addRequirements(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() {
- intake.runIntake(intakePower);
- // spindexer.spindexerLoadingAntiJam(power, current);
- // System.out.println("loading");
- }
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java b/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java
deleted file mode 100644
index 336c665..0000000
--- a/src/main/java/org/team696/robot/commands/IntakeTimerCommand.java
+++ /dev/null
@@ -1,55 +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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Intake;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class IntakeTimerCommand extends CommandBase {
- /**
- * Creates a new IntakeTimerCommand.
- */
- Intake intake;
- int x = 0;
- double timer;
- public IntakeTimerCommand(Intake intake, double timer) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.intake = intake;
- this.timer = timer;
- addRequirements(intake);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- x = 0;
-
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- intake.runIntake(0.7);
- x++;
- // System.out.println(timer);
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- // timer = 0;
- intake.runIntake(0);
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return x>timer;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/OmniKickUp.java b/src/main/java/org/team696/robot/commands/OmniKickUp.java
deleted file mode 100644
index 49ed7c8..0000000
--- a/src/main/java/org/team696/robot/commands/OmniKickUp.java
+++ /dev/null
@@ -1,50 +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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Spindexer;
-
-public class OmniKickUp extends CommandBase {
- /**
- * Creates a new OmniKickUp.
- */
- Spindexer spindexer;
- double power;
- public OmniKickUp(Spindexer spindexer, double power) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.spindexer = spindexer;
- this.power = power;
- addRequirements(spindexer);
-
- }
-
- // 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() {
- spindexer.setKickMotor(power);
-
- }
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java b/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java
deleted file mode 100644
index 4d0c7a2..0000000
--- a/src/main/java/org/team696/robot/commands/OmniKickUpTimer.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Spindexer;
-
-public class OmniKickUpTimer extends CommandBase {
- /**
- * Creates a new OmniKickUp.
- */
- Spindexer spindexer;
- boolean state;
- int x = 0;
- double timer;
-
- public OmniKickUpTimer(Spindexer spindexer, boolean state, double timer) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.spindexer = spindexer;
- this.state = state;
-
- this.timer = timer;
- addRequirements(spindexer);
-
-
- }
-
- // 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() {
- spindexer.setKickMotor(1);
- System.out.println(x);
- x++;
-
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- x = 0;
- System.out.println("done");
- spindexer.setKickMotor(0);
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return x>timer ;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ShooterCommand.java b/src/main/java/org/team696/robot/commands/ShooterCommand.java
deleted file mode 100644
index de47a1b..0000000
--- a/src/main/java/org/team696/robot/commands/ShooterCommand.java
+++ /dev/null
@@ -1,72 +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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-import org.team696.robot.RobotContainer;
-import org.team696.robot.Constants.OperatorConstants;
-import org.team696.robot.Constants.ShooterConstants;
-import org.team696.robot.subsystems.Shooter;
-
-public class ShooterCommand extends CommandBase {
- /**
- * Creates a new ShooterCommand.
- */
- private static final Logger logger = LogManager.getLogger(ShooterCommand.class);
- private Shooter shooter;
-
- double RPM;
- boolean state;
- public ShooterCommand(Shooter shooter, double RPM, boolean state) {
-
- this.shooter = shooter;
- this.state = state;
- this.RPM = RPM;
- addRequirements(shooter);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- logger.info(String.format("Spinning up shooter to %f RPM", RPM));
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterAutoButton)){
- shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(shooter.getShootRPM()));
- // }
- // else if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterManualButton)){
- // // shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM+1000*RobotContainer.operatorPanel.getRawAxis(OperatorConstants.shooterManualAxis)));
- // shooter.setShooterPower(shooter.shooterRPMToTalonVelocity(ShooterConstants.trenchRunTargetRPM));
- // }
- // else{
- // shooter.setShooterPower(0);
- // }
-
- shooter.setAcceleratorPower(state);
-
- //TODO: make another command to run off of feedforward gains
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- logger.info("Done with shooter command.");
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return false;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java b/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java
deleted file mode 100644
index bc2101c..0000000
--- a/src/main/java/org/team696/robot/commands/ShooterHoodCommand.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.RobotContainer;
-import org.team696.robot.Constants.OperatorConstants;
-import org.team696.robot.subsystems.Shooter;
-import org.team696.robot.subsystems.ShooterHood;
-
-public class ShooterHoodCommand extends CommandBase {
- /**
- * Creates a new ShooterHoodCommand.
- */
- private ShooterHood shooterHood;
- private double angle;
-
- private double timer;
- public ShooterHoodCommand(ShooterHood shooterHood, double angle) {
-
- this.shooterHood = shooterHood;
- this.angle = angle;
- addRequirements(shooterHood);
-
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- timer = 0;
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
-
- timer++;
- System.out.println("adjusting hood");
- // if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterAutoButton)){
- shooterHood.moveServoAngle(angle);
- // }
- // else if(RobotContainer.operatorPanel.getRawButton(OperatorConstants.shooterManualButton)){
- // // shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM+1000*RobotContainer.operatorPanel.getRawAxis(OperatorConstants.shooterManualAxis)));
- // shooterHood.moveServoAngle(shooterHood.shootAngleToServoAngle(18));
-
- // }
- // else{
- // // shooter.setShooterPower(0);
- // shooterHood.moveServoAngle(shooterHood.shootAngleToServoAngle(40));
-
- // }
-
- }
-
- // 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 timer>150;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java b/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java
deleted file mode 100644
index 41fc4c5..0000000
--- a/src/main/java/org/team696/robot/commands/ShooterPowerCommand.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Shooter;
-
-public class ShooterPowerCommand extends CommandBase {
- /**
- * Creates a new ShooterPowerCommand.
- */
- private Shooter shooter;
- private double power;
- private boolean state;
- public ShooterPowerCommand(Shooter shooter, double power, boolean state) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.shooter = shooter;
- this.state = state;
- this.power = power;
- addRequirements(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() {
- shooter.setShooterPower(power);
- shooter.setAcceleratorPower(state);
- }
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ShooterPrep.java b/src/main/java/org/team696/robot/commands/ShooterPrep.java
deleted file mode 100644
index 333bffd..0000000
--- a/src/main/java/org/team696/robot/commands/ShooterPrep.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.Shooter;
-import org.team696.robot.subsystems.TurretSubsystem;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class ShooterPrep extends CommandBase {
-
- //maybe add omni to this command or make it part of the spindexer drum one
- TurretSubsystem turretSubsystem;
- Shooter shooter;
- public ShooterPrep(Shooter shooter, TurretSubsystem turretSubsystem) {
- this.turretSubsystem = turretSubsystem;
- this.shooter = shooter;
- addRequirements(shooter);
- addRequirements(turretSubsystem);
- }
-
- // 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() {
- Limelight.setLights(3);
- turretSubsystem.moveToTarget();
- shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(shooter.getShootRPM()));
- shooter.setAcceleratorPower(true);
- //maybe add shooter hood
- }
-
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java b/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java
deleted file mode 100644
index 2dea15f..0000000
--- a/src/main/java/org/team696/robot/commands/ShooterTimerCommand.java
+++ /dev/null
@@ -1,60 +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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Shooter;
-
-public class ShooterTimerCommand extends CommandBase {
- /**
- * Creates a new ShooterCommand.
- */
- private Shooter shooter;
-
- double RPM;
- boolean state;
- int x = 0;
- public ShooterTimerCommand(Shooter shooter, double RPM, boolean state) {
-
- this.shooter = shooter;
- this.state = state;
- this.RPM = RPM;
- addRequirements(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() {
- shooter.setShooterVelocity(shooter.shooterRPMToTalonVelocity(RPM));
- shooter.setAcceleratorPower(state);
- System.out.println("shooting");
- x++;
-
- //TODO: make another command to run off of feedforward gains
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- x = 0;
- shooter.setShooterPower(0);
-
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return x>150;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/SpinToPocket.java b/src/main/java/org/team696/robot/commands/SpinToPocket.java
deleted file mode 100644
index 5ff11af..0000000
--- a/src/main/java/org/team696/robot/commands/SpinToPocket.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-
-import org.team696.robot.Constants;
-import org.team696.robot.subsystems.Spindexer;
-
-public class SpinToPocket extends CommandBase {
- /**
- * Creates a new SpinToIndex.
- */
- private static final Logger logger = LogManager.getLogger(ATCForCommand.class);
- Spindexer spindexer;
-
- int targetPocket;
-
- public SpinToPocket(Spindexer spindexer, int targetPocket) {
- this.spindexer = spindexer;
- this.targetPocket = targetPocket;
- addRequirements(spindexer);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- // oldSpindexerPocket = spindexer.getTargetPocket();
- // int targetPocket;
- // if (spindexer.getTargetPocket() == Constants.SpindexerConstants.nPockets) {
- // targetPocket = 1;
- // } else {
- // targetPocket = spindexer.getTargetPocket() + 1;
- // }
- // logger.info(String.format("Moving spindexer from %d to %d", oldSpindexerPocket, targetPocket));
- // spindexer.setTargetPocket(targetPocket);
- spindexer.setTargetPocket(targetPocket);
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- //System.out.println("spinning to index stuff");
- //System.out.println(spindexer.getCurrentPocket());
-
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- if(interrupted){
- logger.info("Spindexer command interrupted.");
- } else {
- logger.info("Spindexer on target.");
- }
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- return spindexer.getCurrentPocket()==targetPocket;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/SpindexerLoading.java b/src/main/java/org/team696/robot/commands/SpindexerLoading.java
deleted file mode 100644
index bad50e9..0000000
--- a/src/main/java/org/team696/robot/commands/SpindexerLoading.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Intake;
-import org.team696.robot.subsystems.Spindexer;
-
-public class SpindexerLoading extends CommandBase {
- /**
- * Creates a new SpindexerLoading.
- */
- Spindexer spindexer;
- Intake intake;
- double spindexerPower;
- double intakePower;
- public SpindexerLoading(Spindexer spindexer, Intake intake, double spindexerPower, double intakePower) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.spindexerPower = spindexerPower;
- this.intakePower = intakePower;
- this.spindexer = spindexer;
- this.intake = intake;
- addRequirements(spindexer);
- addRequirements(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() {
- spindexer.spindexerLoadingAntiJam(spindexerPower);
- intake.runIntake(intakePower);
- // System.out.println("running the loading stuff");
- }
-
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/StopTurret.java b/src/main/java/org/team696/robot/commands/StopTurret.java
deleted file mode 100644
index ef0a34a..0000000
--- a/src/main/java/org/team696/robot/commands/StopTurret.java
+++ /dev/null
@@ -1,47 +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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.TurretSubsystem;
-
-public class StopTurret extends CommandBase {
- /**
- * Creates a new StopTurret.
- */
- private TurretSubsystem turretSubsystem;
-
- public StopTurret(TurretSubsystem turretSubsystem) {
- // Use addRequirements() here to declare subsystem dependencies.
- this.turretSubsystem = turretSubsystem;
- addRequirements(turretSubsystem);
- }
-
- // 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() {
- turretSubsystem.openLoop(0);
- }
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/TurretLockOn.java b/src/main/java/org/team696/robot/commands/TurretLockOn.java
deleted file mode 100644
index abd8a31..0000000
--- a/src/main/java/org/team696/robot/commands/TurretLockOn.java
+++ /dev/null
@@ -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 org.team696.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.TurretSubsystem;
-
-public class TurretLockOn extends CommandBase {
- /**
- * Creates a new TurretLockOn.
- */
-
- TurretSubsystem turretSubsystem;
- Limelight limelight;
- public TurretLockOn(TurretSubsystem turretSubsystem, Limelight limelight) {
- this.turretSubsystem = turretSubsystem;
- this.limelight = limelight;
- addRequirements(turretSubsystem);
- addRequirements(limelight);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- limelight.setLights(3);
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- // turretSubsystem.openLoop(0.05);
- turretSubsystem.moveToTarget();
- }
-
- // 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 turretSubsystem.onTarget();
- return false;
- }
-}
diff --git a/src/main/java/org/team696/robot/commands/TurretManual.java b/src/main/java/org/team696/robot/commands/TurretManual.java
deleted file mode 100644
index a9cbf90..0000000
--- a/src/main/java/org/team696/robot/commands/TurretManual.java
+++ /dev/null
@@ -1,64 +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 org.team696.robot.commands;
-
-import org.team696.robot.RobotContainer;
-import org.team696.robot.Constants.TurretConstants;
-import org.team696.robot.subsystems.Limelight;
-import org.team696.robot.subsystems.TurretSubsystem;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class TurretManual extends CommandBase {
- /**
- * Creates a new TurretManual.
- */
- TurretSubsystem turretSubsystem;
- Limelight limelight;
- double position;
- public TurretManual(TurretSubsystem turretSubsystem, Limelight limelight) {
- // Use addRequirements() here to declare subsystem dependencies.\
- this.turretSubsystem = turretSubsystem;
- this.limelight = limelight;
- this.position = position;
- addRequirements(turretSubsystem);
- addRequirements(limelight);
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- // limelight.setLights(1);
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- // turretSubsystem.moveToPosition(TurretConstants.turretCenterPos+position*100);
- // System.out.println("turret manual");
- // System.out.println(position);
- if(RobotContainer.operatorPanel.getRawAxis(0)>0.05){
- limelight.setLights(1);
- }
- else{
- limelight.setLights(1);
- }
- turretSubsystem.openLoop(-RobotContainer.operatorPanel.getRawAxis(0));
- }
-
- // 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;
- }
-}
diff --git a/src/main/java/org/team696/robot/subsystems/BallSensors.java b/src/main/java/org/team696/robot/subsystems/BallSensors.java
deleted file mode 100644
index 018d21c..0000000
--- a/src/main/java/org/team696/robot/subsystems/BallSensors.java
+++ /dev/null
@@ -1,138 +0,0 @@
-package org.team696.robot.subsystems;
-
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.I2C.Port;
-
-import org.apache.logging.log4j.Logger;
-import org.apache.logging.log4j.LogManager;
-
-import org.team696.TCA9548ADriver.TCA9548A;
-import org.team696.TCA9548ADriver.TCA9548ADevice;
-import org.team696.TCS34725.Gain;
-import org.team696.TCS34725.IntegrationTime;
-import org.team696.TCS34725.TCS34725;
-import org.team696.robot.Constants.SpindexerConstants;
-import org.team696.utils.ColorUtils.Color;
-
-public class BallSensors {
- private int num_sensors;
- private TCS34725[] sensors;
- private static final Logger logger = LogManager.getLogger(TCA9548ADevice.class);
-
- /**
- * Constructor that assumes a mux with the address given in Constants and sensors connected in order.
- */
- public BallSensors() {
- num_sensors = SpindexerConstants.nPockets;
- sensors = new TCS34725[num_sensors];
- byte muxaddr = SpindexerConstants.ColorSensorsMuxAddress;
- TCA9548A mux = new TCA9548A(Port.kOnboard, muxaddr);
- int i;
- for (i = 0; i < num_sensors; i++) {
- TCA9548ADevice thisMuxChannel = new TCA9548ADevice(mux, TCS34725.I2C_constants.TCS34725_I2C_ADDR, i);
- TCS34725 thisSensor = new TCS34725(thisMuxChannel);
- sensors[i] = thisSensor;
- }
- }
-
- /**
- * Constructor that takes premade I2C channels.
- * @param channels I2C channels (should be num_sensors long)
- */
- public BallSensors(I2C[] channels) {
- num_sensors = SpindexerConstants.nPockets;
- sensors = new TCS34725[num_sensors];
- int i;
- for (i = 0; i < num_sensors; i++) {
- TCS34725 thisSensor = new TCS34725(channels[i]);
- sensors[i] = thisSensor;
- }
- }
-
- /**
- * Constructor that takes a mux and the indices of the mux channels going to the sensors.
- * @param mux A TCA9548A mux object
- * @param muxChannels The mux channels to use (should be num_sensors long)
- */
- public BallSensors(TCA9548A mux, byte[] muxChannels) {
- num_sensors = SpindexerConstants.nPockets;
- sensors = new TCS34725[num_sensors];
- int i;
- for (i = 0; i < num_sensors; i++) {
- TCA9548ADevice thisMuxChannel = new TCA9548ADevice(mux, TCS34725.I2C_constants.TCS34725_I2C_ADDR, muxChannels[i]);
- TCS34725 thisSensor = new TCS34725(thisMuxChannel);
- thisSensor.setName(String.format("Pocket %d", i));
- sensors[i] = thisSensor;
- }
- }
-
- /**
- * Calls init() on all the sensors.
- */
- public void initSensors() {
- for (TCS34725 sensor : sensors) {
- sensor.init(IntegrationTime.IT_24MS, Gain.X1);
- }
- }
-
-
- public double[] getColor(int idx){
- double[] retval = new double[3];
- TCS34725 sensor = sensors[idx];
- if(sensor.readColors() == -1){
- sensor.init(IntegrationTime.IT_24MS, Gain.X1);
- }
- retval[0] = sensor.getRedVal();
- retval[1] = sensor.getGreenVal();
- retval[2] = sensor.getBlueVal();
- return retval;
- }
- /**
- * Gets whether each sensor sees a ball.
- * For each sensor, commands a reading, then compares the red, green,
- * blue, and clear values to the limits in Constants to see if a ball
- * is there.
- * @return An array of num_sensors booleans, each indicating if that sensor sees a ball
- */
- public boolean[] getResults() {
- boolean[] results = new boolean[num_sensors];
- int i;
- for (i = 0; i < num_sensors; i++) {
- TCS34725 sensor = sensors[i];
- if(sensor.readColors() == -1){
- sensor.init(IntegrationTime.IT_24MS, Gain.X1);
- }
- //TODO: figure out how to scale colors
- double red = sensor.getRedVal();
- double green = sensor.getGreenVal();
- double blue = sensor.getBlueVal();
- Color color = new Color(red, green, blue);
- double[] hsv = color.getHSV();
- boolean hasBall = true;
- if (hsv[0] < SpindexerConstants.HueMin || hsv[0] > SpindexerConstants.HueMax) {
- hasBall = false;
- }
- if (hsv[1] < SpindexerConstants.SatMin || hsv[1] > SpindexerConstants.SatMax) {
- hasBall = false;
- }
- if (hsv[2] < SpindexerConstants.ValueMin || hsv[2] > SpindexerConstants.ValueMax) {
- hasBall = false;
- }
- results[i] = hasBall;
- }
-
- return results;
- }
-
- public void setLEDs(boolean state){
- int i;
- for(i=0; i