ACTUALLY added intake subsystem (#27)

I will add the intake stuff to the backup once it works.

Author: ngarnsworthy

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java

@@ -10,6 +10,13 @@ public static class Drivetrain {
 
     public static class Shooter {
         public static final String shooterMotorName = "shooterMotor";
-        public static final double defaultSpeed = 1;
+        public static final String rampMotorName = "rampMotor";
+        public static final double shooterSpeed = 1;
+        public static final double rampSpeed = 0.5;
+    }
+
+    public static class Intake {
+        public static final String intakeMotorName = "intakeMotor";
+        public static final double defaultSpeed = 0.5;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java

@@ -34,9 +34,9 @@ public void initialize() {
         drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
         register(drivetrain);
 
-        shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName);
+        shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName);
         register(shooter);
         coOp.getGamepadButton(GamepadKeys.Button.A).whileHeld(
-                new RunShooter(shooter, Constants.Shooter.defaultSpeed));
+                new RunShooter(shooter, Constants.Shooter.shooterSpeed, Constants.Shooter.rampSpeed));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java

@@ -6,14 +6,17 @@
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.Constants;
+import org.firstinspires.ftc.teamcode.commands.RunIntake;
 import org.firstinspires.ftc.teamcode.commands.RunShooter;
 import org.firstinspires.ftc.teamcode.commands.UserDrive;
 import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 
 @TeleOp(name = "PrimaryOpMode", group = "NormalOpModes")
 public class PrimaryOpMode extends CommandOpMode {
     private Shooter shooter;
+    private Intake intake;
     private Drivetrain drivetrain;
 
     private GamepadEx driverOp;
@@ -33,9 +36,13 @@ public void initialize() {
         );
         drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
         register(drivetrain);
-        shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName);
+        shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName);
         register(shooter);
+        intake = new Intake(hardwareMap, Constants.Intake.intakeMotorName);
+        register(intake);
         coOp.getGamepadButton(GamepadKeys.Button.A).whileHeld(
-                new RunShooter(shooter, Constants.Shooter.defaultSpeed));
+                new RunShooter(shooter, Constants.Shooter.shooterSpeed, Constants.Shooter.rampSpeed));
+        coOp.getGamepadButton(GamepadKeys.Button.B).whileHeld(
+                new RunIntake(intake, Constants.Intake.defaultSpeed));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunIntake.java

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+
+public class RunIntake extends CommandBase {
+    private final Intake subsystem;
+
+    private final double speed;
+
+    public RunIntake(Intake _subsystem, double _speed) {
+        subsystem = _subsystem;
+        addRequirements(subsystem);
+        speed = _speed;
+    }
+
+    @Override
+    public void initialize() {
+        subsystem.run(speed);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.stop();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunShooter.java

@@ -6,17 +6,18 @@
 
 public class RunShooter extends CommandBase {
     private final Shooter subsystem;
-    private final double speed;
+    private final double shooterSpeed, rampSpeed;
 
-    public RunShooter(Shooter _subsystem, double _speed) {
+    public RunShooter(Shooter _subsystem, double _shooterSpeed, double _rampSpeed) {
         subsystem = _subsystem;
         addRequirements(subsystem);
-        speed = _speed;
+        shooterSpeed = _shooterSpeed;
+        rampSpeed = _rampSpeed;
     }
 
     @Override
     public void initialize() {
-        subsystem.run(speed);
+        subsystem.run(shooterSpeed, rampSpeed);
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md

@@ -29,11 +29,6 @@ Subsystem for driving
 - UserDrive
   - Drives with controls from gamepad
   - Stops moving when done
-  - Controls
-    - Left Stick
-      - Y: Forward
-      - X: Strafe
-    - Right Stick: X: Turn
 
 ## Shooter
 
@@ -42,4 +37,30 @@ Subsystem that runs launcher
 ### Commands
 
 - RunShooter
-  - Runs shooter motors in raw mode with the specified power
+  - Runs shooter motors in raw mode with the specified power
+  - Runs ramp motors in raw mode with the specified power
+
+## Intake
+
+Subsystem that runs intake
+
+### Commands
+
+- RunIntake
+  - Runs intake motor in raw mode with the specified power
+
+# Controls
+
+## Robot Driver
+ 
+- Left Stick
+  - Y: Forward
+  - X: Strafe
+- Right Stick: 
+  - X: Turn
+
+## Co-op Driver
+
+- A: Ramp and Shooter
+- B: Intake
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java

@@ -0,0 +1,23 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class Intake extends SubsystemBase {
+    private final Motor intakeMotor;
+
+    public Intake(HardwareMap map, String intakeMotorName) {
+        intakeMotor = new Motor(map, intakeMotorName);
+        intakeMotor.setRunMode(Motor.RunMode.RawPower);
+    }
+
+    public void run(double speed) {
+        double clampedSpeed = Math.max(1.0, Math.min(-1.0, speed));
+        intakeMotor.set(clampedSpeed);
+    }
+
+    public void stop() {
+        intakeMotor.stopMotor();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java

@@ -5,20 +5,25 @@
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 public class Shooter extends SubsystemBase {
-    private final Motor shooterMotor;
+    private final Motor shooterMotor, rampMotor;
 
-    public Shooter(HardwareMap map, String motorName) {
-        shooterMotor = new Motor(map, motorName);
+    public Shooter(HardwareMap map, String shooterMotorName, String rampMotorName) {
+        shooterMotor = new Motor(map, shooterMotorName);
         shooterMotor.setRunMode(Motor.RunMode.RawPower);
+        rampMotor = new Motor(map, rampMotorName);
+        rampMotor.setRunMode(Motor.RunMode.RawPower);
     }
 
-    public void run(double speed) {
+    public void run(double shooterSpeed, double rampSpeed) {
         // Clamp speed to valid motor range [-1.0, 1.0]
-        double clampedSpeed = Math.max(-1.0, Math.min(1.0, speed));
+        double clampedSpeed = Math.max(-1.0, Math.min(1.0, shooterSpeed));
+        double _rampSpeed = Math.max(-1.0, Math.min(1.0, rampSpeed));
         shooterMotor.set(clampedSpeed);
+        rampMotor.set(_rampSpeed);
     }
 
     public void stop() {
         shooterMotor.stopMotor();
+        rampMotor.stopMotor();
     }
 }