Demo routine

src/main/java/com/team766/robot/AutonomousModes.java

@@ -13,6 +13,8 @@ public class AutonomousModes {
 		//    new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)),
 		//    new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
 
+		new AutonomousMode("ExampleDriveSequence", () -> new ExampleDriveSequence()),
+
 		new AutonomousMode("DoNothing", () -> new DoNothing()),
 	};
 }

src/main/java/com/team766/robot/OI.java

@@ -26,13 +26,29 @@ public OI() {
 	}
 
 	public void run(final Context context) {
+		context.takeOwnership(Robot.drive);
+		context.takeOwnership(Robot.intake);
 		while (true) {
 			// wait for driver station data (and refresh it using the WPILib APIs)
 			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
 			RobotProvider.instance.refreshDriverStationData();
 
 			// Add driver controls here - make sure to take/release ownership
 			// of mechanisms when appropriate.
+			Robot.drive.setArcadeDrive(joystick0.getAxis(1), joystick1.getAxis(0));
+
+			if (joystick0.getButtonPressed(1)) {
+				context.startAsync(new Launch());
+			}
+
+			if (joystick1.getButtonPressed(2)) {
+				Robot.intake.setIntakeArm(true);
+				Robot.intake.setIntakePower(1.0);
+			}
+			if (joystick1.getButtonPressed(3)) {
+				Robot.intake.setIntakeArm(false);
+				Robot.intake.setIntakePower(0.0);
+			}
 		}
 	}
 }

src/main/java/com/team766/robot/Robot.java

@@ -4,9 +4,14 @@
 
 public class Robot {
 	// Declare mechanisms here
-
+	public static Drive drive;
+	public static Intake intake;
+	public static Launcher launcher;
 
 	public static void robotInit() {
 		// Initialize mechanisms here
+		drive = new Drive();
+		intake = new Intake();
+		launcher = new Launcher();
 	}
 }

src/main/java/com/team766/robot/mechanisms/Drive.java

@@ -0,0 +1,98 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.EncoderReader;
+import com.team766.hal.GyroReader;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.MotorController;
+
+public class Drive extends Mechanism {
+
+	private MotorController m_leftMotor;
+	private MotorController m_rightMotor;
+	private EncoderReader m_leftEncoder;
+	private EncoderReader m_rightEncoder;
+	private GyroReader m_gyro;
+
+	// variables for calculating position using odometry
+	private double m_x;
+	private double m_y;
+	private double m_previousLeft;
+	private double m_previousRight;
+	private double m_previousAngle;
+	private double m_velocity;
+	private double m_previousTime = RobotProvider.getTimeProvider().get();
+
+	public Drive() {
+		m_leftMotor = RobotProvider.instance.getMotor("drive.leftMotor");
+		m_rightMotor = RobotProvider.instance.getMotor("drive.rightMotor");
+		m_leftEncoder = RobotProvider.instance.getEncoder("drive.leftEncoder");
+		m_rightEncoder = RobotProvider.instance.getEncoder("drive.rightEncoder");
+		m_gyro = RobotProvider.instance.getGyro("drive.gyro");
+	}
+
+	@Override
+	public void run() {
+		double leftDistance = m_leftEncoder.getDistance();
+		double rightDistance = m_rightEncoder.getDistance();
+		double distance = ((leftDistance - m_previousLeft) + (rightDistance - m_previousRight)) / 2.0;
+
+		// Use trapezoidal integration to get better position accuracy
+		m_x += Math.cos(m_previousAngle) * distance / 2;
+		m_y += Math.sin(m_previousAngle) * distance / 2;
+		double angle = getHeadingAngle();
+		m_x += Math.cos(angle) * distance / 2;
+		m_y += Math.sin(angle) * distance / 2;
+
+		m_previousLeft = leftDistance;
+		m_previousRight = rightDistance;
+		m_previousAngle = angle;
+
+		// calculate velocity
+		double currentTime = RobotProvider.getTimeProvider().get();
+		m_velocity = distance / (currentTime - m_previousTime);
+		m_previousTime = currentTime;
+	}
+
+	public double getHeadingAngle() {
+		return m_gyro.getAngle() % 360;
+	}
+
+	public void resetGyro() {
+		m_gyro.reset();
+	}
+
+	public double getXPosition() {
+		return m_x;
+	}
+
+	public double getYPosition() {
+		return m_y;
+	}
+
+	public void setDrivePower(double leftPower, double rightPower) {
+		checkContextOwnership();
+		m_leftMotor.set(leftPower);
+		m_rightMotor.set(rightPower);
+	}
+
+	public void setArcadeDrive(double fwdPower, double turnPower) {
+		double maximum = Math.max(Math.abs(fwdPower), Math.abs(turnPower));
+		double total = fwdPower + turnPower;
+		double difference = fwdPower - turnPower;
+
+		if (fwdPower >= 0) {
+			if (turnPower >= 0) {
+				setDrivePower(maximum, difference);
+			} else {
+				setDrivePower(total, maximum);
+			}
+		} else {
+			if (turnPower >= 0) {
+				setDrivePower(total, -maximum);
+			} else {
+				setDrivePower(-maximum, difference);
+			}
+		}
+	}
+}

