diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 3a841ee..b192f17 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -51,6 +51,8 @@ public static final class Driver { public static final class Manipulator { public static final int port = 1; public static final int Y = Button.kY.value; + public static final int INTAKE_BUTTON = Button.kA.value; //get real button later + public static final int OUTTAKE_BUTTON = Button.kB.value; //get real button later } public static final double JOY_THRESH = 0.13; @@ -268,5 +270,20 @@ public static class LimeLightc { public static final int[] sampleLL1_VALID_IDS = {1, 2, 12, 13}; public static final int[] sampleLL2_VALID_IDS = {1, 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; } + //#region Manipulator + public static final class IntakeC { // FIXME get real values for intake and outtake + public static final int INTAKE_ID = 1; + public static final double INTAKE_SPEED = 0.1; + } + public static final class OuttakeC { + public static final int OUTTAKE_ID = 14; + public static final int FEEDER_ID = 2; + public static final double OUTTAKE_RPM = 60; + public static final double OUTTAKE_KP = 0.01; + public static final double OUTTAKE_KI = 0; + public static final double OUTTAKE_KD = 0; + public static final int FEEDER_SPEED = 1; + public static final int DELAY = 0; + } } //#endregion \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java index 0c602ae..0c80009 100644 --- a/src/main/java/org/carlmontrobotics/Robot.java +++ b/src/main/java/org/carlmontrobotics/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + /** * The methods in this class are called automatically corresponding to each mode, as described in * the TimedRobot documentation. If you change the name of this class or the package after creating @@ -17,64 +18,64 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - public Robot() { - // 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 20 ms, 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) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + public Robot() { + // 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 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 every 20 ms, 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) { + CommandScheduler.getInstance().schedule(m_autonomousCommand); + } } + + /** 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. */ diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b921a51..0469916 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,9 +8,6 @@ //199 files import org.carlmontrobotics.subsystems.*; - -import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; - import java.util.ArrayList; import java.util.List; @@ -48,13 +45,15 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; + //control bindings import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -//constats +//constants import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OuttakeC; import org.carlmontrobotics.Constants.OI.Driver; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; @@ -63,7 +62,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - +import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; +import org.carlmontrobotics.commands.ManipulatorCommands.IntakeBalls; +import org.carlmontrobotics.commands.ManipulatorCommands.ShootBalls; public class RobotContainer { @@ -74,6 +75,8 @@ public class RobotContainer { public final Limelight limelight = new Limelight(); public final Drivetrain drivetrain = new Drivetrain(limelight); + public final Intake intake = new Intake(); + public final Outtake outtake = new Outtake(); private SendableChooser autoChooser = new SendableChooser<>(); public boolean alignOverride = true; @@ -86,9 +89,6 @@ public RobotContainer() { SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putBoolean("AlignOverride", alignOverride); - SmartDashboard.putBoolean("AutoScoring", autoScoring); - SmartDashboard.putBoolean("AlignOverride", true); //#endregion setDefaultCommands(); setBindingsDriver(); @@ -116,7 +116,12 @@ private void setBindingsDriver() { .onFalse(new InstantCommand(() -> drivetrain.setExtraSpeedMult(0))); } - private void setBindingsManipulator() {} + private void setBindingsManipulator() { + new JoystickButton(manipulatorController, Manipulator.INTAKE_BUTTON) + .whileTrue(new IntakeBalls(intake)); + new JoystickButton(manipulatorController, Manipulator.OUTTAKE_BUTTON) + .whileTrue(new ShootBalls(outtake, outtake)); + } //#endregion //#region AutoMaking private void RegisterAutoCommands() {} diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java new file mode 100644 index 0000000..100311d --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java @@ -0,0 +1,41 @@ +// 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.carlmontrobotics.commands.ManipulatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; + +import org.carlmontrobotics.subsystems.Intake; +import org.carlmontrobotics.Constants.IntakeC; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class IntakeBalls extends Command { + Intake intake; + /** Creates a new IntakeBalls. */ + public IntakeBalls(Intake intake) { + addRequirements(this.intake = intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intake.spinIntake(IntakeC.INTAKE_SPEED); + } + + // 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) { + intake.spinIntake(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/ShootBalls.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/ShootBalls.java new file mode 100644 index 0000000..619999b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/ShootBalls.java @@ -0,0 +1,53 @@ +// 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.carlmontrobotics.commands.ManipulatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.Timer; + +import org.carlmontrobotics.subsystems.Outtake; + +import org.carlmontrobotics.Constants.OuttakeC; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ShootBalls extends Command { + Outtake outtake; + Outtake feeder; + Timer timer = new Timer(); + /** Creates a new OuttakeBalls. */ + public ShootBalls(Outtake outtake, Outtake feeder) { + addRequirements(this.outtake = outtake); + addRequirements(this.feeder = feeder); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.start(); + feeder.spinFeeder(OuttakeC.FEEDER_SPEED); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(timer.get() > OuttakeC.DELAY){ + outtake.spinOuttake(OuttakeC.OUTTAKE_RPM); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + outtake.outtake.set(0); + outtake.feeder.set(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 404994a..57aff44 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -113,6 +113,7 @@ import org.carlmontrobotics.subsystems.Limelight; import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; + import static org.carlmontrobotics.Constants.Drivetrainc.*; import static org.carlmontrobotics.Constants.LimeLightc.*; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Intake.java b/src/main/java/org/carlmontrobotics/subsystems/Intake.java new file mode 100644 index 0000000..f5cf450 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Intake.java @@ -0,0 +1,41 @@ +// 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.carlmontrobotics.subsystems; + +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import org.carlmontrobotics.Constants.IntakeC; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +public class Intake extends SubsystemBase { + SparkFlex intake; + /** Creates a new Intake. */ + public Intake() { + intake = MotorControllerFactory.createSparkFlex(IntakeC.INTAKE_ID); + + final SparkFlexConfig intakeConfig = new SparkFlexConfig(); + intakeConfig.idleMode(IdleMode.kCoast); + intake.configure(intakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public void spinIntake(double intakeSpeed) { + intake.set(intakeSpeed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Speed", intake.get()); + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Outtake.java b/src/main/java/org/carlmontrobotics/subsystems/Outtake.java new file mode 100644 index 0000000..481515d --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Outtake.java @@ -0,0 +1,47 @@ +// 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.carlmontrobotics.subsystems; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import org.carlmontrobotics.Constants.OuttakeC; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.SparkClosedLoopController; + +public class Outtake extends SubsystemBase { + public SparkFlex outtake; + public SparkFlex feeder; + private SparkClosedLoopController pidController; + /** Creates a new Outtake. */ + public Outtake() { + outtake = MotorControllerFactory.createSparkFlex(OuttakeC.OUTTAKE_ID); + feeder = MotorControllerFactory.createSparkFlex(OuttakeC.FEEDER_ID); + pidController = outtake.getClosedLoopController(); + final SparkFlexConfig outtakeConfig = new SparkFlexConfig(); + outtakeConfig.idleMode(IdleMode.kCoast); + outtakeConfig.closedLoop.pid(OuttakeC.OUTTAKE_KP, OuttakeC.OUTTAKE_KI, OuttakeC.OUTTAKE_KD); + } + + public void spinOuttake(double input) { + pidController.setSetpoint(input, ControlType.kDutyCycle); + } + + public void spinFeeder(double input) { + feeder.set(input); + } + + @Override + public void periodic() {} + // This method will be called once per scheduler run +}