diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f4b73dc..97d4f30 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,12 +66,42 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 1; + public static final int INTAKE_SPARK_ID = 9; + public static final int EXTENDER_SPARK_ID = 10; } + + public static final class ShooterConstants { + public static final int SHOOTER_FALCON_ID = 8; + /* PID Constants Shooter */ + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final Gains SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + + public static final double ENCODER_TICKS_PER_REV = 2048; + } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 10; + } + + public static final class LevelerConstants { + public static final int LEVELER_CAN_ID = -1; + } + + public static final class StorageConstants { + public static final int STORAGE_CAN_ID = -1; + public static final int BEAM_SENSOR_DIO_0 = 0; + public static final int BEAM_SENSOR_DIO_1 = 1; + public static final int BEAM_SENSOR_DIO_2 = 2; + public static final int BEAM_SENSOR_DIO_3 = 3; + public static final int BEAM_SENSOR_DIO_4 = 4; + public static final int BEAM_SENSOR_DIO_5 = 5; + } + public static final class LEDConstants { public static final int LED_SPARK_ID = 0; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9e4f64d..37a847d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,10 +20,21 @@ import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveWithJoystickAuxPID; +import frc4388.robot.commands.RunClimberWithTriggers; +import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.commands.RunLevelerWithJoystick; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.Storage; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -40,6 +51,10 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Shooter m_robotShooter = new Shooter(); + private final Climber m_robotClimber = new Climber(); + private final Leveler m_robotLeveler = new Leveler(); + private final Storage m_robotStorage = new Storage(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -54,10 +69,18 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); - // drives motor with input from triggers on the opperator controller + // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // drives climber with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // runs the drum shooter in idle mode + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); + // drives the leveler with an axis input from the driver controller + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + // runs storage motor at 50 percent + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); } /** @@ -70,28 +93,47 @@ public class RobotContainer { /* Driver Buttons */ new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144)); - // runs velocity PID while driving straight - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) - .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); - - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); - // resets the yaw of the pigeon - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); - // turn 45 degrees - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); - // interrupts any running command - new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); + + /* PID Test Command */ + // runs velocity PID while driving straight + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) + .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + + //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); + + // resets the yaw of the pigeon + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72)); + + // turn 45 degrees + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive)); + + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + + // interrupts any running command + new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) + .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); } /** diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java new file mode 100644 index 0000000..9ab4285 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Climber; +import frc4388.utility.controller.IHandController; + +public class RunClimberWithTriggers extends CommandBase { + private Climber m_climber; + private IHandController m_controller; + + /** + * Uses input from opperator triggers to control climber motor + * @param subsystem the climber subsystem + * @param controller the driver controller + */ + public RunClimberWithTriggers(Climber subsystem, IHandController controller) { + m_climber = subsystem; + m_controller = controller; + addRequirements(m_climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double rightTrigger = m_controller.getRightTriggerAxis(); + double leftTrigger = m_controller.getLeftTriggerAxis(); + double output = 0; + if (rightTrigger < .5) { + if(rightTrigger > leftTrigger) { + output = rightTrigger; + } + if (leftTrigger > rightTrigger) { + output = -leftTrigger; + } + } else { + output = rightTrigger; + } + m_climber.runClimber(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java new file mode 100644 index 0000000..b0bb140 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Intake; +import frc4388.utility.controller.IHandController; + +public class RunExtenderOutIn extends CommandBase { + private Intake m_intake; + private boolean isOut = false; + private long startTime; + + /** + * Uses input from opperator to run the extender motor. + * The left bumper will run the extender in and out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunExtenderOutIn(Intake subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = subsystem; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + isOut = !isOut; + startTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (isOut){ + m_intake.runExtender(0.3); + } else { + m_intake.runExtender(-0.3); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_intake.runExtender(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (startTime + 3000 < System.currentTimeMillis()) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java new file mode 100644 index 0000000..0b91068 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Leveler; +import frc4388.utility.controller.IHandController; + +public class RunLevelerWithJoystick extends CommandBase { + private Leveler m_leveler; + private IHandController m_controller; + + /** + * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. + * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunLevelerWithJoystick(Leveler subsystem, IHandController controller) { + m_leveler = subsystem; + m_controller = controller; + addRequirements(m_leveler); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double input = m_controller.getLeftXAxis(); + m_leveler.runLeveler(input); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java new file mode 100644 index 0000000..67b5cb1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; + +public class ShooterVelocityControlPID extends CommandBase { + Shooter m_shooter; + double m_targetVel; + /** + * Creates a new ShooterVelocityControlPID. + */ + public ShooterVelocityControlPID(Shooter subsystem, double targetVel) { + // Use addRequirements() here to declare subsystem dependencies. + m_shooter = subsystem; + m_targetVel = targetVel; + addRequirements(m_shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + } + + // 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/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..6b35036 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; + +public class Climber extends SubsystemBase { + CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_forwardLimit, m_reverseLimit; + /** + * Creates a new Climber. + */ + public Climber() { + m_climberMotor.restoreFactoryDefaults(); + + m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + + m_forwardLimit.enableLimitSwitch(false); + m_reverseLimit.enableLimitSwitch(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs climber motor + * @param input the voltage to run motor at + */ + public void runClimber(double input) { + m_climberMotor.set(input); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index e629ab8..9c45d3d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -21,6 +21,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -50,6 +51,8 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public DoubleSolenoid speedShift; + /** * Add your docs here. */ @@ -62,6 +65,8 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); + speedShift = new DoubleSolenoid(7,0,1); + /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); @@ -79,7 +84,6 @@ public class Drive extends SubsystemBase { m_leftBackMotor.setInverted(InvertType.FollowMaster); m_rightBackMotor.setInverted(InvertType.FollowMaster); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); @@ -244,6 +248,7 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -430,4 +435,17 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + speedShift.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + speedShift.set(DoubleSolenoid.Value.kReverse); + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index ea2ba9c..62bb30a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,18 +7,38 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { - Spark m_intakeMotor = new Spark(IntakeConstants.INTAKE_SPARK_ID); - + CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); + CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + /** * Creates a new Intake. */ public Intake() { + m_intakeMotor.restoreFactoryDefaults(); + m_extenderMotor.restoreFactoryDefaults(); + + m_intakeMotor.setIdleMode(IdleMode.kCoast); + m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); + m_extenderMotor.setInverted(false); + + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderForwardLimit.enableLimitSwitch(false); + m_extenderReverseLimit.enableLimitSwitch(false); } @Override @@ -33,4 +53,12 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + /** + * Runs extender motor + * @param input the percent output to run motor at + */ + public void runExtender(double input) { + m_extenderMotor.set(input); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java new file mode 100644 index 0000000..02df406 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.LevelerConstants; + +public class Leveler extends SubsystemBase { + CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); + + /** + * Creates a new Leveler. + */ + public Leveler() { + m_levelerMotor.restoreFactoryDefaults(); + m_levelerMotor.setIdleMode(IdleMode.kCoast); + m_levelerMotor.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs intake motor + * @param input the percent output to run motor at + */ + public void runLeveler(double input) { + m_levelerMotor.set(input); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java new file mode 100644 index 0000000..169e36f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Gains; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); + + public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + + double velP; + /** + * Creates a new Shooter subsystem. + */ + public Shooter() { + m_shooterFalcon.configFactoryDefault(); + + m_shooterFalcon.setNeutralMode(NeutralMode.Coast); + + m_shooterFalcon.setInverted(true); + + setShooterGains(); + + m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + + int closedLoopTimeMs = 1; + m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); + } + + /** + * Runs drum shooter motor. + * @param speed Speed to set the motor at + */ + public void runDrumShooter(double speed) { + m_shooterFalcon.set(speed); + } + + /** + * Configures gains for shooter PID. + */ + public void setShooterGains() { + m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + } + /** + * Runs drum shooter velocity PID. + * @param falcon Motor to use + * @param targetVel Target velocity to run motor at + */ + public void runDrumShooterVelocityPID(double targetVel, double actualVel) { + velP = actualVel/targetVel; //Percent of target + double runSpeed = actualVel + (12000*velP); //Ramp up equation + //if (runSpeed > targetVel) {runSpeed = targetVel;} + SmartDashboard.putNumber("shoot", actualVel); + runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); + } + else{ //PID Based on targetVel + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java new file mode 100644 index 0000000..a7d1bfb --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.StorageConstants; + +public class Storage extends SubsystemBase { + private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + private DigitalInput[] m_beamSensors = new DigitalInput[6]; + /** + * Creates a new Storage. + */ + public Storage() { + m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); + m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + } + + @Override + public void periodic() { + // NO + } + + /** + * Runs storage motor + * @param input the voltage to run motor at + */ + public void runStorage(double input) { + m_storageMotor.set(input); + boolean beam_on = m_beamSensors[0].get(); + + if (beam_on) { + System.err.println("Beam on"); + } else { + System.err.println("Beam off"); + } + + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index f8d42a4..a633555 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.3", + "version": "5.17.4", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.3" + "version": "5.17.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.3" + "version": "5.17.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json new file mode 100644 index 0000000..63380b6 --- /dev/null +++ b/vendordeps/REVRobotics.json @@ -0,0 +1,70 @@ +{ + "fileName": "REVRobotics.json", + "name": "REVRobotics", + "version": "1.5.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-java", + "version": "1.5.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-cpp", + "version": "1.5.1", + "libName": "SparkMax", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.5.1", + "libName": "SparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file