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 final class Constants +{ + public static final double P = 0.00010; + public static final double I = 0.0; + public static final double D = 0.0000; + public static final double Iz = 0.0; + public static final double FF = 0.000175; } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 4d2951a..620a1dc 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -9,21 +9,25 @@ import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ -public final class Main { - private Main() { - } - - /** - * Main initialization function. Do not perform any initialization here. - * - *
If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } +public final class Main +{ + /** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ + private Main() + { + + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *
If you change your main robot class, change the parameter type. + */ + public static void main(String... args) + { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bd546a8..35703cc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,106 +8,68 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -/** - * 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 Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - /** - * 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() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); +public class Robot extends TimedRobot +{ + public RobotContainer robotContainer; + @Override + public void robotInit() + { + robotContainer = new RobotContainer(); } - } - - /** - * 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(); + + @Override + public void robotPeriodic() + { + RobotContainer.driveSubsystem.updatePID(); + } + + @Override + public void disabledInit() + { + + } + + @Override + public void disabledPeriodic() + { + + } + + @Override + public void autonomousInit() + { + + } + + @Override + public void autonomousPeriodic() + { + } - } - - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() { - } - - @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() { - } + @Override + public void teleopInit() + { + RobotContainer.driveCommand.schedule(); + } + + @Override + public void teleopPeriodic() + { + CommandScheduler.getInstance().run(); + } + + @Override + public void testInit() + { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() + { + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f60f0d1..abea229 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,51 +7,61 @@ package frc.robot; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.conveyor.ToggleConveyorForwardCommand; +import frc.robot.commands.conveyor.ToggleConveyorReverseCommand; +import frc.robot.commands.conveyor.ToggleConveyorSolenoidCommand; +import frc.robot.commands.drive.DriveCommand; +import frc.robot.commands.intake.ToggleExtensionCommand; +import frc.robot.commands.intake.ToggleRotationCommand; +import frc.robot.commands.shooter.ShooterCommand; +import frc.robot.subsystems.ConveyorSubsystem; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; -/** - * 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... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - - private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); - - - - /** - * 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 - */ - public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return m_autoCommand; - } +public class RobotContainer +{ + public static Joystick controller = new Joystick(0); + public static JoystickButton buttonOne = new JoystickButton(controller, 1); + public static JoystickButton buttonTwo = new JoystickButton(controller, 2); + public static JoystickButton buttonThree = new JoystickButton(controller, 3); + public static JoystickButton buttonFour = new JoystickButton(controller, 4); + public static JoystickButton buttonFive = new JoystickButton(controller, 5); + public static JoystickButton buttonSix = new JoystickButton(controller, 6); + public static int joystick = 0; + + public static ShuffleboardTab sbTab = Shuffleboard.getTab("Subsystems"); + public static DriveSubsystem driveSubsystem = new DriveSubsystem(); + public static IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + public static ConveyorSubsystem conveyorSubsystem = new ConveyorSubsystem(); + public static ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + + public static DriveCommand driveCommand = new DriveCommand(); + + + public RobotContainer() + { + driveSubsystem.setDefaultCommand(driveCommand); + configureButtonBindings(); + } + + private void configureButtonBindings() + { + buttonOne.whenReleased(new ToggleExtensionCommand()); + buttonTwo.whenReleased(new ToggleRotationCommand()); + buttonThree.whileHeld(new ShooterCommand(10.0)); + buttonFour.whenReleased(new ToggleConveyorForwardCommand()); + buttonFive.whenReleased(new ToggleConveyorReverseCommand()); + buttonSix.whenReleased(new ToggleConveyorSolenoidCommand()); + } + + public Command getAutonomousCommand() + { + return null; + } } diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 42c175f..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,51 +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 frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * An example command that uses an example subsystem. - */ -public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/conveyor/ToggleConveyorForwardCommand.java b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorForwardCommand.java new file mode 100644 index 0000000..afd7c9d --- /dev/null +++ b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorForwardCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.conveyor; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ToggleConveyorForwardCommand extends CommandBase +{ + public ToggleConveyorForwardCommand() + { + addRequirements(RobotContainer.conveyorSubsystem); + } + + @Override + public void initialize() + { + RobotContainer.conveyorSubsystem.toggleRotation(false); + } + + @Override + public boolean isFinished() + { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/conveyor/ToggleConveyorReverseCommand.java b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorReverseCommand.java new file mode 100644 index 0000000..e546dd5 --- /dev/null +++ b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorReverseCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.conveyor; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ToggleConveyorReverseCommand extends CommandBase +{ + public ToggleConveyorReverseCommand() + { + addRequirements(RobotContainer.conveyorSubsystem); + } + + @Override + public void initialize() + { + RobotContainer.conveyorSubsystem.toggleRotation(true); + } + + @Override + public boolean isFinished() + { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/conveyor/ToggleConveyorSolenoidCommand.java b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorSolenoidCommand.java new file mode 100644 index 0000000..c2436ab --- /dev/null +++ b/src/main/java/frc/robot/commands/conveyor/ToggleConveyorSolenoidCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.conveyor; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ToggleConveyorSolenoidCommand extends CommandBase +{ + public ToggleConveyorSolenoidCommand() + { + addRequirements(RobotContainer.conveyorSubsystem); + } + + @Override + public void initialize() + { + RobotContainer.conveyorSubsystem.toggleSolenoid(); + } + + @Override + public boolean isFinished() + { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java new file mode 100644 index 0000000..701575d --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class DriveCommand extends CommandBase +{ + private Joystick joystick = new Joystick(RobotContainer.joystick); + + public DriveCommand() + { + addRequirements(RobotContainer.driveSubsystem); + } + + @Override + public void execute() + { + RobotContainer.driveSubsystem.arcadeDrive(joystick.getRawAxis(1), joystick.getRawAxis(2) * 0.7); + } + + @Override + public boolean isFinished() + { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/ToggleExtensionCommand.java b/src/main/java/frc/robot/commands/intake/ToggleExtensionCommand.java new file mode 100644 index 0000000..14b3763 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/ToggleExtensionCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ToggleExtensionCommand extends CommandBase +{ + public ToggleExtensionCommand() + { + addRequirements(RobotContainer.intakeSubsystem); + } + + @Override + public void initialize() + { + RobotContainer.intakeSubsystem.toggleExtension(); + } + + @Override + public boolean isFinished() + { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/ToggleRotationCommand.java b/src/main/java/frc/robot/commands/intake/ToggleRotationCommand.java new file mode 100644 index 0000000..7fe5a31 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/ToggleRotationCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ToggleRotationCommand extends CommandBase +{ + public ToggleRotationCommand() + { + addRequirements(RobotContainer.intakeSubsystem); + } + + @Override + public void initialize() + { + RobotContainer.intakeSubsystem.toggleRotation(); + } + + @Override + public boolean isFinished() + { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/ShooterCommand.java b/src/main/java/frc/robot/commands/shooter/ShooterCommand.java new file mode 100644 index 0000000..b1812f4 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ShooterCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; + +public class ShooterCommand extends CommandBase +{ + private double velocity; + + public ShooterCommand(double velocity) + { + addRequirements(RobotContainer.shooterSubsystem); + this.velocity = velocity; + } + + @Override + public void initialize() + { + RobotContainer.shooterSubsystem.setPIDReference(velocity); + } + + @Override + public void end(boolean interrupted) + { + RobotContainer.shooterSubsystem.setPIDReference(0.0); + } + + @Override + public boolean isFinished() + { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java new file mode 100644 index 0000000..8775002 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -0,0 +1,137 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class ConveyorSubsystem extends SubsystemBase +{ + private static CANSparkMax motor; + private DoubleSolenoid solenoid; + private CANEncoder encoder; + private CANPIDController PIDController; + private double P, I, D, Iz, FF, maxOutput, minOutput; + private boolean spinning; + + public ConveyorSubsystem() + { + motor = new CANSparkMax(10, MotorType.kBrushless); + motor.restoreFactoryDefaults(); + motor.setOpenLoopRampRate(0.2); + solenoid = new DoubleSolenoid(3, 4); + solenoid.set(Value.kReverse); + encoder = motor.getEncoder(); + PIDController = motor.getPIDController(); + + spinning = false; + + P = Constants.P; + I = Constants.I; + D = Constants.D; + Iz = Constants.Iz; + FF = Constants.FF; + maxOutput = 1; + minOutput = -1; + + PIDController.setP(P); + PIDController.setI(I); + PIDController.setD(D); + PIDController.setIZone(Iz); + PIDController.setFF(FF); + PIDController.setOutputRange(minOutput, maxOutput); + PIDController.setReference(0.0, ControlType.kVelocity); + + RobotContainer.sbTab.add("Conveyor Pos", encoder.getPosition()).withPosition(3, 0).withSize(1, 1); + RobotContainer.sbTab.add("Conveyor Vel", encoder.getVelocity()).withPosition(3, 1).withSize(1, 1); + //RobotContainer.sbTab.add("Conveyor PID", PIDController).withWidget(BuiltInWidgets.kPIDController).withPosition(10, 1).withSize(1, 2); + + SmartDashboard.putNumber("Conveyor - P", P); + SmartDashboard.putNumber("Conveyor - I", I); + SmartDashboard.putNumber("Conveyor - D", D); + SmartDashboard.putNumber("Conveyor - Iz", Iz); + SmartDashboard.putNumber("Conveyor - FF", FF); + SmartDashboard.putNumber("Conveyor - minOutput", minOutput); + SmartDashboard.putNumber("Conveyor - maxOutput", maxOutput); + } + + public void updatePID() + { + double p = SmartDashboard.getNumber("Conveyor - P", 0); + double i = SmartDashboard.getNumber("Conveyor - I", 0); + double d = SmartDashboard.getNumber("Conveyor - D", 0); + double min = SmartDashboard.getNumber("Conveyor - minOutput", 0); + double max = SmartDashboard.getNumber("Conveyor - maxOutput", 0); + + if(p != P) + { + PIDController.setP(p); + P = p; + } + if(i != I) + { + PIDController.setI(i); + I = i; + } + if(d != D) + { + PIDController.setD(d); + D = d; + } + if(max != maxOutput || min != minOutput) + { + PIDController.setOutputRange(min, max); + minOutput = min; + maxOutput = max; + } + } + + public void setPIDReference(double velocity) + { + PIDController.setReference(velocity, ControlType.kVelocity); + } + + public void toggleRotation(boolean reversed) + { + if(spinning) + { + setPIDReference(0.0); + } + else + { + if(reversed) + { + setPIDReference(-10.0); + } + else + { + setPIDReference(10.0); + } + } + spinning = !spinning; + } + + public void toggleSolenoid() + { + switch(solenoid.get()) + { + case kForward: + solenoid.set(Value.kReverse); + break; + case kReverse: + solenoid.set(Value.kForward); + break; + default: + solenoid.set(Value.kReverse); + break; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..81faf85 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,167 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class DriveSubsystem extends SubsystemBase +{ + private static CANSparkMax leftMotor1; + private static CANSparkMax leftMotor2; + private static CANSparkMax leftMotor3; + private static CANSparkMax rightMotor1; + private static CANSparkMax rightMotor2; + private static CANSparkMax rightMotor3; + private DifferentialDrive differentialDrive; + private CANPIDController leftPIDController; + private CANPIDController rightPIDController; + private CANEncoder leftEncoder; + private CANEncoder rightEncoder; + private double P, I, D, Iz, FF, maxOutput, minOutput; + + public DriveSubsystem() + { + leftMotor1 = new CANSparkMax(1, MotorType.kBrushless); + leftMotor2 = new CANSparkMax(2, MotorType.kBrushless); + leftMotor3 = new CANSparkMax(3, MotorType.kBrushless); + leftMotor1.restoreFactoryDefaults(); + leftMotor2.restoreFactoryDefaults(); + leftMotor3.restoreFactoryDefaults(); + leftMotor1.setOpenLoopRampRate(0.2); + leftMotor2.setOpenLoopRampRate(0.2); + leftMotor3.setOpenLoopRampRate(0.2); + leftMotor2.follow(leftMotor1); + leftMotor3.follow(leftMotor1); + + rightMotor1 = new CANSparkMax(4, MotorType.kBrushless); + rightMotor2 = new CANSparkMax(5, MotorType.kBrushless); + rightMotor3 = new CANSparkMax(6, MotorType.kBrushless); + rightMotor1.restoreFactoryDefaults(); + rightMotor2.restoreFactoryDefaults(); + rightMotor3.restoreFactoryDefaults(); + rightMotor1.setOpenLoopRampRate(0.2); + rightMotor2.setOpenLoopRampRate(0.2); + rightMotor3.setOpenLoopRampRate(0.2); + rightMotor2.follow(rightMotor1); + rightMotor3.follow(rightMotor1); + + differentialDrive = new DifferentialDrive(leftMotor1, rightMotor1); + + leftPIDController = leftMotor1.getPIDController(); + rightPIDController = rightMotor1.getPIDController(); + + leftEncoder = leftMotor1.getEncoder(); + rightEncoder = rightMotor1.getEncoder(); + + P = Constants.P; + I = Constants.I; + D = Constants.D; + Iz = Constants.Iz; + FF = Constants.FF; + maxOutput = 1; + minOutput = -1; + + leftPIDController.setP(P); + leftPIDController.setI(I); + leftPIDController.setD(D); + leftPIDController.setIZone(Iz); + leftPIDController.setFF(FF); + leftPIDController.setOutputRange(minOutput, maxOutput); + + rightPIDController.setP(P); + rightPIDController.setI(I); + rightPIDController.setD(D); + rightPIDController.setIZone(Iz); + rightPIDController.setFF(FF); + rightPIDController.setOutputRange(minOutput, maxOutput); + + RobotContainer.sbTab.add("L Drive Pos", leftEncoder.getPosition()).withPosition(0, 0).withSize(1, 1); + RobotContainer.sbTab.add("L Drive Vel", leftEncoder.getVelocity()).withPosition(0, 1).withSize(1, 1); + RobotContainer.sbTab.add("R Drive Pos", rightEncoder.getPosition()).withPosition(1, 0).withSize(1, 1); + RobotContainer.sbTab.add("R Drive Vel", rightEncoder.getVelocity()).withPosition(1, 1).withSize(1, 1); + + //RobotContainer.driveTab.add("Left Drive PID", leftPIDController).withWidget(BuiltInWidgets.kPIDController).withPosition(0, 1).withSize(1, 2); + //RobotContainer.driveTab.add("Right Drive PID", rightPIDController).withWidget(BuiltInWidgets.kPIDController).withPosition(4, 1).withSize(1, 2); + + SmartDashboard.putNumber("Drive - P", P); + SmartDashboard.putNumber("Drive - I", I); + SmartDashboard.putNumber("Drive - D", D); + SmartDashboard.putNumber("Drive - Iz", Iz); + SmartDashboard.putNumber("Drive - FF", FF); + SmartDashboard.putNumber("Drive - minOutput", minOutput); + SmartDashboard.putNumber("Drive - maxOutput", maxOutput); + } + + public void updatePID() + { + double p = SmartDashboard.getNumber("Drive - P", 0); + double i = SmartDashboard.getNumber("Drive - I", 0); + double d = SmartDashboard.getNumber("Drive - D", 0); + double min = SmartDashboard.getNumber("Drive - minOutput", 0); + double max = SmartDashboard.getNumber("Drive - maxOutput", 0); + + if(p != P) + { + leftPIDController.setP(p); + rightPIDController.setP(p); + P = p; + } + if(i != I) + { + leftPIDController.setI(i); + rightPIDController.setI(i); + I = i; + } + if(d != D) + { + leftPIDController.setD(d); + rightPIDController.setD(d); + D = d; + } + if(max != maxOutput || min != minOutput) + { + minOutput = min; + maxOutput = max; + } + } + + @Override + public void periodic() + { + SmartDashboard.putNumber("Drive - leftVelocity", -RobotContainer.driveSubsystem.getLeftVelocity()); + SmartDashboard.putNumber("Drive - rightVelocity", -RobotContainer.driveSubsystem.getRightVelocity()); + } + + public void arcadeDrive(double velocity, double heading) + { + differentialDrive.arcadeDrive(velocity, heading, true); + } + + public void setLeftPIDReference(double velocity) + { + leftPIDController.setReference(velocity, ControlType.kVelocity); + } + + public void setRightPidVelocitySetpoint(double velocity) + { + rightPIDController.setReference(velocity, ControlType.kVelocity); + } + + public double getLeftVelocity() + { + return leftEncoder.getVelocity(); + } + + public double getRightVelocity() + { + return rightEncoder.getVelocity(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index b2585b9..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,24 +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 frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** - * Creates a new ExampleSubsystem. - */ - public ExampleSubsystem() { - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..b64edf7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,136 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class IntakeSubsystem extends SubsystemBase +{ + private static CANSparkMax sparkMax; + private static CANSparkMax hopperMotor; + private DoubleSolenoid extensionSolenoid; + private CANPIDController PIDController; + private CANEncoder encoder; + private double P, I, D, Iz, FF, maxOutput, minOutput; + private boolean spinning; + private boolean extended; + + public IntakeSubsystem() + { + sparkMax = new CANSparkMax(7, MotorType.kBrushless); + hopperMotor = new CANSparkMax(8, MotorType.kBrushless); + sparkMax.restoreFactoryDefaults(); + hopperMotor.restoreFactoryDefaults(); + sparkMax.setOpenLoopRampRate(0.2); + hopperMotor.setOpenLoopRampRate(0.2); + hopperMotor.follow(sparkMax); + + PIDController = sparkMax.getPIDController(); + encoder = sparkMax.getEncoder(); + + extensionSolenoid = new DoubleSolenoid(1, 2); + extensionSolenoid.set(Value.kOff); + + spinning = false; + extended = false; + + P = Constants.P; + I = Constants.I; + D = Constants.D; + Iz = Constants.Iz; + FF = Constants.FF; + maxOutput = 1; + minOutput = -1; + + PIDController.setP(P); + PIDController.setI(I); + PIDController.setD(D); + PIDController.setIZone(Iz); + PIDController.setFF(FF); + PIDController.setOutputRange(minOutput, maxOutput); + PIDController.setReference(0.0, ControlType.kVelocity); + + RobotContainer.sbTab.add("Intake Pos", encoder.getPosition()).withPosition(2, 0).withSize(1, 1); + RobotContainer.sbTab.add("Intake Vel", encoder.getVelocity()).withPosition(2, 1).withSize(1, 1); + //RobotContainer.sbTab.add("Intake PID", PIDController).withWidget(BuiltInWidgets.kPIDController).withPosition(7, 1).withSize(1, 2); + + SmartDashboard.putNumber("Intake - P", P); + SmartDashboard.putNumber("Intake - I", I); + SmartDashboard.putNumber("Intake - D", D); + SmartDashboard.putNumber("Intake - Iz", Iz); + SmartDashboard.putNumber("Intake - FF", FF); + SmartDashboard.putNumber("Intake - minOutput", minOutput); + SmartDashboard.putNumber("Intake - maxOutput", maxOutput); + } + + public void updatePID() + { + double p = SmartDashboard.getNumber("Intake - P", 0); + double i = SmartDashboard.getNumber("Intake - I", 0); + double d = SmartDashboard.getNumber("Intake - D", 0); + double min = SmartDashboard.getNumber("Intake - minOutput", 0); + double max = SmartDashboard.getNumber("Intake - maxOutput", 0); + + if(p != P) + { + PIDController.setP(p); + P = p; + } + if(i != I) + { + PIDController.setI(i); + I = i; + } + if(d != D) + { + PIDController.setD(d); + D = d; + } + if(max != maxOutput || min != minOutput) + { + PIDController.setOutputRange(min, max); + minOutput = min; + maxOutput = max; + } + } + + public void setPIDReference(double velocity) + { + PIDController.setReference(velocity, ControlType.kVelocity); + } + + public void toggleExtension() + { + if(extended) + { + extensionSolenoid.set(Value.kReverse); + } + else + { + extensionSolenoid.set(Value.kForward); + } + extended = !extended; + } + + public void toggleRotation() + { + if(spinning) + { + setPIDReference(0.0); + } + else + { + setPIDReference(10.0); + } + spinning = !spinning; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..f87c1f7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,99 @@ +package frc.robot.subsystems; + +import java.util.function.BooleanSupplier; + +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotContainer; + +public class ShooterSubsystem extends SubsystemBase +{ + private static CANSparkMax sparkMax; + private CANPIDController PIDController; + private CANEncoder encoder; + private double P, I, D, Iz, FF, maxOutput, minOutput; + private boolean distanceBool; + + public ShooterSubsystem() + { + sparkMax = new CANSparkMax(9, MotorType.kBrushless); + sparkMax.restoreFactoryDefaults(); + sparkMax.setOpenLoopRampRate(0.2); + + PIDController = sparkMax.getPIDController(); + encoder = sparkMax.getEncoder(); + + P = Constants.P; + I = Constants.I; + D = Constants.D; + Iz = Constants.Iz; + FF = Constants.FF; + maxOutput = 1; + minOutput = -1; + + PIDController.setP(P); + PIDController.setI(I); + PIDController.setD(D); + PIDController.setIZone(Iz); + PIDController.setFF(FF); + PIDController.setOutputRange(minOutput, maxOutput); + PIDController.setReference(0.5, ControlType.kVelocity); + + RobotContainer.sbTab.add("Shooter Pos", encoder.getPosition()).withPosition(4, 0).withSize(1, 1); + RobotContainer.sbTab.add("Shooter Vel", encoder.getVelocity()).withPosition(4, 1).withSize(1, 1); + //RobotContainer.sbTab.add("Intake PID", PIDController).withWidget(BuiltInWidgets.kPIDController).withPosition(7, 3).withSize(1, 2); + + SmartDashboard.putBoolean("Shooter Dist", distanceBool); + + SmartDashboard.putNumber("Shooter - P", P); + SmartDashboard.putNumber("Shooter - I", I); + SmartDashboard.putNumber("Shooter - D", D); + SmartDashboard.putNumber("Shooter - Iz", Iz); + SmartDashboard.putNumber("Shooter - FF", FF); + SmartDashboard.putNumber("Shooter - minOutput", minOutput); + SmartDashboard.putNumber("Shooter - maxOutput", maxOutput); + } + + public void updatePID() + { + double p = SmartDashboard.getNumber("Shooter - P", 0); + double i = SmartDashboard.getNumber("Shooter - I", 0); + double d = SmartDashboard.getNumber("Shooter - D", 0); + double min = SmartDashboard.getNumber("Shooter - minOutput", 0); + double max = SmartDashboard.getNumber("Shooter - maxOutput", 0); + + if(p != P) + { + PIDController.setP(p); + P = p; + } + if(i != I) + { + PIDController.setI(i); + I = i; + } + if(d != D) + { + PIDController.setD(d); + D = d; + } + if(max != maxOutput || min != minOutput) + { + PIDController.setOutputRange(min, max); + minOutput = min; + maxOutput = max; + } + } + + public void setPIDReference(double velocity) + { + PIDController.setReference(velocity, ControlType.kVelocity); + } +} \ No newline at end of file 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