diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0683984..538bf93 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -166,7 +166,7 @@ public final class Constants { public static final double PIVOT_SPEED = 0.2; //TODO: public static final class ArmPID { - public static final Gains INTAKE_GAINS = new Gains(0, 0, 0, 0, 0, 1.0); + public static final Gains INTAKE_GAINS = new Gains(0.00005, 0, 0, 0, 0, 1.0); } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 47bdb52..1a29285 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -81,20 +81,20 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ + // /* + // * String autoSelected = SmartDashboard.getString("Auto Selector", + // * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + // * = new MyAutoCommand(); break; case "Default Auto": default: + // * autonomousCommand = new ExampleCommand(); break; } + // */ - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - m_robotTime.startMatchTime(); + // // schedule the autonomous command (example) + // if (m_autonomousCommand != null) { + // m_autonomousCommand.schedule(); + // } + // m_robotTime.startMatchTime(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 48bb6c2..5807995 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,6 +8,7 @@ package frc4388.robot; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -56,7 +57,9 @@ public class RobotContainer { private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - private PlaybackChooser playbackChooser; + + + // private PlaybackChooser playbackChooser; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -75,9 +78,12 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - .buildDisplay(); + // playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + // .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .buildDisplay(); + //SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); + + // m_robotIntake.resetPostion(); } /** @@ -123,6 +129,18 @@ public class RobotContainer { // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake)) // .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) @@ -135,10 +153,10 @@ public class RobotContainer { * * @return the command to run in autonomous */ - public Command getAutonomousCommand() { - // no auto - return playbackChooser.getCommand(); - } + // public Command getAutonomousCommand() { + // no auto + // return playbackChooser.getCommand(); + // } /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 27b83bb..f1dbb0f 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.IntakeConstants; @@ -31,15 +32,16 @@ public class Intake extends SubsystemBase { this.pivot = pivot; pivot.restoreFactoryDefaults(); + //pivot.setInverted(true); - forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); - reverseLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); + forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); forwardLimit.enableLimitSwitch(true); reverseLimit.enableLimitSwitch(true); //Arm PID - m_spedController = intakeMotor.getPIDController(); + m_spedController = pivot.getPIDController(); m_spedController.setP(armGains.kP); m_spedController.setI(armGains.kI); m_spedController.setD(armGains.kD); @@ -52,6 +54,7 @@ public class Intake extends SubsystemBase { //Rotate robot in for handoff public void rotateArmIn() { + //pivot.set(IntakeConstants.PIVOT_SPEED); pivot.set(IntakeConstants.PIVOT_SPEED); } @@ -61,6 +64,15 @@ public class Intake extends SubsystemBase { } + public void pidIn() { + m_spedController.setReference(8000, CANSparkMax.ControlType.kVelocity); + //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); + } + + public void pidOut() { + + } + public void handoff() { intakeMotor.set(-IntakeConstants.INTAKE_SPEED); } @@ -68,12 +80,36 @@ public class Intake extends SubsystemBase { public void stopIntakeMotors() { intakeMotor.set(0); } + + public void stopArmMotor() { + pivot.set(0); + } + public RelativeEncoder getEncoder() { return pivot.getEncoder(); } public void setVoltage(double voltage) { pivot.setVoltage(voltage); } + + public double getVelocity() { + return pivot.getEncoder().getVelocity(); + } + + public void resetPostion() { + pivot.getEncoder().setPosition(0); + } + + public void resetPosition1() { + if(forwardLimit.isPressed() == true) { + resetPostion(); + } + } + + public double getPos() { + return pivot.getEncoder().getPosition(); + } + public void rotateArm() { } @@ -81,5 +117,8 @@ public class Intake extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Vel Output", getVelocity()); + SmartDashboard.putNumber("Position", getPos()); + resetPosition1(); } }