Done with subsystem, ready for testing

yay!!!

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;
+  }
+}

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;
+
+    }
+
 }

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));
   }
 }

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);
   }
 }