From 9b282f4a6dd39cafc32dbc984beaa0ddbfeb7d3c Mon Sep 17 00:00:00 2001
From: Ryan Cahoon <ryan.m.cahoon@gmail.com>
Date: Sun, 4 Oct 2020 22:20:17 +0000
Subject: [PATCH] Intake tutorial

---
 .../com/team766/robot/AutonomousModes.java    |  1 +
 src/main/java/com/team766/robot/OI.java       |  7 +++++
 src/main/java/com/team766/robot/Robot.java    |  2 ++
 .../com/team766/robot/mechanisms/Intake.java  | 28 +++++++++++++++++++
 .../robot/procedures/CollectBalls.java        | 16 +++++++++++
 .../team766/robot/procedures/StartIntake.java | 16 +++++++++++
 .../team766/robot/procedures/StopIntake.java  | 16 +++++++++++
 7 files changed, 86 insertions(+)
 create mode 100644 src/main/java/com/team766/robot/mechanisms/Intake.java
 create mode 100644 src/main/java/com/team766/robot/procedures/CollectBalls.java
 create mode 100644 src/main/java/com/team766/robot/procedures/StartIntake.java
 create mode 100644 src/main/java/com/team766/robot/procedures/StopIntake.java

diff --git a/src/main/java/com/team766/robot/AutonomousModes.java b/src/main/java/com/team766/robot/AutonomousModes.java
index 8d4cc3dd..6c2da794 100644
--- a/src/main/java/com/team766/robot/AutonomousModes.java
+++ b/src/main/java/com/team766/robot/AutonomousModes.java
@@ -13,6 +13,7 @@ public class AutonomousModes {
 		//    new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)),
 		//    new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
 
+		new AutonomousMode("CollectBalls", () -> new CollectBalls()),
 		new AutonomousMode("Launch", () -> new Launch()),
 		new AutonomousMode("DriveSquare", () -> new DriveSquare()),
 		new AutonomousMode("TurnRight", () -> new TurnRight()),
diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java
index ded13e7b..9b0aab50 100644
--- a/src/main/java/com/team766/robot/OI.java
+++ b/src/main/java/com/team766/robot/OI.java
@@ -39,6 +39,13 @@ public void run(final Context context) {
 			if (joystick0.getButtonPressed(1)) {
 				context.startAsync(new Launch());
 			}
+
+			if (joystick1.getButtonPressed(2)) {
+				context.startAsync(new StartIntake());
+			}
+			if (joystick1.getButtonPressed(3)) {
+				context.startAsync(new StopIntake());
+			}
 		}
 	}
 }
diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java
index 37de81eb..1055c9b6 100644
--- a/src/main/java/com/team766/robot/Robot.java
+++ b/src/main/java/com/team766/robot/Robot.java
@@ -6,10 +6,12 @@ public class Robot {
 	// Declare mechanisms here
 	public static Drive drive;
 	public static Launcher launcher;
+	public static Intake intake;
 
 	public static void robotInit() {
 		// Initialize mechanisms here
 		drive = new Drive();
 		launcher = new Launcher();
+		intake = new Intake();
 	}
 }
diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java
new file mode 100644
index 00000000..3b64aad5
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Intake.java
@@ -0,0 +1,28 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+import com.team766.hal.MotorController;
+
+public class Intake extends Mechanism {
+	private SolenoidController m_extend;
+	private MotorController m_wheels;
+
+	public Intake() {
+		m_extend = RobotProvider.instance.getSolenoid("intakeArm");
+		m_wheels = RobotProvider.instance.getMotor("intakeWheels");
+	}
+
+	public void setExtended(boolean extended) {
+		checkContextOwnership();
+
+		m_extend.set(extended);
+	}
+
+	public void setWheelPower(double power) {
+		checkContextOwnership();
+
+		m_wheels.set(power);
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/procedures/CollectBalls.java b/src/main/java/com/team766/robot/procedures/CollectBalls.java
new file mode 100644
index 00000000..90e9856c
--- /dev/null
+++ b/src/main/java/com/team766/robot/procedures/CollectBalls.java
@@ -0,0 +1,16 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Procedure;
+import com.team766.framework.Context;
+
+public class CollectBalls extends Procedure {
+
+	public void run(Context context) {
+		new StartIntake().run(context);
+
+		new DriveStraight().run(context);
+
+		new StopIntake().run(context);
+	}
+
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/procedures/StartIntake.java b/src/main/java/com/team766/robot/procedures/StartIntake.java
new file mode 100644
index 00000000..7045b338
--- /dev/null
+++ b/src/main/java/com/team766/robot/procedures/StartIntake.java
@@ -0,0 +1,16 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Procedure;
+import com.team766.framework.Context;
+import com.team766.robot.Robot;
+
+public class StartIntake extends Procedure {
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.intake);
+
+		Robot.intake.setWheelPower(1.0);
+		Robot.intake.setExtended(true);
+	}
+
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/procedures/StopIntake.java b/src/main/java/com/team766/robot/procedures/StopIntake.java
new file mode 100644
index 00000000..cf31e9f0
--- /dev/null
+++ b/src/main/java/com/team766/robot/procedures/StopIntake.java
@@ -0,0 +1,16 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Procedure;
+import com.team766.framework.Context;
+import com.team766.robot.Robot;
+
+public class StopIntake extends Procedure {
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.intake);
+
+		Robot.intake.setWheelPower(0.0);
+		Robot.intake.setExtended(false);
+	}
+
+}
\ No newline at end of file