temporarily stash changes for testing advantagekit

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

@@ -1,38 +1,56 @@
 package com.team766.robot;
 
 import com.team766.framework.Procedure;
+
+import java.io.IOException;
+
 import com.team766.framework.Context;
 import com.team766.hal.JoystickReader;
 import com.team766.hal.RobotProvider;
+import com.team766.hal.simulator.Camera;
 import com.team766.logging.Category;
+import com.team766.robot.constants.InputConstants;
 import com.team766.robot.procedures.*;
+import com.team766.simulator.interfaces.ElectricalDevice.Input;
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the code that allow control of the robot.
  */
 public class OI extends Procedure {
-	private JoystickReader joystick0;
-	private JoystickReader joystick1;
-	private JoystickReader joystick2;
+	private final static int LEFT_STICK_X_AXIS = 0;
+	private final static int LEFT_STICK_Y_AXIS = 1;
+	private final static int RIGHT_STICK_X_AXIS = 2;
+	private final static int RIGHT_STICK_Y_AXIS = 3;
+
+	private JoystickReader gamepad;
 
 	public OI() {
 		loggerCategory = Category.OPERATOR_INTERFACE;
 
-		joystick0 = RobotProvider.instance.getJoystick(0);
-		joystick1 = RobotProvider.instance.getJoystick(1);
-		joystick2 = RobotProvider.instance.getJoystick(2);
+		gamepad = RobotProvider.instance.getJoystick(0);
 	}
+
+	public void run(Context context) {
+		CameraServer.startAutomaticCapture();
+		context.takeOwnership(Robot.drive);
 
-	public void run(final Context context) {
 		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.
+			double leftX = gamepad.getAxis(LEFT_STICK_X_AXIS);
+			double leftY = gamepad.getAxis(LEFT_STICK_Y_AXIS);
+			double rightX = gamepad.getAxis(RIGHT_STICK_X_AXIS);
+			double rightY = gamepad.getAxis(RIGHT_STICK_Y_AXIS);
+
+			log(/*"leftX: " + leftX + */ ", leftY: " + leftY + ", rightX: " + rightX /*+ ", rightY: " + rightY */);
+			Robot.drive.setArcadeDrivePower(-leftY, rightX);
 		}
 	}
 }

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

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

src/main/java/com/team766/robot/constants/InputConstants.java

@@ -0,0 +1,24 @@
+package com.team766.robot.constants;
+
+/**
+ * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc.
+ * 
+ * TODO: consider moving this into a config file.
+ */
+public final class InputConstants {
+
+	//Joysticks
+	public static final int LEFT_JOYSTICK = 0;
+	public static final int RIGHT_JOYSTICK = 1;
+	public static final int CONTROL_PANEL = 2;
+
+	//Navigation
+	public static final int AXIS_LEFT_RIGHT = 0;
+	public static final int AXIS_FORWARD_BACKWARD = 1;
+	public static final int AXIS_TWIST = 3;
+	// Joystick buttons
+	public static final int CROSS_DEFENSE = 1;
+	public static final int RESET_GYRO = 1;
+	public static final int RESET_CURRENT_POSITION = 11;
+	//Other buttons
+}

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

@@ -0,0 +1,31 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.MotorController;
+
+public class Drive extends Mechanism {
+    private MotorController leftMotor;
+    private MotorController rightMotor;
+
+    public Drive() {
+        leftMotor = RobotProvider.instance.getMotor("drive.leftMotor");
+        rightMotor = RobotProvider.instance.getMotor("drive.rightMotor");
+    }
+
+    public void setArcadeDrivePower(double forward, double turn) {
+        checkContextOwnership();
+
+        // less is moar
+        forward *= 0.15;
+        turn *= 0.15;
+
+        double leftMotorPower = turn + forward;
+        double rightMotorPower = -turn + forward;
+
+        log("left: " + leftMotorPower + ", right: " + rightMotorPower);
+
+        leftMotor.set(leftMotorPower);
+        rightMotor.set(rightMotorPower);
+    }
+}