Intake tutorial

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()),

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

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

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

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

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

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