src/main/java/com/team766/robot/mechanisms/Intake.java

@@ -0,0 +1,26 @@
+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_intakeArm;
+	private MotorController m_intakeWheels;
+
+	public Intake() {
+		m_intakeArm = RobotProvider.instance.getSolenoid("intakeArm");
+		m_intakeWheels = RobotProvider.instance.getMotor("intake");
+	}
+
+	public void setIntakePower(double intakePower) {
+		checkContextOwnership();
+		m_intakeWheels.set(intakePower);
+	}
+
+	public void setIntakeArm(boolean state) {
+		checkContextOwnership();
+		m_intakeArm.set(state);
+	}
+}

src/main/java/com/team766/robot/mechanisms/Launcher.java

@@ -0,0 +1,18 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+
+public class Launcher extends Mechanism {
+	private SolenoidController m_plungerSolenoid;
+
+	public Launcher() {
+		m_plungerSolenoid = RobotProvider.instance.getSolenoid("launch");
+	}
+
+	public void setPlunger(boolean state) {
+		checkContextOwnership();
+		m_plungerSolenoid.set(state);
+	}
+}

src/main/java/com/team766/robot/procedures/ExampleDriveSequence.java

@@ -0,0 +1,36 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Context;
+import com.team766.framework.Procedure;
+import com.team766.robot.Robot;
+import com.team766.logging.Category;
+
+public class ExampleDriveSequence extends Procedure {
+	public ExampleDriveSequence() {
+		loggerCategory = Category.AUTONOMOUS;
+	}
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.drive);
+		context.takeOwnership(Robot.intake);
+
+		Robot.intake.setIntakeArm(true);
+		Robot.intake.setIntakePower(1.0);
+
+		for (int i = 0; i < 4; ++i) {
+			log("Forward movement begins");
+			Robot.drive.setDrivePower(0.3, 0.3);
+			context.waitForSeconds(2.3);
+			log("Forward movement finished");
+
+			log("Turning movement begins");
+			Robot.drive.setDrivePower(0.1, -0.1);
+			context.waitForSeconds(2.35);
+			log("Turning movement finished");
+
+			context.startAsync(new Launch());
+		}
+
+		Robot.drive.setDrivePower(0.0, 0.0);
+	}
+}

src/main/java/com/team766/robot/procedures/Launch.java

@@ -0,0 +1,16 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Context;
+import com.team766.framework.Procedure;
+import com.team766.robot.Robot;
+
+public class Launch extends Procedure {
+	public void run(Context context) {
+		context.takeOwnership(Robot.launcher);
+
+		log("Launching ball");
+		Robot.launcher.setPlunger(true);
+		context.waitForSeconds(0.5);
+		Robot.launcher.setPlunger(false);
+	}
+}