Manipulator

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

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.
-   *
-   * <p>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.
+     *
+     * <p>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. */

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<Command> 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() {}

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

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

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.*;
 

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