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 db29b5d..0000000 --- a/src/main/java/frc/robot/Commands/ExampleCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ExampleCommand extends CommandBase { - /** Creates a new ExampleCommand. */ - public ExampleCommand() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // 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/IntakeCommand.java b/src/main/java/frc/robot/Commands/IntakeCommand.java new file mode 100644 index 0000000..c17585e --- /dev/null +++ b/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.Subsystems.IntakeSubsystem; + +public class IntakeCommand extends CommandBase { + + IntakeSubsystem intakeSubsystem; + boolean isCone;//wheather or not your dealing with a cone or a cube + boolean isIntake;//if its intake or outtake + + + /** Creates a new LoadCube. */ + public IntakeCommand(IntakeSubsystem intakeSubsystem, boolean isCone, boolean isIntake) { + this.intakeSubsystem = intakeSubsystem; + this.isCone = isCone; + this.isIntake = isIntake; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if(isCone && isIntake){ + intakeSubsystem.SetMotorPower(Constants.Intake.CONE_LOADING_SPEED); + }else if(!isCone && isIntake){ + intakeSubsystem.SetMotorPower(Constants.Intake.CUBE_LOADING_SPEED); + }else if(isCone && !isIntake){ + intakeSubsystem.SetMotorPower(Constants.Intake.CONE_UNLOADING_SPEED); + }else if(!isCone && !isIntake){ + intakeSubsystem.SetMotorPower(Constants.Intake.CUBE_UNLOADING_SPEED); + } + + intakeSubsystem.SetObjectType(isCone); + } + + // 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 true; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 915ef9c..0989db0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,4 +14,20 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + public static class Intake{ + + public static final int INTAKE_MOTOR_NUMBER = 0; + + public static final double CUBE_LOADING_SPEED = -.5d; + public static final double CONE_LOADING_SPEED = .5d; + + public static final double CUBE_UNLOADING_SPEED = -.5d; + public static final double CONE_UNLOADING_SPEED = .5d; + + public static final double CUBE_STATOR_LIMIT = 70; + public static final double CONE_STATOR_LIMIT = 70; + + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d3b8e4..43ec846 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,14 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.Commands.IntakeCommand; +import frc.robot.Subsystems.IntakeSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + /** * 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} @@ -13,6 +21,11 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... + //XBOX CONTROLLER + public CommandXboxController driverB = new CommandXboxController(0); + + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings @@ -41,6 +54,7 @@ public void containerTeleopInit() { */ public void containerMatchStarting() { // runs when the match starts + } /** @@ -50,5 +64,7 @@ public void containerMatchStarting() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + driverB.rightTrigger().onTrue(new IntakeCommand(intakeSubsystem, false, true)); + driverB.leftTrigger().onTrue(new IntakeCommand(intakeSubsystem, true, true)); } } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 4316f37..87f6cda 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,14 +4,73 @@ package frc.robot.Subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class IntakeSubsystem extends SubsystemBase { + + + //MOTORS + TalonFX intakeMotor; + private double motorPower; + + private LinearFilter filter; + private double filterOutput; + + private boolean isCone; + /** Creates a new IntakeSubsystem. */ - public IntakeSubsystem() {} + public IntakeSubsystem() { + + //setup the motor + intakeMotor = new TalonFX(Constants.Intake.INTAKE_MOTOR_NUMBER); + + intakeMotor.configFactoryDefault(); + + intakeMotor.clearStickyFaults(); + + intakeMotor.setSelectedSensorPosition(0d); + + intakeMotor.setNeutralMode(NeutralMode.Brake); + + filter = LinearFilter.movingAverage(30); + + } + + public boolean IsLoaded(){//returns true when the cube or cone has finished loading + boolean loaded = false; + if(isCone && filterOutput >= Constants.Intake.CONE_STATOR_LIMIT){ + loaded = true; + }else if(!isCone && filterOutput >= Constants.Intake.CONE_STATOR_LIMIT){ + loaded = true; + } + return loaded; + } + + public void SetMotorPower(double motorPower){ + this.motorPower = motorPower; + } + + public void SetObjectType(boolean isCone){ + this.isCone = isCone; + } + @Override public void periodic() { // This method will be called once per scheduler run + filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); + + //set the motor power to the var + intakeMotor.set(TalonFXControlMode.PercentOutput, motorPower); + + //if we are done loading than set the motor power to 0 + if(IsLoaded()) + SetMotorPower(0); } }