Configuration Schemas are small classes that only contain non-private fields
+ * that are declared in compatible types. The following is an example:
+ *
Each field in a schema corresponds to an entry in the configuration map with the same name.
+ * For the example above, a valid configuration map would be as follows, formatted in Properties
+ * style:
+ *
+ *
+ * @see InvalidFormatError
+ * @see InvalidSchemaError
+ */
+public final class Configuration {
+
+ private static final String PROPERTIES_DIRECTORY = "/storage/self/primary/FIRST/config";
+
+ private final Map map;
+
+ private Configuration(Map map) {
+ this.map = map;
+ }
+
+ /**
+ * Reads a file with the given name from the standard directory for configuration file storage,
+ * parses it into {@link Properties} format, and returns a new instance of {@link Configuration}
+ * with the parsed mapping.
+ * @param fileName Name of the file to be read
+ * @return The Configuration instance whose entries have been read from the given file
+ * @throws IOException If the file with the given name cannot be read
+ */
+ public static Configuration fromPropertiesFile(String fileName)
+ throws IOException {
+ return fromProperties(new FileReader(new File(PROPERTIES_DIRECTORY, fileName)));
+ }
+ /**
+ * Parses data from the given {@link Reader} in Java Properties format, and then creates a new
+ * instance of {@link Configuration} from the parsed result.
+ * @param file The Reader from which to parse data
+ * @return The new Configuration instance
+ * @throws IOException if an error is encountered while reading from the given Reader
+ */
+ public static Configuration fromProperties(Reader file)
+ throws IOException {
+
+ Properties props = new Properties();
+ props.load(file);
+ // Java's sloppiness here. Properties was done in a rush and wasn't adapted to generics.
+ return Configuration.from(new HashMap(props));
+ }
+
+ /**
+ * Creates a new instance of {@link Configuration} from the given String-to-String {@link Map}.
+ *
+ * @param map The map to apply to the new Configuration instance
+ * @return The new Configuration instance
+ */
+ public static Configuration from(Map map) {
+ return new Configuration(map);
+ }
+
+ /**
+ * Loads the entries from this Configuration to the given Schema instance.
+ * @param schemaInstance Configuration Schema instance into which the entries are loaded
+ * @param Type of the given Schema instance
+ * @return The given Schema instance with its fields populated
+ * @throws InvalidSchemaError if the type of the given Schema instance is not suitable for loading
+ */
+ public T loadToSchema(T schemaInstance) {
+ Class schemaClass = schemaInstance.getClass();
+ try {
+ for (Field field : schemaClass.getDeclaredFields()) {
+ populateSchemaField(schemaInstance, field);
+ }
+ return schemaInstance;
+ } catch (Exception reflectionError) {
+ throw new InvalidSchemaError(schemaClass, reflectionError);
+ }
+ }
+
+ /**
+ * Retrieves the value of the given key in the mapping, parsed as an integer.
+ * @param key The key for the requesting value
+ * @return The parsed integer represented by the key's corresponding value
+ * @throws InvalidFormatError if the given key's corresponding value cannot be parsed as an integer
+ * @throws NoSuchFieldError if the given key does not exist in this Configuration instance's mapping
+ */
+ public int getInt(String key) {
+ String strValue = getString(key);
+ try {
+ return Integer.parseInt(strValue);
+ } catch (NumberFormatException numberException) {
+ throw new InvalidFormatError(key, strValue, "int");
+ }
+ }
+
+ /**
+ * Retrieves the value of the given key in the mapping, parsed as a double.
+ * @param key The key for the requesting value
+ * @return The parsed double represented by the key's corresponding value
+ * @throws InvalidFormatError if the given key's corresponding value cannot be parsed as a double
+ * @throws NoSuchFieldError if the given key does not exist in this Configuration instance's mapping
+ */
+ public double getDouble(String key) {
+ String strValue = getString(key);
+ try {
+ return Double.parseDouble(strValue);
+ } catch (NumberFormatException numberException) {
+ throw new InvalidFormatError(key, strValue, "double");
+ }
+ }
+
+ /**
+ * Retrieves the value of the given key in the mapping, parsed as a boolean. Accepted values are
+ * "true" and "false", case-insensitive.
+ * @param key The key for the requesting value
+ * @return The parsed boolean represented by the key's corresponding value
+ * @throws InvalidFormatError if the given key's corresponding value is not an accepted value
+ * @throws NoSuchFieldError if the given key does not exist in this Configuration instance's mapping
+ */
+ public boolean getBoolean(String key) {
+ String value = getString(key);
+ if (value.equalsIgnoreCase("true")) {
+ return true;
+ } else if (value.equalsIgnoreCase("false")) {
+ return false;
+ } else {
+ throw new InvalidFormatError(key, value, "boolean");
+ }
+ }
+
+ /**
+ * Retrieves the value of the given key in the mapping.
+ * @param key The key for the requesting value
+ * @return The key's corresponding value
+ * @throws NoSuchFieldError if the given key does not exist in this mapping
+ */
+ public String getString(String key) {
+ String value = map.get(key);
+ if (value == null) {
+ throw new NoSuchFieldError(key);
+ }
+ return value;
+ }
+
+ @Override
+ public boolean equals(Object o) {
+ if (this == o) {
+ return true;
+ }
+ if (o == null || getClass() != o.getClass()) {
+ return false;
+ }
+ Configuration that = (Configuration) o;
+ return Objects.equals(map, that.map);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(map);
+ }
+
+ private void populateSchemaField(T schemaInstance, Field field)
+ throws IllegalAccessException {
+
+ String fieldName = field.getName();
+ String typeName = field.getType().getCanonicalName();
+
+ switch (typeName) {
+ case "int":
+ field.setInt(schemaInstance, getInt(fieldName));
+ break;
+ case "double":
+ field.setDouble(schemaInstance, getDouble(fieldName));
+ break;
+ case "boolean":
+ field.setBoolean(schemaInstance, getBoolean(fieldName));
+ break;
+ case "java.lang.String":
+ field.set(schemaInstance, getString(fieldName));
+ break;
+ default:
+ throw new InvalidSchemaError(schemaInstance.getClass(),
+ new UnsupportedOperationException("Invalid type: " + typeName));
+ }
+ }
+
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/config/InvalidFormatError.java b/ARCCore/src/main/com/andoverrobotics/core/config/InvalidFormatError.java
new file mode 100644
index 0000000..6795f4b
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/config/InvalidFormatError.java
@@ -0,0 +1,15 @@
+package com.andoverrobotics.core.config;
+
+/**
+ * Error indicating that a {@link Configuration} value cannot be parsed into a specified type.
+ * The detailMessage of an instance indicates the key, the mis-formatted value, and the
+ * format into which it was attempted to be parsed.
+ *
+ * @see Configuration
+ */
+public class InvalidFormatError extends RuntimeException {
+
+ InvalidFormatError(String key, String value, String expectedFormat) {
+ super(String.format("%s (%s) not a %s", key, value, expectedFormat));
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/config/InvalidSchemaError.java b/ARCCore/src/main/com/andoverrobotics/core/config/InvalidSchemaError.java
new file mode 100644
index 0000000..55e93db
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/config/InvalidSchemaError.java
@@ -0,0 +1,14 @@
+package com.andoverrobotics.core.config;
+
+/**
+ * Error indicating that a Schema given to {@link Configuration#loadToSchema} is not suitable for
+ * loading. The detailed cause for this error is retrievable with {@link Throwable#getCause()}.
+ *
+ * @see Configuration
+ */
+public class InvalidSchemaError extends RuntimeException {
+
+ InvalidSchemaError(Class schemaClass, Exception problem) {
+ super(schemaClass.getCanonicalName(), problem);
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/config/package-info.java b/ARCCore/src/main/com/andoverrobotics/core/config/package-info.java
new file mode 100644
index 0000000..740aa8d
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/config/package-info.java
@@ -0,0 +1,6 @@
+/**
+ * Provides the functionality of Configuration, whose purpose is to facilitate the ubiquitous action
+ * of changing parameters such as ticksPerInch, default power for the drivetrain,
+ * autonomous routines, etc.
+ */
+package com.andoverrobotics.core.config;
\ No newline at end of file
diff --git a/ARCCore/src/main/com/andoverrobotics/core/drivetrain/DriveTrain.java b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/DriveTrain.java
new file mode 100644
index 0000000..23eeacd
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/DriveTrain.java
@@ -0,0 +1,171 @@
+package com.andoverrobotics.core.drivetrain;
+
+
+import com.andoverrobotics.core.utilities.IMotor;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+import com.qualcomm.robotcore.util.Range;
+
+/**
+ * Defines the interface for an ordinary DriveTrain object with support for a default motor power.
+ */
+public abstract class DriveTrain {
+
+ protected final OpMode opMode;
+ protected double defaultPower;
+
+ protected DriveTrain(OpMode opMode) {
+ this.opMode = opMode;
+ }
+
+ /**
+ * Sets the default driving power.
+ *
+ * @param power The default power
+ */
+ public final void setDefaultDrivePower(double power) {
+ defaultPower = Range.clip(power, 0, 1);
+ }
+
+ /**
+ * Gets the default driving power.
+ *
+ * @return The current value of defaultPower
+ */
+ public final double getDefaultDrivePower() {
+ return defaultPower;
+ }
+
+ // -- Autonomous Methods --
+
+ /**
+ * Drives forwards a specific distance at the default power.
+ *
+ * @param distanceInInches The distance to travel, in inches
+ */
+ public final void driveForwards(double distanceInInches) {
+ driveForwards(distanceInInches, defaultPower);
+ }
+
+ /**
+ * Drives forwards a specific distance at a specific power.
+ *
+ * @param distanceInInches The distance to travel, in inches
+ * @param power The power to drive at, between 0 and 1, inclusive
+ */
+ public abstract void driveForwards(double distanceInInches, double power);
+
+ /**
+ * Drives backwards a specific distance at the default power.
+ *
+ * @param distanceInInches The distance to travel, in inches
+ */
+ public final void driveBackwards(double distanceInInches) {
+ driveBackwards(distanceInInches, defaultPower);
+ }
+
+ /**
+ * Drives backwards a specific distance at a specific power.
+ *
+ * @param distanceInInches The distance to travel, in inches
+ * @param power The power to drive at, between 0 and 1, inclusive
+ */
+ public abstract void driveBackwards(double distanceInInches, double power);
+
+ /**
+ * Rotates clockwise a specific amount, at the default power.
+ *
+ * @param degrees The amount to rotate (in degrees), between 0 and 360, inclusive
+ */
+ public final void rotateClockwise(int degrees) {
+ rotateClockwise(degrees, defaultPower);
+ }
+
+ /**
+ * Rotates clockwise a specific amount, at a specified power.
+ *
+ * @param degrees The amount to rotate (in degrees), between 0 and 360, inclusive
+ * @param power The power to rotate at, between 0 and 1, inclusive
+ */
+ public abstract void rotateClockwise(int degrees, double power);
+
+ /**
+ * Rotates counterclockwise a specific amount, at the default power.
+ *
+ * @param degrees The amount to rotate (in degrees), between 0 and 360, inclusive
+ */
+ public final void rotateCounterClockwise(int degrees) {
+ rotateCounterClockwise(degrees, defaultPower);
+ }
+
+ /**
+ * Rotates counterclockwise a specific amount, at a specified power.
+ *
+ * @param degrees The amount to rotate (in degrees), between 0 and 360, inclusive
+ * @param power The power to rotate at, between 0 and 1, inclusive
+ */
+ public abstract void rotateCounterClockwise(int degrees, double power);
+
+ // -- Teleop Methods --
+
+ /**
+ * Sets the power to move at.
+ *
+ * @param power The power, between -1 and 1, inclusive. Positive is forwards, negative is
+ * backwards
+ */
+ public abstract void setMovementPower(double power);
+
+ /**
+ * Sets the rotation power.
+ *
+ * @param power The power, between -1 and 1, inclusive. Positive is clockwise, negative is
+ * counterclockwise
+ */
+ public abstract void setRotationPower(double power);
+
+ /**
+ * Sets the power to move at and rotate at.
+ *
+ * @param movePower The power to move at, between -1 and 1, inclusive. Positive is forwards,
+ * negative is backwards
+ * @param rotatePower The power to rotate at, between -1 and 1, inclusive. Positive is clockwise,
+ * negative is counterclockwise
+ */
+ public abstract void setMovementAndRotation(double movePower, double rotatePower);
+
+ protected boolean opModeIsActive() {
+ boolean isAutonomous = opMode instanceof LinearOpMode;
+
+ return !isAutonomous || ((LinearOpMode) opMode).opModeIsActive();
+ }
+
+ /**
+ * Stops the motor(s).
+ */
+ public void stop() {
+ for (IMotor motor : getMotors()) {
+ motor.setPower(0);
+ }
+ }
+
+ // -- Internal methods
+
+ protected abstract IMotor[] getMotors();
+
+ protected void setMotorMode(RunMode mode) {
+ for (IMotor motor : getMotors()) {
+ motor.setMode(mode);
+ }
+ }
+
+ protected boolean isBusy() {
+ for (IMotor motor : getMotors()) {
+ if (motor.isBusy()) {
+ return true;
+ }
+ }
+ return false;
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/drivetrain/MecanumDrive.java b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/MecanumDrive.java
new file mode 100644
index 0000000..6855d5e
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/MecanumDrive.java
@@ -0,0 +1,250 @@
+package com.andoverrobotics.core.drivetrain;
+
+import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_USING_ENCODER;
+import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_WITHOUT_ENCODER;
+
+import com.andoverrobotics.core.utilities.Converter;
+import com.andoverrobotics.core.utilities.Coordinate;
+import com.andoverrobotics.core.utilities.IMotor;
+import com.andoverrobotics.core.utilities.MotorPair;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+
+/**
+ * Implements the {@link StrafingDriveTrain} for a Mecanum drivetrain.
See {@link
+ * #fromOctagonalMotors(DcMotor, DcMotor, DcMotor, DcMotor, OpMode, int, int)} and {@link
+ * #fromCrossedMotors(DcMotor, DcMotor, DcMotor, DcMotor, OpMode, int, int)} for instructions about
+ * easier construction.
+ */
+public class MecanumDrive extends StrafingDriveTrain {
+
+ private final IMotor leftDiagonal, rightDiagonal, leftSide, rightSide;
+
+ private final int ticksPerInch;
+ private final int ticksPer360;
+
+ /**
+ * Constructs a new MecanumDrive instance with the given {@link IMotor}s and encoder
+ * parameters.
+ *
+ * @param leftDiagonal The {@link IMotor} that represents the two physical motors that make the
+ * robot go (forward and) to the left when the power is positive
+ * @param rightDiagonal The {@link IMotor} that represents the two physical motors that make the
+ * robot go (forward and) to the right when the power is positive
+ * @param leftSide The {@link IMotor} that represents the two physical motors that are on the left
+ * side of the robot
+ * @param rightSide The {@link IMotor} that represents the two physical motors that are on the
+ * right side of the robot
+ * @param opMode The main {@link OpMode}
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ */
+ public MecanumDrive(IMotor leftDiagonal, IMotor rightDiagonal, IMotor leftSide, IMotor rightSide,
+ OpMode opMode, int ticksPerInch, int ticksPer360) {
+
+ super(opMode);
+
+ this.leftDiagonal = leftDiagonal;
+ this.rightDiagonal = rightDiagonal;
+ this.leftSide = leftSide;
+ this.rightSide = rightSide;
+
+ this.ticksPerInch = ticksPerInch;
+ this.ticksPer360 = ticksPer360;
+ }
+
+ /**
+ * Constructs a new MecanumDrive instance that uses the given physical motors, which are arranged
+ * in an octagonal manner.
The Octagonal Configuration
In the octagonal configuration,
+ * the front left and the back right motors cause the robot to move forward and to the right
+ * (diagonally), and the other two motors cause the robot to move forward and to the left.
+ *
+ * It is named to be "octagonal" because of its diagram:
+ *
+ * /-\
+ * | |
+ * \-/
+ *
+ *
+ * @param motorFL The motor located at the front-left of the robot
+ * @param motorFR The motor located at the front-right of the robot
+ * @param motorBL The motor located at the rear-left of the robot
+ * @param motorBR The motor located at the rear-right of the robot
+ * @param opMode The main {@link OpMode}
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ * @return The new MecanumDrive instance
+ */
+ public static MecanumDrive fromOctagonalMotors(DcMotor motorFL, DcMotor motorFR, DcMotor motorBL,
+ DcMotor motorBR,
+ OpMode opMode, int ticksPerInch, int ticksPer360) {
+ // /-\
+ // | |
+ // \-/
+
+ return new MecanumDrive(
+ MotorPair.of(motorFR, motorBL),
+ MotorPair.of(motorFL, motorBR),
+ MotorPair.of(motorFL, motorBL),
+ MotorPair.of(motorFR, motorBR), opMode, ticksPerInch, ticksPer360);
+ }
+
+ /**
+ * Constructs a new MecanumDrive instance that uses the given physical motors, which are arranged
+ * in a a crossed manner.
The Crossed Configuration In the crossed configuration, the
+ * front left and the back right motors cause the robot to move forward and to the left
+ * (diagonally), and the other two motors cause the robot to move forward and to the right.
+ *
+ * It is named to be "octagonal" because of its diagram:
+ *
+ * \-/
+ * | |
+ * /-\
+ *
+ *
+ * @param motorFL The motor located at the front-left of the robot
+ * @param motorFR The motor located at the front-right of the robot
+ * @param motorBL The motor located at the rear-left of the robot
+ * @param motorBR The motor located at the rear-right of the robot
+ * @param opMode The main {@link OpMode}
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ * @return The new MecanumDrive instance
+ */
+ public static MecanumDrive fromCrossedMotors(DcMotor motorFL, DcMotor motorFR, DcMotor motorBL,
+ DcMotor motorBR,
+ OpMode opMode, int ticksPerInch, int ticksPer360) {
+ // \-/
+ // | |
+ // /-\
+
+ return new MecanumDrive(
+ MotorPair.of(motorFL, motorBR),
+ MotorPair.of(motorFR, motorBL),
+ MotorPair.of(motorFL, motorBL),
+ MotorPair.of(motorFR, motorBR), opMode, ticksPerInch, ticksPer360);
+ }
+
+ // Rotates the given displacement by 45deg clockwise, assigns its components to the diagonals as
+ // a tick offset (setTargetPosition), then scales its components down such that the greatest
+ // component is equal to the power given, followed by assigning these components to the diagonals
+ // as power.
+ private void driveWithEncoder(Coordinate displacement, double power) {
+ double clippedPower = Range.clip(power, -1, 1);
+
+ if (displacement.getPolarDistance() < 1e-5) {
+ return;
+ }
+
+ Coordinate diagonalOffsets = displacement.rotate(-45);
+ double maxOffset = Math.max(diagonalOffsets.getX(), diagonalOffsets.getY());
+
+ int leftOffset = (int) (diagonalOffsets.getY() * ticksPerInch),
+ rightOffset = (int) (diagonalOffsets.getX() * ticksPerInch);
+
+ double leftPower = clippedPower * (diagonalOffsets.getY() / maxOffset),
+ rightPower = clippedPower * (diagonalOffsets.getX() / maxOffset);
+
+ leftDiagonal.startRunToPosition(leftOffset, leftPower);
+ rightDiagonal.startRunToPosition(rightOffset, rightPower);
+
+ while (isBusy() && opModeIsActive()) {
+ }
+
+ stop();
+ setMotorMode(RUN_USING_ENCODER);
+ }
+
+ @Override
+ public void rotateClockwise(int degrees, double power) {
+ rotateWithEncoder(-Converter.normalizedDegrees(degrees), -Math.abs(power));
+ }
+
+ @Override
+ public void rotateCounterClockwise(int degrees, double power) {
+ rotateWithEncoder(Converter.normalizedDegrees(degrees), Math.abs(power));
+ }
+
+ // Positive input means counter-clockwise
+ private void rotateWithEncoder(double degrees, double power) {
+ double clippedPower = Math.abs(Range.clip(power, -1, 1));
+ double rotationTicks = degrees / 360.0 * ticksPer360;
+
+ leftSide.startRunToPosition((int) -rotationTicks, -clippedPower);
+ rightSide.startRunToPosition((int) rotationTicks, clippedPower);
+
+ while (isBusy() && opModeIsActive()) {
+ }
+
+ stop();
+ setMotorMode(RUN_USING_ENCODER);
+ }
+
+ @Override
+ public void strafeInches(Coordinate inchOffset, double power) {
+ driveWithEncoder(inchOffset, power);
+ }
+
+ // -- TeleOp methods --
+
+ @Override
+ public void setMovementPower(double power) {
+ double clippedPower = Range.clip(power, -1, 1);
+
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ leftSide.setPower(clippedPower);
+ rightSide.setPower(clippedPower);
+ }
+
+ @Override
+ public void setRotationPower(double power) { //clockwise if power is positive
+ double clippedPower = Range.clip(power, -1, 1);
+
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ leftSide.setPower(clippedPower);
+ rightSide.setPower(-clippedPower);
+ }
+
+ @Override
+ public void setStrafe(Coordinate offset, double unscaledPower) {
+ double direction = Converter.degreesToRadians(offset.getPolarDirection() - 45);
+ double magnitude = Math.min(1, offset.getPolarDistance());
+ double power = Range.clip(unscaledPower, -1, 1);
+
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ leftDiagonal.setPower(Math.sin(direction) * magnitude * Math.abs(power));
+ rightDiagonal.setPower(Math.cos(direction) * magnitude * Math.abs(power));
+ }
+
+ @Override
+ public void setMovementAndRotation(double movePower, double rotatePower) {
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ double leftPower = movePower + rotatePower,
+ rightPower = movePower - rotatePower;
+
+ double maxPower = Math.max(Math.abs(leftPower), Math.abs(rightPower));
+ if (maxPower > 1) {
+ leftPower /= maxPower;
+ rightPower /= maxPower;
+ }
+
+ leftSide.setPower(leftPower);
+ rightSide.setPower(rightPower);
+ }
+
+ @Override
+ protected IMotor[] getMotors() {
+ return new IMotor[]{leftDiagonal, rightDiagonal, leftSide, rightSide};
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/main/com/andoverrobotics/core/drivetrain/StrafingDriveTrain.java b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/StrafingDriveTrain.java
new file mode 100644
index 0000000..bacac92
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/StrafingDriveTrain.java
@@ -0,0 +1,173 @@
+package com.andoverrobotics.core.drivetrain;
+
+import com.andoverrobotics.core.utilities.Coordinate;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+/**
+ * Describes the interface of a {@link DriveTrain} that is capable of "strafing," or moving the
+ * robot in any direction without turning.
+ */
+public abstract class StrafingDriveTrain extends DriveTrain {
+
+ protected StrafingDriveTrain(OpMode opMode) {
+ super(opMode);
+ }
+
+ /**
+ * Drives forwards with given power.
+ *
+ * @param distanceInInches The number of inches to strafe forwards
+ * @param power The power at which to strafe forwards
+ */
+ @Override
+ public void driveForwards(double distanceInInches, double power) {
+ strafeInches(0, Math.abs(distanceInInches), Math.abs(power));
+ }
+
+ /**
+ * Drives backwards with given power.
+ *
+ * @param distanceInInches The number of inches to strafe backwards
+ * @param power The power at which to strafe backwards
+ */
+ @Override
+ public void driveBackwards(double distanceInInches, double power) {
+ strafeInches(0, -Math.abs(distanceInInches), -Math.abs(power));
+ }
+
+ /**
+ * Strafes right with defaultPower.
+ *
+ * @param distanceInInches The number of inches to strafe right
+ */
+ public final void strafeRight(double distanceInInches) {
+ strafeRight(distanceInInches, defaultPower);
+ }
+
+ /**
+ * Strafes right at given power.
+ *
+ * @param distanceInInches The number of inches to strafe right
+ * @param power The power at which to strafe right
+ */
+ public void strafeRight(double distanceInInches, double power) {
+ strafeInches(Math.abs(distanceInInches), 0, power);
+ }
+
+ /**
+ * Strafes left at default power.
+ *
+ * @param distanceInInches The number of inches to strafe left
+ */
+ public final void strafeLeft(double distanceInInches) {
+ strafeLeft(distanceInInches, defaultPower);
+ }
+
+ /**
+ * Strafes left at given power.
+ *
+ * @param distanceInInches The number of inches to strafe left
+ * @param power The power at which to strafe left
+ */
+ public void strafeLeft(double distanceInInches, double power) {
+ strafeInches(-Math.abs(distanceInInches), 0, power);
+ }
+
+ /**
+ * Strafes the vector at the default power.
+ *
+ * @param xInInches The number of inches to strafe in the x-direction
+ * @param yInInches The number of inches to strafe in the y-direction
+ */
+ public final void strafeInches(double xInInches, double yInInches) {
+ strafeInches(xInInches, yInInches, defaultPower);
+ }
+
+ /**
+ * Strafes the vector at the given power.
+ *
+ * @param xInInches The number of inches to strafe in the x-direction
+ * @param yInInches The number of inches to strafe in the y-direction
+ * @param power The power at which to strafe
+ */
+ public final void strafeInches(double xInInches, double yInInches, double power) {
+ strafeInches(Coordinate.fromXY(xInInches, yInInches), power);
+ }
+
+ /**
+ * Strafes the vector inchOffset at the default power.
+ *
+ * @param inchOffset The coordinate (relative to the current position) to strafe to
+ */
+ public final void strafeInches(Coordinate inchOffset) {
+ strafeInches(inchOffset, defaultPower);
+ }
+
+ /**
+ * Strafes the vector inchOffset at the given power.
+ *
+ * @param inchOffset The coordinate (relative to the current position) to strafe to
+ * @param power The power at which to strafe
+ */
+ public abstract void strafeInches(Coordinate inchOffset, double power);
+
+ // -- Teleop Methods --
+
+ /**
+ * Sets the robot to strafe forward with given power.
+ *
+ * @param power The power, between -1 and 1, inclusive. Positive is forwards, negative is
+ * backwards
+ */
+ @Override
+ public void setMovementPower(double power) {
+ setStrafe(0, 1, power);
+ }
+
+ /**
+ * Sets the robot to strafe in the direction of the vector .
+ *
+ * @param x The number of inches in the x-direction to strafe.
+ * @param y The number of inches in the y-direction to strafe.
+ */
+ public final void setStrafe(double x, double y) {
+ setStrafe(Coordinate.fromXY(x, y));
+ }
+
+ /**
+ * Sets the robot to strafe in the direction of (Coordinate) direction with the default power.
+ *
+ * @param direction The direction in which to strafe
+ */
+ public final void setStrafe(Coordinate direction) {
+ setStrafe(direction, defaultPower);
+ }
+
+ /**
+ * Sets the robot to strafe in the direction of polarDirection with the default power.
+ *
+ * @param polarDirection The polar direction in which to strafe
+ */
+ public final void setStrafe(int polarDirection) {
+ setStrafe(polarDirection, defaultPower);
+ }
+
+ /**
+ * Sets the robot to strafe in the direction of the vector at the given power.
+ *
+ * @param x The number of inches in the x-direction to strafe
+ * @param y The number of inches in the y-direction to strafe
+ * @param power The power at which to strafe
+ */
+ public final void setStrafe(double x, double y, double power) {
+ setStrafe(Coordinate.fromXY(x, y), power);
+ }
+
+ /**
+ * Sets the robot to strafe in the direction of (Coordinate) direction at the given power.
+ *
+ * @param direction The direction in which to strafe
+ * @param power The power at which to strafe, between -1 and 1, inclusive
+ */
+ public abstract void setStrafe(Coordinate direction, double power);
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/drivetrain/TankDrive.java b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/TankDrive.java
new file mode 100644
index 0000000..720b4cf
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/TankDrive.java
@@ -0,0 +1,202 @@
+package com.andoverrobotics.core.drivetrain;
+
+import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_USING_ENCODER;
+import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_WITHOUT_ENCODER;
+
+import com.andoverrobotics.core.utilities.Converter;
+import com.andoverrobotics.core.utilities.IMotor;
+import com.andoverrobotics.core.utilities.MotorAdapter;
+import com.andoverrobotics.core.utilities.MotorPair;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+
+/**
+ * Implements the tank drive DriveTrain for either two motors or four motors.
See {@link
+ * #fromMotors(DcMotor, DcMotor, OpMode, int, int)} and {@link #fromMotors(DcMotor, DcMotor,
+ * DcMotor, DcMotor, OpMode, int, int)} for instructions about easier construction.
+ */
+public class TankDrive extends DriveTrain {
+
+ private IMotor motorL;
+ private IMotor motorR;
+ private final int ticksPerInch;
+ private final int ticksPer360;
+
+ // Verify that the motor(s) on one side is reversed if the motors point in opposite directions!
+
+ /**
+ * Creates a TankDrive from two IMotors
+ *
+ * @param motorL The left IMotor
+ * @param motorR The right IMotor
+ * @param opMode The OpMode to set
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ */
+ public TankDrive(IMotor motorL, IMotor motorR, OpMode opMode,
+ int ticksPerInch, int ticksPer360) {
+ super(opMode);
+
+ this.motorL = motorL;
+ this.motorR = motorR;
+ this.ticksPerInch = ticksPerInch;
+ this.ticksPer360 = ticksPer360;
+ }
+
+ /**
+ * Creates a TankDrive from two DcMotors.
+ *
+ * @param motorL The left DcMotor
+ * @param motorR The right DcMotor
+ * @param opMode The OpMode to set
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ * @return A TankDrive created with the inputted motors
+ */
+ public static TankDrive fromMotors(DcMotor motorL, DcMotor motorR, OpMode opMode,
+ int ticksPerInch, int ticksPer360) {
+
+ return new TankDrive(new MotorAdapter(motorL),
+ new MotorAdapter(motorR), opMode, ticksPerInch, ticksPer360);
+ }
+
+ /**
+ * Creates a TankDrive from four DcMotors.
+ *
+ * @param motorL1 One of the left DcMotors
+ * @param motorL2 The other left DcMotor
+ * @param motorR1 One of the right DcMotors
+ * @param motorR2 The other right DcMotor
+ * @param opMode The OpMode to set
+ * @param ticksPerInch The number of encoder ticks required to cause a diagonal displacement of 1
+ * inch for the robot
+ * @param ticksPer360 The number of encoder ticks required to cause a full rotation for the robot,
+ * when this amount is applied to the left and right sides in opposite directions
+ * @return A TankDrive created using MotorPairs
+ */
+ public static TankDrive fromMotors(DcMotor motorL1, DcMotor motorL2, DcMotor motorR1,
+ DcMotor motorR2,
+ OpMode opMode, int ticksPerInch, int ticksPer360) {
+
+ return new TankDrive(
+ MotorPair.of(motorL1, motorL2),
+ MotorPair.of(motorR1, motorR2), opMode, ticksPerInch, ticksPer360);
+ }
+
+ @Override
+ public void driveForwards(double distanceInInches, double power) {
+ driveWithEncoder(Math.abs(distanceInInches), Math.abs(power));
+ }
+
+ private void driveWithEncoder(double displacementInInches, double givenPower) {
+
+ if (givenPower == 0) {
+ stop();
+ return;
+ }
+
+ double power = Math.abs(Range.clip(givenPower, -1, 1));
+
+ if (displacementInInches < 0) {
+ power *= -1;
+ }
+
+ double robotTurn = displacementInInches * ticksPerInch;
+
+ runWithEncoder((int) robotTurn, (int) robotTurn, power, power);
+ }
+
+ @Override
+ public void driveBackwards(double distanceInInches, double power) {
+ driveWithEncoder(-Math.abs(distanceInInches), -Math.abs(power));
+ }
+
+ @Override
+ public void rotateClockwise(int degrees, double givenPower) {
+ double power = Range.clip(givenPower, -1, 1);
+ power = Math.abs(power);
+ double normalizedDegrees = Converter.normalizedDegrees(degrees);
+
+ rotateWithEncoder(normalizedDegrees, -normalizedDegrees, power, -power);
+ }
+
+ @Override
+ public void rotateCounterClockwise(int degrees, double givenPower) {
+ double power = Range.clip(givenPower, -1, 1);
+ power = Math.abs(power);
+ double normalizedDegrees = Converter.normalizedDegrees(degrees);
+
+ rotateWithEncoder(-normalizedDegrees, normalizedDegrees, -power, power);
+ }
+
+ private void rotateWithEncoder(double leftDegrees, double rightDegrees,
+ double leftPower, double rightPower) {
+
+ runWithEncoder(
+ (int) Math.round(leftDegrees / 360.0 * ticksPer360),
+ (int) Math.round(rightDegrees / 360.0 * ticksPer360),
+ leftPower, rightPower);
+ }
+
+ private void runWithEncoder(int leftTickOffset, int rightTickOffset,
+ double leftPower, double rightPower) {
+
+ // Fails unit tests
+ /*Log.d("TankDrive Encoder",
+ String.format("leftTickOffset=%d rightTickOffset=%d leftPower=%.3f rightPower=%.3f",
+ leftTickOffset, rightTickOffset, leftPower, rightPower));*/
+
+ motorL.startRunToPosition(leftTickOffset, leftPower);
+ motorR.startRunToPosition(rightTickOffset, rightPower);
+
+ while (isBusy() && opModeIsActive()) {
+ }
+
+ stop();
+ setMotorMode(RUN_USING_ENCODER);
+ }
+
+ // -- TeleOp methods --
+
+ @Override
+ public void setMovementPower(double power) {
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ motorL.setPower(power);
+ motorR.setPower(power);
+ }
+
+ @Override
+ public void setRotationPower(double power) { //clockwise if power is positive
+ setMotorMode(RUN_WITHOUT_ENCODER);
+
+ motorL.setPower(power);
+ motorR.setPower(-power);
+ }
+
+ @Override
+ public void setMovementAndRotation(double movePower, double rotatePower) {
+ double leftPower = movePower + rotatePower,
+ rightPower = movePower - rotatePower,
+ maxAbsPower = Math.max(Math.abs(leftPower), Math.abs(rightPower));
+
+ if (maxAbsPower > 1) {
+ leftPower /= maxAbsPower;
+ rightPower /= maxAbsPower;
+ }
+
+ motorL.setPower(leftPower);
+ motorR.setPower(rightPower);
+ }
+
+ @Override
+ protected IMotor[] getMotors() {
+ return new IMotor[]{motorL, motorR};
+ }
+
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/drivetrain/package-info.java b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/package-info.java
new file mode 100644
index 0000000..426e1fb
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/drivetrain/package-info.java
@@ -0,0 +1,4 @@
+/**
+ * Includes classes that provide abstractions for robotics drivetrains.
+ */
+package com.andoverrobotics.core.drivetrain;
\ No newline at end of file
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/Converter.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/Converter.java
new file mode 100644
index 0000000..fe13bfe
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/Converter.java
@@ -0,0 +1,97 @@
+package com.andoverrobotics.core.utilities;
+
+// Radians are [0, tau)
+// Degrees are [0, 360)
+
+/**
+ * Provides utility functions that convert between radians and degrees, millimeters and inches, and
+ * normalize angle measures.
+ */
+public class Converter {
+
+ /**
+ * The τ constant, or two times π.
+ */
+ public static final double TAU = 2 * Math.PI;
+
+ /**
+ * Converts the given angle measure in degrees into the equivalent measure in radians.
+ *
+ * @param degrees The degrees of which the radian representation will be returned
+ * @return The radian representation of the given angle measure
+ */
+ public static double degreesToRadians(double degrees) {
+ return normalizedRadians(degrees / 180.0 * Math.PI);
+ }
+
+ /**
+ * Converts the given angle measure in radians into the equivalent measure in degrees.
+ *
+ * @param radians The radians of which the degree representation will be returned
+ * @return The degree representation of the given angle measure
+ */
+ public static double radiansToDegrees(double radians) {
+ return normalizedDegrees(radians / Math.PI * 180);
+ }
+
+ /**
+ * Converts the given distance (in inches) to millimeters.
+ *
+ * @param inches The distance, in inches, to be converted into millimeters
+ * @return The given distance in millimeters
+ */
+ public static double inchesToMillimeters(double inches) {
+ return inches * 25.4;
+ }
+
+ /**
+ * Converts the given distance (in millimeters) to inches.
+ *
+ * @param millimeters The distance, in millimeters, to be converted into inches
+ * @return The given distance in inches
+ */
+ public static double millimetersToInches(double millimeters) {
+ return millimeters / 25.4;
+ }
+
+ /**
+ * Converts the given angle measure in degrees to its equivalent value in the [0, 360] range.
+ *
+ * @param inputDegrees The angle measure of which the equivalent value in the [0, 360] range will
+ * be returned
+ * @return The equivalent value of the given angle measure in the [0, 360] range
+ */
+ public static double normalizedDegrees(final double inputDegrees) {
+ double degrees = inputDegrees;
+
+ while (degrees < 0) {
+ degrees += 360;
+ }
+ while (degrees >= 360) {
+ degrees -= 360;
+ }
+
+ return degrees;
+ }
+
+ /**
+ * Converts the given angle measure in radians to its equivalent value in the [0, {@link #TAU}]
+ * range.
+ *
+ * @param inputRadians The angle measure of which the equivalent value in the [0, {@link #TAU}]
+ * range will be returned
+ * @return The equivalent value of the given angle measure in the [0, {@link #TAU}] range
+ */
+ public static double normalizedRadians(final double inputRadians) {
+ double radians = inputRadians;
+
+ while (radians < 0) {
+ radians += TAU;
+ }
+ while (radians >= TAU) {
+ radians -= TAU;
+ }
+
+ return radians;
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/Coordinate.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/Coordinate.java
new file mode 100644
index 0000000..7826992
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/Coordinate.java
@@ -0,0 +1,118 @@
+package com.andoverrobotics.core.utilities;
+
+import java.util.Objects;
+
+/**
+ * Represents a coordinate in the 2D coordinate plane with support for both Cartesian and polar
+ * components.
+ */
+public class Coordinate {
+
+ private double x;
+ private double y;
+
+ private Coordinate(double x, double y) {
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * Creates a Coordinate instance with the components of another Coordinate.
+ *
+ * @param other The Coordinate instance from which the components are copied
+ */
+ public Coordinate(Coordinate other) {
+ this.x = other.x;
+ this.y = other.y;
+ }
+
+ // Counter-clockwise by default
+
+ /**
+ * Rotates the point at this Coordinate by the given number of degrees in the counter-clockwise
+ * direction about the origin.
+ *
+ * @param degrees The number of degrees to rotate. Positive means counter-clockwise, negative
+ * means clockwise.
+ * @return The coordinate of the rotated point
+ */
+ public Coordinate rotate(int degrees) {
+ return Coordinate.fromPolar(
+ getPolarDistance(),
+ Converter.normalizedDegrees(getPolarDirection() + degrees));
+ }
+
+ /**
+ * Calculates the angle of the polar representation of this coordinate.
+ *
+ * @return The angle of the polar representation in degrees
+ */
+ public double getPolarDirection() {
+ return Converter.radiansToDegrees(Math.atan2(y, x));
+ }
+
+ /**
+ * Calculates the distance from the origin to the point at this coordinate.
+ *
+ * @return The distance from the origin to the point at this coordinate
+ */
+ public double getPolarDistance() {
+ return Math.hypot(x, y);
+ }
+
+ /**
+ * @return The x value of the Cartesian representation of this coordinate
+ */
+ public double getX() {
+ return x;
+ }
+
+ /**
+ * @return The y value of the Cartesian representation of this coordinate
+ */
+ public double getY() {
+ return y;
+ }
+
+ /**
+ * Constructs a new coordinate with the given Cartesian components.
+ *
+ * @param x The x component of the Cartesian representation of the new coordinate
+ * @param y The y component of the Cartesian representation of the new coordinate
+ * @return The new coordinate with the given components
+ */
+ public static Coordinate fromXY(double x, double y) {
+ return new Coordinate(x, y);
+ }
+
+ /**
+ * Constructs a new coordinate with the given polar components.
+ *
+ * @param distance The distance from the origin to the new coordinate
+ * @param degrees The angle of the polar representation of the new coordinate
+ * @return The new coordinate with the given polar components
+ */
+ public static Coordinate fromPolar(double distance, double degrees) {
+ double angle = Converter.degreesToRadians(degrees);
+
+ return new Coordinate(distance * Math.cos(angle), distance * Math.sin(angle));
+ }
+
+ @Override
+ public boolean equals(Object o) {
+ if (this == o) {
+ return true;
+ }
+ if (o == null || getClass() != o.getClass()) {
+ return false;
+ }
+ Coordinate that = (Coordinate) o;
+ return Double.compare(that.x, x) == 0 &&
+ Double.compare(that.y, y) == 0;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(x, y);
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/IMotor.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/IMotor.java
new file mode 100644
index 0000000..153d0d7
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/IMotor.java
@@ -0,0 +1,46 @@
+package com.andoverrobotics.core.utilities;
+
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+
+/**
+ * Represents an entity or a set of entities that can be controlled like a motor.
+ */
+public interface IMotor {
+
+ /**
+ * Sets the power of the motor(s).
+ *
+ * @param power The new power
+ */
+ void setPower(double power);
+
+ /**
+ * Adds the given offset to the current position, and sets that sum as the target position.
+ *
+ * @param tickOffset The offset that is added to the current position
+ */
+ void addTargetPosition(int tickOffset);
+
+ /**
+ * Starts moving to the specified target position with the given power.
+ *
+ * @param tickOffset The number of ticks to travel, which specifies the direction and distance of
+ * travel
+ * @param absPower The absolute value of the power to assign to the motors
+ */
+ void startRunToPosition(int tickOffset, double absPower);
+
+ /**
+ * Sets the {@link com.qualcomm.robotcore.hardware.DcMotor.RunMode} of the motor(s).
+ *
+ * @param mode The new {@link com.qualcomm.robotcore.hardware.DcMotor.RunMode}
+ */
+ void setMode(RunMode mode);
+
+ /**
+ * Tells if any motor is busy.
+ *
+ * @return True if any motor is busy
+ */
+ boolean isBusy();
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorAdapter.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorAdapter.java
new file mode 100644
index 0000000..1754e00
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorAdapter.java
@@ -0,0 +1,59 @@
+package com.andoverrobotics.core.utilities;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+
+/**
+ * Adapts a DcMotor to the IMotor interface for the DriveTrains.
+ */
+public class MotorAdapter implements IMotor {
+
+ private final DcMotor motor;
+
+ /**
+ * Creates a new MotorAdapter for the given motor.
+ *
+ * @param motor The motor to send commands to
+ */
+ public MotorAdapter(DcMotor motor) {
+ this.motor = motor;
+ }
+
+ /**
+ * @return The motor that this instance adapts
+ */
+ public DcMotor getMotor() {
+ return motor;
+ }
+
+ @Override
+ public void setPower(double power) {
+ motor.setPower(power);
+ }
+
+ @Override
+ public void addTargetPosition(int tickOffset) {
+ motor.setTargetPosition(motor.getCurrentPosition() + tickOffset);
+ }
+
+ @Override
+ public void startRunToPosition(int tickOffset, double absPower) {
+ if (tickOffset == 0 || absPower < 1e-5) {
+ return;
+ }
+
+ setMode(RunMode.RUN_TO_POSITION);
+ addTargetPosition(tickOffset);
+ setPower(tickOffset > 0 ? Math.abs(absPower) : -Math.abs(absPower));
+ }
+
+ @Override
+ public void setMode(RunMode mode) {
+ motor.setMode(mode);
+ }
+
+ @Override
+ public boolean isBusy() {
+ return motor.isBusy();
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorPair.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorPair.java
new file mode 100644
index 0000000..5faf162
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/MotorPair.java
@@ -0,0 +1,63 @@
+package com.andoverrobotics.core.utilities;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+
+/**
+ * Implements the {@link IMotor} interface for the collective control of two physical motors.
+ */
+public class MotorPair implements IMotor {
+
+ private DcMotor first;
+ private DcMotor second;
+
+ private MotorPair(DcMotor first, DcMotor second) {
+ this.first = first;
+ this.second = second;
+ }
+
+ /**
+ * Creates a MotorPair.
+ *
+ * @param one The first motor in a pair
+ * @param two The second motor in a pair
+ * @return The new MotorPair
+ */
+ public static MotorPair of(DcMotor one, DcMotor two) {
+ return new MotorPair(one, two);
+ }
+
+ @Override
+ public void setPower(double power) {
+ first.setPower(power);
+ second.setPower(power);
+ }
+
+ @Override
+ public void addTargetPosition(int position) {
+ first.setTargetPosition(first.getCurrentPosition() + position);
+ second.setTargetPosition(second.getCurrentPosition() + position);
+ }
+
+ @Override
+ public void startRunToPosition(int tickOffset, double absPower) {
+ if (tickOffset == 0 || absPower < 1e-5) {
+ return;
+ }
+
+ setMode(RunMode.RUN_TO_POSITION);
+ addTargetPosition(tickOffset);
+ setPower(tickOffset > 0 ? Math.abs(absPower) : -Math.abs(absPower));
+ }
+
+ @Override
+ public void setMode(RunMode mode) {
+ first.setMode(mode);
+ second.setMode(mode);
+ }
+
+ @Override
+ public boolean isBusy() {
+ return first.isBusy() || second.isBusy();
+ }
+}
diff --git a/ARCCore/src/main/com/andoverrobotics/core/utilities/package-info.java b/ARCCore/src/main/com/andoverrobotics/core/utilities/package-info.java
new file mode 100644
index 0000000..b7f014a
--- /dev/null
+++ b/ARCCore/src/main/com/andoverrobotics/core/utilities/package-info.java
@@ -0,0 +1,4 @@
+/**
+ * Includes utility functions and constants.
+ */
+package com.andoverrobotics.core.utilities;
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/ExampleUnitTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/ExampleUnitTest.java
deleted file mode 100644
index 6a0da83..0000000
--- a/ARCCore/src/test/java/com/andoverrobotics/core/ExampleUnitTest.java
+++ /dev/null
@@ -1,17 +0,0 @@
-package com.andoverrobotics.core;
-
-import org.junit.Test;
-
-import static org.junit.Assert.*;
-
-/**
- * Example local unit test, which will execute on the development machine (host).
- *
- * @see Testing documentation
- */
-public class ExampleUnitTest {
- @Test
- public void addition_isCorrect() throws Exception {
- assertEquals(4, 2 + 2);
- }
-}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/config/ConfigurationTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/config/ConfigurationTest.java
new file mode 100644
index 0000000..a8cb05d
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/config/ConfigurationTest.java
@@ -0,0 +1,125 @@
+package com.andoverrobotics.core.config;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import java.io.IOException;
+import java.io.StringReader;
+import java.util.HashMap;
+import java.util.Map;
+import org.junit.Test;
+
+public class ConfigurationTest {
+
+ private static final Map schemaProvider = new HashMap() {{
+ put("autonomousTrials", "4");
+ put("motorSpeed", "0.412");
+ put("useEncoders", "false");
+ put("useSensors", "true");
+ put("robotName", "ConfigTestBot");
+ }};
+ private static final String schemaDataFileContents =
+ "autonomousTrials=4\n" +
+ "motorSpeed=0.412\n" +
+ "useEncoders=false\n" +
+ "useSensors=true\n" +
+ "robotName=ConfigTestBot";
+ private static final Configuration config = Configuration.from(schemaProvider);
+
+ // Must be static; otherwise, it will contain a "declared" field named "this",
+ // referring to ConfigurationTest.
+ static class TestSchema {
+
+ public int autonomousTrials;
+ public double motorSpeed;
+ public boolean useEncoders;
+ public boolean useSensors;
+ public String robotName;
+ }
+
+ static class BadTestSchema {
+
+ public double motorSpeed;
+ public boolean useEncoders;
+ public float badType;
+ }
+
+ @Test
+ public void fromPropertiesReaderGivenPropertiesReturnsCorrectly() throws IOException {
+ Configuration stringConfig =
+ Configuration.fromProperties(new StringReader(schemaDataFileContents));
+ assertEquals(config, stringConfig);
+ }
+
+ // NORMAL COURSE /////
+
+ @Test
+ public void getIntGivenIntKeyReturnsCorrectValue() {
+ assertEquals(4,
+ config.getInt("autonomousTrials"));
+ }
+
+ @Test
+ public void getDoubleGivenDoubleKeyReturnsCorrectValue() {
+ assertEquals(0.412,
+ config.getDouble("motorSpeed"), 1e-8);
+ }
+
+ @Test
+ public void getBooleanGivenBooleanKeyReturnsCorrectValue() {
+ assertFalse(config.getBoolean("useEncoders"));
+ }
+
+ @Test
+ public void getStringGivenKeyReturnsCorrectValue() {
+ assertEquals("ConfigTestBot",
+ config.getString("robotName"));
+ }
+
+ // BAD FORMAT COURSE /////
+
+ @Test(expected = InvalidFormatError.class)
+ public void getIntGivenFloatKeyThrowsException() {
+ config.getInt("motorSpeed");
+ }
+
+ @Test(expected = InvalidFormatError.class)
+ public void getDoubleGivenBooleanKeyThrowsException() {
+ config.getDouble("useEncoders");
+ }
+
+ @Test(expected = InvalidFormatError.class)
+ public void getBooleanGivenIntKeyThrowsException() {
+ config.getBoolean("autonomousTrials");
+ }
+
+ // NONEXISTENT FIELD COURSE /////
+
+ @Test(expected = NoSuchFieldError.class)
+ public void getGivenInvalidKeyThrowsException() {
+ config.getString("badKey");
+ }
+
+ @Test
+ public void fromMapToSchemaGivenProviderLoadsCorrectValues() {
+ TestSchema config = Configuration.from(schemaProvider).loadToSchema(new TestSchema());
+
+ assertEquals(4, config.autonomousTrials);
+ assertEquals(0.412, config.motorSpeed, 1e-5);
+ assertFalse(config.useEncoders);
+ assertTrue(config.useSensors);
+ assertEquals("ConfigTestBot", config.robotName);
+ }
+
+ @Test
+ public void equalsGivenEqualConfigsReturnsTrue() {
+ Configuration config1 = Configuration.from(schemaProvider);
+ assertEquals(config1, config);
+ }
+
+ @Test(expected = InvalidSchemaError.class)
+ public void fromMapToSchemaGivenBadSchemaThrowsException() {
+ Configuration.from(schemaProvider).loadToSchema(new BadTestSchema());
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/MecanumDriveTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/MecanumDriveTest.java
new file mode 100644
index 0000000..09901a2
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/MecanumDriveTest.java
@@ -0,0 +1,240 @@
+package com.andoverrobotics.core.drivetrain;
+
+import static org.mockito.AdditionalMatchers.or;
+import static org.mockito.ArgumentMatchers.eq;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.reset;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.verifyNoMoreInteractions;
+
+import com.andoverrobotics.core.utilities.Coordinate;
+import com.andoverrobotics.core.utilities.IMotor;
+import com.andoverrobotics.core.utilities.MotorPair;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+import org.junit.Before;
+import org.junit.Test;
+import org.mockito.AdditionalMatchers;
+
+public class MecanumDriveTest {
+ private OpMode opMode = mock(OpMode.class);
+ private MotorPair
+ rightDiagonal = mock(MotorPair.class),
+ leftDiagonal = mock(MotorPair.class),
+ rightSide = mock(MotorPair.class),
+ leftSide = mock(MotorPair.class);
+
+ private StrafingDriveTrain driveTrain = new MecanumDrive(
+ leftDiagonal, rightDiagonal, leftSide, rightSide,
+ opMode, 5, 100);
+
+ @Before
+ public void setUp() {
+ reset(rightDiagonal, leftDiagonal, rightSide, leftSide, opMode);
+ }
+
+ @Test
+ public void driveForwards() {
+ driveTrain.driveForwards(5, 1);
+
+ int tickOffset = (int) (5 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, 1);
+ verifyRunToPosition(rightDiagonal, tickOffset, 1);
+ }
+
+ @Test
+ public void driveBackwardsPositivePower() {
+ driveTrain.driveBackwards(8, 0.6);
+
+ int tickOffset = (int) (-8 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, -0.6);
+ verifyRunToPosition(rightDiagonal, tickOffset, -0.6);
+ }
+
+ @Test
+ public void driveBackwardsNegativePower() {
+ driveTrain.driveBackwards(8, -0.4);
+
+ int tickOffset = (int) (-8 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, -0.4);
+ verifyRunToPosition(rightDiagonal, tickOffset, -0.4);
+ }
+
+ @Test
+ public void driveBackwardsNegativeDistance() {
+ driveTrain.driveBackwards(-4, 0.6);
+
+ int tickOffset = (int) (-4 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, -0.6);
+ verifyRunToPosition(rightDiagonal, tickOffset, -0.6);
+ }
+
+ @Test
+ public void rotateClockwise() {
+ driveTrain.rotateClockwise(50, 0.6);
+
+ verifyRunToPosition(leftSide, (int) (50 / 360.0 * 100), 0.6);
+ verifyRunToPosition(rightSide, (int) (-50 / 360.0 * 100), 0.6);
+ }
+
+ @Test
+ public void rotateClockwiseNegativeDegree() {
+ driveTrain.rotateClockwise(-36, 0.2);
+
+ verifyRunToPosition(leftSide, (int) ((360 - 36) / 360.0 * 100), 0.2);
+ verifyRunToPosition(rightSide, (int) ((36 - 360) / 360.0 * 100), 0.2);
+ }
+
+ @Test
+ public void rotateCounterClockwise() {
+ driveTrain.rotateCounterClockwise(50, -0.5);
+
+ verifyRunToPosition(leftSide, (int) (-50 / 360.0 * 100), 0.5);
+ verifyRunToPosition(rightSide, (int) (50 / 360.0 * 100), 0.5);
+ }
+
+ @Test
+ public void rotateCounterClockwiseNegativeDegree() {
+ driveTrain.rotateCounterClockwise(-70, -0.8);
+
+ verifyRunToPosition(leftSide, (int) ((70 - 360) / 360.0 * 100), 0.8);
+ verifyRunToPosition(rightSide, (int) ((360 - 70) / 360.0 * 100), 0.8);
+ }
+
+ @Test
+ public void strafeRight() {
+ driveTrain.strafeRight(20.5, 0.6);
+
+ int tickOffset = (int) (20.5 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, -tickOffset, -0.6);
+ verifyRunToPosition(rightDiagonal, tickOffset, 0.6);
+ }
+
+ @Test
+ public void strafeRightNegativeDistance() {
+ driveTrain.strafeRight(-14.8, -0.7);
+
+ int tickOffset = (int) (14.8 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, -tickOffset, -0.7);
+ verifyRunToPosition(rightDiagonal, tickOffset, 0.7);
+ }
+
+ @Test
+ public void strafeLeft() {
+ driveTrain.strafeLeft(23.1, 0.4);
+
+ int tickOffset = (int) (23.1 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, 0.4);
+ verifyRunToPosition(rightDiagonal, -tickOffset, -0.4);
+ }
+
+ @Test
+ public void strafeLeftNegativeDistance() {
+ driveTrain.strafeLeft(-24.5, -0.64);
+
+ int tickOffset = (int) (24.5 * 5 / Math.sqrt(2));
+
+ verifyRunToPosition(leftDiagonal, tickOffset, 0.64);
+ verifyRunToPosition(rightDiagonal, -tickOffset, -0.64);
+ }
+
+ @Test
+ public void strafeInches() {
+ driveTrain.strafeInches(10, 15, 0.4);
+
+ verifyRunToPosition(leftDiagonal, (int) (3.5355339 * 5), 0.08);
+ verifyRunToPosition(rightDiagonal, (int) (17.67766953 * 5), 0.4);
+ }
+
+ @Test
+ public void strafeInchesNullVector() {
+ driveTrain.strafeInches(0, 0, 0.4);
+
+ verifyNoMoreInteractions(leftDiagonal, rightDiagonal);
+ }
+
+ @Test
+ public void setMovementPower() {
+ driveTrain.setMovementPower(0.7);
+ verifyPowersWithoutEncoder(0.7, 0.7);
+ }
+
+ @Test
+ public void setRotationPower() {
+ driveTrain.setRotationPower(0.4);
+ verifyPowersWithoutEncoder(0.4, -0.4);
+ }
+
+ @Test
+ public void setRotationPowerNegative() {
+ driveTrain.setRotationPower(-1.2);
+ verifyPowersWithoutEncoder(-1, 1);
+ }
+
+ @Test
+ public void setMovementAndRotation() {
+ driveTrain.setMovementAndRotation(0.5, 0.2);
+ verifyPowersWithoutEncoder(0.7, 0.3);
+ }
+
+ @Test
+ public void setMovementAndRotationWithSingleOverflow() {
+ driveTrain.setMovementAndRotation(0.8, -0.6);
+ verifyPowersWithoutEncoder(0.2 / 1.4, 1);
+ }
+
+ @Test
+ public void setMovementAndRotationWithMultipleOverflows() {
+ driveTrain.setMovementAndRotation(0.8, 5);
+ verifyPowersWithoutEncoder(1, -4.2 / 5.8);
+ }
+
+ @Test
+ public void setStrafeWithinUnitCircle() {
+ driveTrain.setStrafe(Coordinate.fromXY(0.5, 0.7), 1);
+
+ verifyPairPower(leftDiagonal, 0.141421356);
+ verifyPairPower(rightDiagonal, 0.8485281374);
+ }
+
+ @Test
+ public void setStrafeOutsideUnitCircle() {
+ driveTrain.setStrafe(Coordinate.fromXY(525, -1441), 0.5);
+
+ verifyPairPower(leftDiagonal, -0.45322105262);
+ verifyPairPower(rightDiagonal, -0.211165047915);
+ }
+
+ @Test
+ public void setStrafeWithOrigin() {
+ driveTrain.setStrafe(0, 0, Double.MAX_VALUE);
+
+ verifyPairPower(leftDiagonal, 0);
+ verifyPairPower(rightDiagonal, 0);
+ }
+
+ private void verifyPowersWithoutEncoder(double leftPower, double rightPower) {
+ verify(leftSide).setMode(RunMode.RUN_WITHOUT_ENCODER);
+ verify(rightSide).setMode(RunMode.RUN_WITHOUT_ENCODER);
+
+ verifyPairPower(leftSide, leftPower);
+ verifyPairPower(rightSide, rightPower);
+ }
+
+ private void verifyRunToPosition(IMotor mock, int tickOffset, double power) {
+ verify(mock).startRunToPosition(eq(tickOffset),
+ or(AdditionalMatchers.eq(power, 1e-4),
+ AdditionalMatchers.eq(-power, 1e-4)));
+ }
+
+ private void verifyPairPower(MotorPair pair, double power) {
+ verify(pair).setPower(AdditionalMatchers.eq(power, 1e-3));
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/TankDriveTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/TankDriveTest.java
new file mode 100644
index 0000000..27489ee
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/drivetrain/TankDriveTest.java
@@ -0,0 +1,180 @@
+package com.andoverrobotics.core.drivetrain;
+
+import static org.mockito.ArgumentMatchers.eq;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.reset;
+import static org.mockito.Mockito.verify;
+
+import com.andoverrobotics.core.utilities.IMotor;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import org.junit.Before;
+import org.junit.Test;
+import org.mockito.AdditionalMatchers;
+
+public class TankDriveTest {
+
+ private final OpMode opMode = mock(OpMode.class);
+ private final IMotor motorL = mock(IMotor.class),
+ motorR = mock(IMotor.class);
+ private DriveTrain driveTrain = new TankDrive(motorL, motorR, opMode,50, 720);
+
+ @Before
+ public void setUp() {
+ reset(motorL);
+ reset(motorR);
+ }
+
+ @Test
+ public void driveForwards() {
+ driveTrain.driveForwards(5, 0.5);
+ verifyDrivenDisplacementWithPower(5, 0.5);
+ }
+
+ @Test
+ public void driveForwardsWithNegativeDistance() {
+ driveTrain.driveForwards(-5, 0.5);
+ verifyDrivenDisplacementWithPower(5, 0.5);
+ }
+
+ @Test
+ public void driveForwardsWithNegativePower() {
+ driveTrain.driveForwards(5, -0.5);
+ verifyDrivenDisplacementWithPower(5, 0.5);
+ }
+
+ @Test
+ public void driveForwardsWithNegativeDistanceAndPower() {
+ driveTrain.driveForwards(-5, -0.5);
+ verifyDrivenDisplacementWithPower(5, 0.5);
+ }
+
+ @Test
+ public void driveForwardsWithZeroPower() {
+ driveTrain.driveForwards(2, 0);
+ verifyPowersSet(0, 0);
+ }
+
+ @Test
+ public void driveBackwards() {
+ driveTrain.driveBackwards(5, 0.7);
+ verifyDrivenDisplacementWithPower(-5, -0.7);
+ }
+
+ @Test
+ public void driveBackwardsWithNegativeDistance() {
+ driveTrain.driveBackwards(-5, 0.7);
+ verifyDrivenDisplacementWithPower(-5, -0.7);
+ }
+
+ @Test
+ public void driveBackwardsWithNegativePower() {
+ driveTrain.driveBackwards(5, -0.7);
+ verifyDrivenDisplacementWithPower(-5, -0.7);
+ }
+
+ @Test
+ public void driveBackwardsWithZeroPower() {
+ driveTrain.driveBackwards(2, 0);
+ verifyPowersSet(0, 0);
+ }
+
+ @Test
+ public void rotateClockwise() {
+ driveTrain.rotateClockwise(270, 0.6);
+
+ verifyRunToPosition(270 * 2, -270 * 2, 0.6, -0.6);
+ }
+
+ @Test
+ public void rotateClockwiseNegativeDegree() {
+ driveTrain.rotateClockwise(-45, 0.4);
+
+ verifyRunToPosition((360-45) * 2, (45-360) * 2, 0.4, -0.4);
+ }
+
+ @Test
+ public void rotateClockwiseNegativePower() {
+ driveTrain.rotateClockwise(45, -0.4);
+
+ verifyRunToPosition(45 * 2, -45 * 2, 0.4, -0.4);
+ }
+
+ @Test
+ public void rotateCounterClockwise() {
+ driveTrain.rotateCounterClockwise(70, 0.2);
+
+ verifyRunToPosition(-70 * 2, 70 * 2, -0.2, 0.2);
+ }
+
+ @Test
+ public void rotateCounterClockwiseNegativePower() {
+ driveTrain.rotateCounterClockwise(80, -0.3);
+
+ verifyRunToPosition(-80 * 2, 80 * 2, -0.3, 0.3);
+ }
+
+ @Test
+ public void rotateCounterClockwiseNegativeDegree() {
+ driveTrain.rotateCounterClockwise(-50, 0.5);
+
+ verifyRunToPosition((50-360) * 2, (360-50) * 2, -0.5, 0.5);
+ }
+
+ @Test
+ public void setMovementPower() {
+ driveTrain.setMovementPower(1.0);
+ verifyPowersSet(1.0, 1.0);
+
+ driveTrain.setMovementPower(-0.6);
+ verifyPowersSet(-0.6, -0.6);
+
+ driveTrain.setMovementPower(0.0);
+ verifyPowersSet(0.0, 0.0);
+ }
+
+ @Test
+ public void setRotationPower() {
+ driveTrain.setRotationPower(0.2);
+ verifyPowersSet(0.2, -0.2);
+
+ driveTrain.setRotationPower(-0.4);
+ verifyPowersSet(-0.4, 0.4);
+
+ driveTrain.setRotationPower(0.8);
+ verifyPowersSet(0.8, -0.8);
+ }
+
+ @Test
+ public void setMovementAndRotation() {
+ driveTrain.setMovementAndRotation(0.5, 0.2);
+ verifyPowersSet(0.7, 0.3);
+
+ driveTrain.setMovementAndRotation(0.5, 0.8);
+ verifyPowersSet(1, -0.3/1.3);
+
+ driveTrain.setMovementAndRotation(-0.4, -0.3);
+ verifyPowersSet(-0.7, -0.1);
+
+ driveTrain.setMovementAndRotation(-0.9, -0.2);
+ verifyPowersSet(-1, -0.7/1.1);
+ }
+
+ // -- Start verification methods --
+
+ private void verifyDrivenDisplacementWithPower(
+ int inches, double power) {
+ verifyRunToPosition(inches * 50, inches * 50, power, power);
+ }
+
+ private void verifyRunToPosition(int ticksLeft, int ticksRight, double powerLeft, double powerRight) {
+ verify(motorL).startRunToPosition(
+ eq(ticksLeft), AdditionalMatchers.eq(powerLeft, 1e-4));
+ verify(motorR).startRunToPosition(
+ eq(ticksRight), AdditionalMatchers.eq(powerRight, 1e-4));
+ }
+
+ private void verifyPowersSet(double left, double right) {
+ verify(motorL).setPower(AdditionalMatchers.eq(left, 1e-4));
+ verify(motorR).setPower(AdditionalMatchers.eq(right, 1e-4));
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/utilities/ConverterTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/ConverterTest.java
new file mode 100644
index 0000000..bf8a975
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/ConverterTest.java
@@ -0,0 +1,47 @@
+package com.andoverrobotics.core.utilities;
+
+import static org.junit.Assert.*;
+
+import org.junit.Test;
+
+public class ConverterTest {
+
+ @Test
+ public void degreesToRadiansInRange() {
+ assertEquals(0.0, Converter.degreesToRadians(0), 1e-7);
+ assertEquals(Math.PI / 4, Converter.degreesToRadians(45), 1e-7);
+ assertEquals(Math.PI / 2, Converter.degreesToRadians(90), 1e-7);
+ }
+
+ @Test
+ public void degreesToRadiansUnderRange() {
+ assertEquals(Math.PI, Converter.degreesToRadians(-180), 1e-7);
+ assertEquals(Math.PI / 2, Converter.degreesToRadians(-270), 1e-7);
+ assertEquals(Math.PI, Converter.degreesToRadians(-180 - 360), 1e-7);
+ }
+
+ @Test
+ public void degreesToRadiansAboveRange() {
+ assertEquals(0.0, Converter.degreesToRadians(720), 1e-7);
+ assertEquals(Math.PI, Converter.degreesToRadians(180 + 360 * 3), 1e-7);
+ }
+
+ @Test
+ public void radiansToDegreesInRange() {
+ assertEquals(0, Converter.radiansToDegrees(0.0), 1e-7);
+ assertEquals(45, Converter.radiansToDegrees(Math.PI / 4), 1e-7);
+ assertEquals(90, Converter.radiansToDegrees(Math.PI / 2), 1e-7);
+ }
+
+ @Test
+ public void radiansToDegreesUnderRange() {
+ assertEquals(180, Converter.radiansToDegrees(-Math.PI), 1e-7);
+ assertEquals(90, Converter.radiansToDegrees(-Math.PI * 1.5), 1e-7);
+ }
+
+ @Test
+ public void radiansToDegreesAboveRange() {
+ assertEquals(135, Converter.radiansToDegrees(Math.PI * 2.75), 1e-7);
+ assertEquals(270, Converter.radiansToDegrees(Math.PI * 5.5), 1e-7);
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/utilities/CoordinateTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/CoordinateTest.java
new file mode 100644
index 0000000..205c8c1
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/CoordinateTest.java
@@ -0,0 +1,75 @@
+package com.andoverrobotics.core.utilities;
+
+import static org.junit.Assert.assertEquals;
+
+import org.junit.Test;
+
+public class CoordinateTest {
+ private static Coordinate testee;
+
+ @Test
+ public void getPolarDirection() {
+ givenEuclidean(1, 0);
+ assertEquals(0, testee.getPolarDirection(), 1e-7);
+
+ givenEuclidean(2, 2);
+ assertEquals(45, testee.getPolarDirection(), 1e-7);
+
+ givenEuclidean(3, -1);
+ assertEquals(341.565051177, testee.getPolarDirection(), 1e-7);
+ }
+
+ @Test
+ public void getPolarDistance() {
+ givenEuclidean(3, 4);
+ assertEquals(5, testee.getPolarDistance(), 1e-7);
+
+ givenPolar(4.24, 120);
+ assertEquals(4.24, testee.getPolarDistance(), 1e-7);
+ }
+
+ @Test
+ public void fromPolar() {
+ givenPolar(10, 45);
+ assertEquals(10 / Math.sqrt(2), testee.getX(), 1e-7);
+ assertEquals(10 / Math.sqrt(2), testee.getY(), 1e-7);
+
+ givenPolar(20, 140);
+ assertEquals(20 * Math.cos(2.44346), testee.getX(), 1e-4);
+ assertEquals(20 * Math.sin(2.44346), testee.getY(), 1e-4);
+ }
+
+ @Test
+ public void rotate() {
+ givenPolar(5, 24);
+
+ testee = testee.rotate(30);
+
+ assertEquals(5, testee.getPolarDistance(), 1e-5);
+ assertEquals(54, testee.getPolarDirection(), 1e-5);
+ assertEquals(5 * Math.cos(54 / 180.0 * Math.PI), testee.getX(), 1e-5);
+ assertEquals(5 * Math.sin(54 / 180.0 * Math.PI), testee.getY(), 1e-5);
+
+ givenPolar(5, 350);
+
+ testee = testee.rotate(20);
+
+ assertEquals(5, testee.getPolarDistance(), 1e-5);
+ assertEquals(10, testee.getPolarDirection(), 1e-5);
+
+ testee = testee.rotate(-35);
+
+ assertEquals(5 * Math.cos(25 / 180.0 * Math.PI), testee.getX(), 1e-5);
+ assertEquals(-5 * Math.sin(25 / 180.0 * Math.PI), testee.getY(), 1e-5);
+ }
+
+ private void given(Coordinate coord) {
+ testee = coord;
+ }
+ private void givenEuclidean(double x, double y) {
+ given(Coordinate.fromXY(x, y));
+ }
+ private void givenPolar(double distance, int degrees) {
+ given(Coordinate.fromPolar(distance, degrees));
+ }
+}
\ No newline at end of file
diff --git a/ARCCore/src/test/java/com/andoverrobotics/core/utilities/MotorPairTest.java b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/MotorPairTest.java
new file mode 100644
index 0000000..b34b235
--- /dev/null
+++ b/ARCCore/src/test/java/com/andoverrobotics/core/utilities/MotorPairTest.java
@@ -0,0 +1,72 @@
+package com.andoverrobotics.core.utilities;
+
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.reset;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotor.RunMode;
+import org.junit.Before;
+import org.junit.Test;
+
+public class MotorPairTest {
+ private DcMotor one = mock(DcMotor.class),
+ two = mock(DcMotor.class);
+
+ private MotorPair pair = MotorPair.of(one, two);
+
+ @Before
+ public void setUp() {
+ reset(one, two);
+ }
+
+ @Test
+ public void setPower() {
+ pair.setPower(0.2);
+
+ verify(one).setPower(0.2);
+ verify(two).setPower(0.2);
+ }
+
+ @Test
+ public void addTargetPosition() {
+ when(one.getCurrentPosition()).thenReturn(50);
+ when(two.getCurrentPosition()).thenReturn(-40);
+
+ pair.addTargetPosition(20);
+
+ verify(one).setTargetPosition(70);
+ verify(two).setTargetPosition(-20);
+
+ pair.addTargetPosition(-10);
+
+ verify(one).setTargetPosition(40);
+ verify(two).setTargetPosition(-50);
+ }
+
+ @Test
+ public void setMode() {
+ pair.setMode(RunMode.RUN_USING_ENCODER);
+
+ verify(one).setMode(RunMode.RUN_USING_ENCODER);
+ verify(two).setMode(RunMode.RUN_USING_ENCODER);
+ }
+
+ @Test
+ public void isBusy() {
+ when(one.isBusy()).thenReturn(true);
+
+ assertTrue(pair.isBusy());
+ }
+
+ @Test
+ public void isNotBusy() {
+ when(one.isBusy()).thenReturn(false);
+ when(two.isBusy()).thenReturn(false);
+
+ assertFalse(pair.isBusy());
+ }
+}
\ No newline at end of file
diff --git a/DogeCV/build.gradle b/DogeCV/build.gradle
index 7d2f7a5..de87867 100644
--- a/DogeCV/build.gradle
+++ b/DogeCV/build.gradle
@@ -2,7 +2,6 @@ apply plugin: 'com.android.library'
android {
compileSdkVersion 23
- buildToolsVersion '26.0.2'
defaultConfig {
minSdkVersion 19
@@ -21,6 +20,6 @@ android {
}
dependencies {
- compile fileTree(dir: 'libs', include: ['*.jar'])
- compile project(path: ':openCVLibrary320')
+ implementation fileTree(dir: 'libs', include: ['*.jar'])
+ implementation project(path: ':openCVLibrary320')
}
diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle
index b73fbe4..13a0bb9 100644
--- a/FtcRobotController/build.gradle
+++ b/FtcRobotController/build.gradle
@@ -11,7 +11,6 @@ android {
}
compileSdkVersion 23
- buildToolsVersion '25.0.3'
compileOptions {
sourceCompatibility JavaVersion.VERSION_1_7
diff --git a/FtcRobotController/build.release.gradle b/FtcRobotController/build.release.gradle
index 4b3fe90..1cb2035 100644
--- a/FtcRobotController/build.release.gradle
+++ b/FtcRobotController/build.release.gradle
@@ -4,6 +4,5 @@ dependencies {
compile (name:'RobotCore-release', ext: 'aar')
compile (name:'Hardware-release', ext: 'aar')
compile (name:'FtcCommon-release', ext: 'aar')
- compile (name:'Analytics-release', ext:'aar')
compile (name:'WirelessP2p-release', ext:'aar')
}
diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml
index 63fa417..387f1bb 100644
--- a/FtcRobotController/src/main/AndroidManifest.xml
+++ b/FtcRobotController/src/main/AndroidManifest.xml
@@ -2,8 +2,8 @@
+ android:versionCode="28"
+ android:versionName="4.0">
diff --git a/FtcRobotController/src/main/assets/RoverRuckus.dat b/FtcRobotController/src/main/assets/RoverRuckus.dat
new file mode 100644
index 0000000..461add8
Binary files /dev/null and b/FtcRobotController/src/main/assets/RoverRuckus.dat differ
diff --git a/FtcRobotController/src/main/assets/RoverRuckus.xml b/FtcRobotController/src/main/assets/RoverRuckus.xml
new file mode 100644
index 0000000..05eac7e
--- /dev/null
+++ b/FtcRobotController/src/main/assets/RoverRuckus.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java
index b5920f9..cd1da0a 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CompassSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -52,7 +52,7 @@
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name="Concept: Compass Calibration", group="Concept")
+@TeleOp(name="Concept: Compass Calibration", group="Concept")
@Disabled
public class ConceptCompassCalibration extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java
index 699d2e5..75b6ad4 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -51,7 +51,7 @@
* Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Concept: DIM As Indicator", group = "Concept")
+@TeleOp(name = "Concept: DIM As Indicator", group = "Concept")
@Disabled
public class ConceptDIMAsIndicator extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java
index ee83ec0..2f84695 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
import java.text.SimpleDateFormat;
@@ -40,7 +40,7 @@
/**
* Demonstrates empty OpMode
*/
-@Autonomous(name = "Concept: NullOp", group = "Concept")
+@TeleOp(name = "Concept: NullOp", group = "Concept")
@Disabled
public class ConceptNullOp extends OpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
index 4508540..0a8f3dc 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
/**
@@ -46,7 +46,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Concept: Ramp Motor Speed", group = "Concept")
+@TeleOp(name = "Concept: Ramp Motor Speed", group = "Concept")
@Disabled
public class ConceptRampMotorSpeed extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRegisterOpModes.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRegisterOpModes.java
index a0046ba..2427c7f 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRegisterOpModes.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRegisterOpModes.java
@@ -111,5 +111,8 @@ public static void registerMyOpModes(OpModeManager manager) {
// manager.register("Scan Servo", ConceptScanServo.class);
// manager.register("Telemetry", ConceptTelemetry.class);
// manager.register("Vuforia Navigation", ConceptVuforiaNavigation.class);
+ // manager.register("Vuforia Navigation Webcam", ConceptVuforiaNavigationWebcam.class);
+ // manager.register("Vuforia VuMarkID", ConceptVuforiaVuMarkIdentification.class);
+ // manager.register("Vuforia VuMarkID Webcam", ConceptVuforiaVuMarkIdentificationWebcam.class);
}
}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java
index 0f4e235..ce389ce 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
/**
@@ -48,7 +48,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Concept: Scan Servo", group = "Concept")
+@TeleOp(name = "Concept: Scan Servo", group = "Concept")
@Disabled
public class ConceptScanServo extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java
index 5994561..d3b4682 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -47,7 +47,7 @@
*
* @see Telemetry
*/
-@Autonomous(name = "Concept: Telemetry", group = "Concept")
+@TeleOp(name = "Concept: Telemetry", group = "Concept")
@Disabled
public class ConceptTelemetry extends LinearOpMode {
/** keeps track of the line of the poem which is to be emitted next */
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java
index 041e2a2..f0d735b 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java
@@ -28,7 +28,7 @@
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -65,7 +65,7 @@
* is explained in {@link ConceptVuforiaNavigation}.
*/
-@Autonomous(name="Concept: VuMark Id", group ="Concept")
+@TeleOp(name="Concept: VuMark Id", group ="Concept")
@Disabled
public class ConceptVuMarkIdentification extends LinearOpMode {
@@ -83,7 +83,6 @@ public class ConceptVuMarkIdentification extends LinearOpMode {
/*
* To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
- * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
*/
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
@@ -101,9 +100,9 @@ public class ConceptVuMarkIdentification extends LinearOpMode {
* random data. As an example, here is a example of a fragment of a valid key:
* ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
* Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code onthe next line, between the double quotes.
+ * and paste it in to your code on the next line, between the double quotes.
*/
- parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
+ parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
/*
* We also indicate which camera on the RC that we wish to use.
@@ -111,7 +110,12 @@ public class ConceptVuMarkIdentification extends LinearOpMode {
* for a competition robot, the front camera might be more convenient.
*/
parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
- this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
+
+ /**
+ * Instantiate the Vuforia engine
+ */
+ vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
/**
* Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java
new file mode 100644
index 0000000..866261f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java
@@ -0,0 +1,194 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
+import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+
+/**
+ * This OpMode illustrates the basics of using the Vuforia engine to determine
+ * the identity of Vuforia VuMarks encountered on the field. The code is structured as
+ * a LinearOpMode. It shares much structure with {@link ConceptVuforiaNavigationWebcam}; we do not here
+ * duplicate the core Vuforia documentation found there, but rather instead focus on the
+ * differences between the use of Vuforia for navigation vs VuMark identification.
+ *
+ * @see ConceptVuforiaNavigationWebcam
+ * @see VuforiaLocalizer
+ * @see VuforiaTrackableDefaultListener
+ * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
+ *
+ * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
+ * is explained in {@link ConceptVuforiaNavigationWebcam}.
+ */
+
+@TeleOp(name="Concept: VuMark Id Webcam", group ="Concept")
+@Disabled
+public class ConceptVuMarkIdentificationWebcam extends LinearOpMode {
+
+ public static final String TAG = "Vuforia VuMark Sample";
+
+ OpenGLMatrix lastLocation = null;
+
+ /**
+ * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
+ * localization engine.
+ */
+ VuforiaLocalizer vuforia;
+
+ /**
+ * This is the webcam we are to use. As with other hardware devices such as motors and
+ * servos, this device is identified using the robot configuration tool in the FTC application.
+ */
+ WebcamName webcamName;
+
+ @Override public void runOpMode() {
+
+ /*
+ * Retrieve the camera we are to use.
+ */
+ webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
+
+ /*
+ * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
+ * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
+ */
+ int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+ VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+ // OR... Do Not Activate the Camera Monitor View, to save power
+ // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+ /*
+ * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
+ * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
+ * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
+ * web site at https://developer.vuforia.com/license-manager.
+ *
+ * Vuforia license keys are always 380 characters long, and look as if they contain mostly
+ * random data. As an example, here is a example of a fragment of a valid key:
+ * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
+ * Once you've obtained a license key, copy the string from the Vuforia web site
+ * and paste it in to your code on the next line, between the double quotes.
+ */
+ parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
+
+
+ /**
+ * We also indicate which camera on the RC we wish to use. For pedagogical purposes,
+ * we use the same logic as in {@link ConceptVuforiaNavigationWebcam}.
+ */
+ parameters.cameraName = webcamName;
+ this.vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+ /**
+ * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
+ * in this data set: all three of the VuMarks in the game were created from this one template,
+ * but differ in their instance id information.
+ * @see VuMarkInstanceId
+ */
+ VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
+ VuforiaTrackable relicTemplate = relicTrackables.get(0);
+ relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
+
+ telemetry.addData(">", "Press Play to start");
+ telemetry.update();
+ waitForStart();
+
+ relicTrackables.activate();
+
+ while (opModeIsActive()) {
+
+ /**
+ * See if any of the instances of {@link relicTemplate} are currently visible.
+ * {@link RelicRecoveryVuMark} is an enum which can have the following values:
+ * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
+ * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
+ */
+ RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
+ if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
+
+ /* Found an instance of the template. In the actual game, you will probably
+ * loop until this condition occurs, then move on to act accordingly depending
+ * on which VuMark was visible. */
+ telemetry.addData("VuMark", "%s visible", vuMark);
+
+ /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
+ * it is perhaps unlikely that you will actually need to act on this pose information, but
+ * we illustrate it nevertheless, for completeness. */
+ OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getFtcCameraFromTarget();
+ telemetry.addData("Pose", format(pose));
+
+ /* We further illustrate how to decompose the pose into useful rotational and
+ * translational components */
+ if (pose != null) {
+ VectorF trans = pose.getTranslation();
+ Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
+
+ // Extract the X, Y, and Z components of the offset of the target relative to the robot
+ double tX = trans.get(0);
+ double tY = trans.get(1);
+ double tZ = trans.get(2);
+
+ // Extract the rotational components of the target relative to the robot
+ double rX = rot.firstAngle;
+ double rY = rot.secondAngle;
+ double rZ = rot.thirdAngle;
+ }
+ }
+ else {
+ telemetry.addData("VuMark", "not visible");
+ }
+
+ telemetry.update();
+ }
+ }
+
+ String format(OpenGLMatrix transformationMatrix) {
+ return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavRoverRuckus.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavRoverRuckus.java
new file mode 100644
index 0000000..eb21cf7
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavRoverRuckus.java
@@ -0,0 +1,311 @@
+/* Copyright (c) 2018 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+
+import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
+import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK;
+import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.FRONT;
+
+import java.util.ArrayList;
+import java.util.List;
+
+
+/**
+ * This 2018-2019 OpMode illustrates the basics of using the Vuforia localizer to determine
+ * positioning and orientation of robot on the FTC field.
+ * The code is structured as a LinearOpMode
+ *
+ * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images.
+ *
+ * When images are located, Vuforia is able to determine the position and orientation of the
+ * image relative to the camera. This sample code than combines that information with a
+ * knowledge of where the target images are on the field, to determine the location of the camera.
+ *
+ * This example assumes a "square" field configuration where the red and blue alliance stations
+ * are on opposite walls of each other.
+ *
+ * From the Audience perspective, the Red Alliance station is on the right and the
+ * Blue Alliance Station is on the left.
+
+ * The four vision targets are located in the center of each of the perimeter walls with
+ * the images facing inwards towards the robots:
+ * - BlueRover is the Mars Rover image target on the wall closest to the blue alliance
+ * - RedFootprint is the Lunar Footprint target on the wall closest to the red alliance
+ * - FrontCraters is the Lunar Craters image target on the wall closest to the audience
+ * - BackSpace is the Deep Space image target on the wall farthest from the audience
+ *
+ * A final calculation then uses the location of the camera on the robot to determine the
+ * robot's location and orientation on the field.
+ *
+ * @see VuforiaLocalizer
+ * @see VuforiaTrackableDefaultListener
+ * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
+ *
+ * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
+ * is explained below.
+ */
+
+@TeleOp(name="Concept: Vuforia Rover Nav", group ="Concept")
+@Disabled
+public class ConceptVuforiaNavRoverRuckus extends LinearOpMode {
+
+ /*
+ * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
+ * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
+ * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
+ * web site at https://developer.vuforia.com/license-manager.
+ *
+ * Vuforia license keys are always 380 characters long, and look as if they contain mostly
+ * random data. As an example, here is a example of a fragment of a valid key:
+ * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
+ * Once you've obtained a license key, copy the string from the Vuforia web site
+ * and paste it in to your code on the next line, between the double quotes.
+ */
+ private static final String VUFORIA_KEY = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
+
+ // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension.
+ // We will define some constants and conversions here
+ private static final float mmPerInch = 25.4f;
+ private static final float mmFTCFieldWidth = (12*6) * mmPerInch; // the width of the FTC field (from the center point to the outer panels)
+ private static final float mmTargetHeight = (6) * mmPerInch; // the height of the center of the target image above the floor
+
+ // Select which camera you want use. The FRONT camera is the one on the same side as the screen.
+ // Valid choices are: BACK or FRONT
+ private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK;
+
+ private OpenGLMatrix lastLocation = null;
+ private boolean targetVisible = false;
+
+ /**
+ * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
+ * localization engine.
+ */
+ VuforiaLocalizer vuforia;
+
+ @Override public void runOpMode() {
+ /*
+ * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
+ * We can pass Vuforia the handle to a camera preview resource (on the RC phone);
+ * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
+ */
+ int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+ VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+ // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+ parameters.vuforiaLicenseKey = VUFORIA_KEY ;
+ parameters.cameraDirection = CAMERA_CHOICE;
+
+ // Instantiate the Vuforia engine
+ vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+ // Load the data sets that for the trackable objects. These particular data
+ // sets are stored in the 'assets' part of our application.
+ VuforiaTrackables targetsRoverRuckus = this.vuforia.loadTrackablesFromAsset("RoverRuckus");
+ VuforiaTrackable blueRover = targetsRoverRuckus.get(0);
+ blueRover.setName("Blue-Rover");
+ VuforiaTrackable redFootprint = targetsRoverRuckus.get(1);
+ redFootprint.setName("Red-Footprint");
+ VuforiaTrackable frontCraters = targetsRoverRuckus.get(2);
+ frontCraters.setName("Front-Craters");
+ VuforiaTrackable backSpace = targetsRoverRuckus.get(3);
+ backSpace.setName("Back-Space");
+
+ // For convenience, gather together all the trackable objects in one easily-iterable collection */
+ List allTrackables = new ArrayList();
+ allTrackables.addAll(targetsRoverRuckus);
+
+ /**
+ * In order for localization to work, we need to tell the system where each target is on the field, and
+ * where the phone resides on the robot. These specifications are in the form of transformation matrices.
+ * Transformation matrices are a central, important concept in the math here involved in localization.
+ * See Transformation Matrix
+ * for detailed information. Commonly, you'll encounter transformation matrices as instances
+ * of the {@link OpenGLMatrix} class.
+ *
+ * If you are standing in the Red Alliance Station looking towards the center of the field,
+ * - The X axis runs from your left to the right. (positive from the center to the right)
+ * - The Y axis runs from the Red Alliance Station towards the other side of the field
+ * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station)
+ * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor)
+ *
+ * This Rover Ruckus sample places a specific target in the middle of each perimeter wall.
+ *
+ * Before being transformed, each target image is conceptually located at the origin of the field's
+ * coordinate system (the center of the field), facing up.
+ */
+
+ /**
+ * To place the BlueRover target in the middle of the blue perimeter wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright.
+ * - Then, we translate it along the Y axis to the blue perimeter wall.
+ */
+ OpenGLMatrix blueRoverLocationOnField = OpenGLMatrix
+ .translation(0, mmFTCFieldWidth, mmTargetHeight)
+ .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0));
+ blueRover.setLocation(blueRoverLocationOnField);
+
+ /**
+ * To place the RedFootprint target in the middle of the red perimeter wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright.
+ * - Second, we rotate it 180 around the field's Z axis so the image is flat against the red perimeter wall
+ * and facing inwards to the center of the field.
+ * - Then, we translate it along the negative Y axis to the red perimeter wall.
+ */
+ OpenGLMatrix redFootprintLocationOnField = OpenGLMatrix
+ .translation(0, -mmFTCFieldWidth, mmTargetHeight)
+ .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180));
+ redFootprint.setLocation(redFootprintLocationOnField);
+
+ /**
+ * To place the FrontCraters target in the middle of the front perimeter wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright.
+ * - Second, we rotate it 90 around the field's Z axis so the image is flat against the front wall
+ * and facing inwards to the center of the field.
+ * - Then, we translate it along the negative X axis to the front perimeter wall.
+ */
+ OpenGLMatrix frontCratersLocationOnField = OpenGLMatrix
+ .translation(-mmFTCFieldWidth, 0, mmTargetHeight)
+ .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90));
+ frontCraters.setLocation(frontCratersLocationOnField);
+
+ /**
+ * To place the BackSpace target in the middle of the back perimeter wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright.
+ * - Second, we rotate it -90 around the field's Z axis so the image is flat against the back wall
+ * and facing inwards to the center of the field.
+ * - Then, we translate it along the X axis to the back perimeter wall.
+ */
+ OpenGLMatrix backSpaceLocationOnField = OpenGLMatrix
+ .translation(mmFTCFieldWidth, 0, mmTargetHeight)
+ .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90));
+ backSpace.setLocation(backSpaceLocationOnField);
+
+ /**
+ * Create a transformation matrix describing where the phone is on the robot.
+ *
+ * The coordinate frame for the robot looks the same as the field.
+ * The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis.
+ * Z is UP on the robot. This equates to a bearing angle of Zero degrees.
+ *
+ * The phone starts out lying flat, with the screen facing Up and with the physical top of the phone
+ * pointing to the LEFT side of the Robot. It's very important when you test this code that the top of the
+ * camera is pointing to the left side of the robot. The rotation angles don't work if you flip the phone.
+ *
+ * If using the rear (High Res) camera:
+ * We need to rotate the camera around it's long axis to bring the rear camera forward.
+ * This requires a negative 90 degree rotation on the Y axis
+ *
+ * If using the Front (Low Res) camera
+ * We need to rotate the camera around it's long axis to bring the FRONT camera forward.
+ * This requires a Positive 90 degree rotation on the Y axis
+ *
+ * Next, translate the camera lens to where it is on the robot.
+ * In this example, it is centered (left to right), but 110 mm forward of the middle of the robot, and 200 mm above ground level.
+ */
+
+ final int CAMERA_FORWARD_DISPLACEMENT = 110; // eg: Camera is 110 mm in front of robot center
+ final int CAMERA_VERTICAL_DISPLACEMENT = 200; // eg: Camera is 200 mm above ground
+ final int CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line
+
+ OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix
+ .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)
+ .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES,
+ CAMERA_CHOICE == FRONT ? 90 : -90, 0, 0));
+
+ /** Let all the trackable listeners know where the phone is. */
+ for (VuforiaTrackable trackable : allTrackables)
+ {
+ ((VuforiaTrackableDefaultListener)trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection);
+ }
+
+ /** Wait for the game to begin */
+ telemetry.addData(">", "Press Play to start tracking");
+ telemetry.update();
+ waitForStart();
+
+ /** Start tracking the data sets we care about. */
+ targetsRoverRuckus.activate();
+ while (opModeIsActive()) {
+
+ // check all the trackable target to see which one (if any) is visible.
+ targetVisible = false;
+ for (VuforiaTrackable trackable : allTrackables) {
+ if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
+ telemetry.addData("Visible Target", trackable.getName());
+ targetVisible = true;
+
+ // getUpdatedRobotLocation() will return null if no new information is available since
+ // the last time that call was made, or if the trackable is not currently visible.
+ OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
+ if (robotLocationTransform != null) {
+ lastLocation = robotLocationTransform;
+ }
+ break;
+ }
+ }
+
+ // Provide feedback as to where the robot is located (if we know).
+ if (targetVisible) {
+ // express position (translation) of robot in inches.
+ VectorF translation = lastLocation.getTranslation();
+ telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f",
+ translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
+
+ // express the rotation of the robot in degrees.
+ Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
+ telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
+ }
+ else {
+ telemetry.addData("Visible Target", "none");
+ }
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigation.java
index 0fb2c42..81ab6b6 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigation.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigation.java
@@ -29,7 +29,7 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;
@@ -80,7 +80,7 @@
* is explained below.
*/
-@Autonomous(name="Concept: Vuforia Navigation", group ="Concept")
+@TeleOp(name="Concept: Vuforia Navigation", group ="Concept")
@Disabled
public class ConceptVuforiaNavigation extends LinearOpMode {
@@ -115,9 +115,9 @@ public class ConceptVuforiaNavigation extends LinearOpMode {
* random data. As an example, here is a example of a fragment of a valid key:
* ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
* Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code onthe next line, between the double quotes.
+ * and paste it in to your code on the next line, between the double quotes.
*/
- parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
+ parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
/*
* We also indicate which camera on the RC that we wish to use.
@@ -125,7 +125,11 @@ public class ConceptVuforiaNavigation extends LinearOpMode {
* for a competition robot, the front camera might be more convenient.
*/
parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
- this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
+
+ /**
+ * Instantiate the Vuforia engine
+ */
+ vuforia = ClassFactory.getInstance().createVuforia(parameters);
/**
* Load the data sets that for the trackable objects we wish to track. These particular data
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java
new file mode 100644
index 0000000..1b619f3
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaNavigationWebcam.java
@@ -0,0 +1,461 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.graphics.Bitmap;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.ThreadPool;
+import com.vuforia.Frame;
+
+import org.firstinspires.ftc.robotcore.external.ClassFactory;
+import org.firstinspires.ftc.robotcore.external.function.Consumer;
+import org.firstinspires.ftc.robotcore.external.function.Continuation;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.matrices.MatrixF;
+import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
+import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Locale;
+
+/**
+ * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine
+ * positioning and orientation of robot on the FTC field.
+ * The code is structured as a LinearOpMode
+ *
+ * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images.
+ *
+ * When images are located, Vuforia is able to determine the position and orientation of the
+ * image relative to the camera. This sample code than combines that information with a
+ * knowledge of where the target images are on the field, to determine the location of the camera.
+ *
+ * This example assumes a "diamond" field configuration where the red and blue alliance stations
+ * are adjacent on the corner of the field furthest from the audience.
+ * From the Audience perspective, the Red driver station is on the right.
+ * The two vision target are located on the two walls closest to the audience, facing in.
+ * The Stones are on the RED side of the field, and the Chips are on the Blue side.
+ *
+ * A final calculation then uses the location of the camera on the robot to determine the
+ * robot's location and orientation on the field.
+ *
+ * @see VuforiaLocalizer
+ * @see VuforiaTrackableDefaultListener
+ * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
+ *
+ * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
+ * is explained below.
+ */
+
+@TeleOp(name="Concept: Vuforia Nav Webcam", group ="Concept")
+@Disabled
+public class ConceptVuforiaNavigationWebcam extends LinearOpMode {
+
+ public static final String TAG = "Vuforia Navigation Sample";
+
+ OpenGLMatrix lastLocation = null;
+
+ /**
+ * @see #captureFrameToFile()
+ */
+ int captureCounter = 0;
+ File captureDirectory = AppUtil.ROBOT_DATA_DIR;
+
+ /**
+ * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
+ * localization engine.
+ */
+ VuforiaLocalizer vuforia;
+
+ /**
+ * This is the webcam we are to use. As with other hardware devices such as motors and
+ * servos, this device is identified using the robot configuration tool in the FTC application.
+ */
+ WebcamName webcamName;
+
+ @Override public void runOpMode() {
+
+ /*
+ * Retrieve the camera we are to use.
+ */
+ webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
+
+ /*
+ * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
+ * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
+ */
+ int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+ VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+ // OR... Do Not Activate the Camera Monitor View, to save power
+ // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
+
+ /*
+ * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
+ * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
+ * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
+ * web site at https://developer.vuforia.com/license-manager.
+ *
+ * Vuforia license keys are always 380 characters long, and look as if they contain mostly
+ * random data. As an example, here is a example of a fragment of a valid key:
+ * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
+ * Once you've obtained a license key, copy the string from the Vuforia web site
+ * and paste it in to your code on the next line, between the double quotes.
+ */
+ parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
+
+ /**
+ * We also indicate which camera on the RC we wish to use.
+ */
+ parameters.cameraName = webcamName;
+
+ /**
+ * Instantiate the Vuforia engine
+ */
+ vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+ /**
+ * Because this opmode processes frames in order to write them to a file, we tell Vuforia
+ * that we want to ensure that certain frame formats are available in the {@link Frame}s we
+ * see.
+ */
+ vuforia.enableConvertFrameToBitmap();
+
+ /** @see #captureFrameToFile() */
+ AppUtil.getInstance().ensureDirectoryExists(captureDirectory);
+
+
+ /**
+ * Load the data sets that for the trackable objects we wish to track. These particular data
+ * sets are stored in the 'assets' part of our application (you'll see them in the Android
+ * Studio 'Project' view over there on the left of the screen). You can make your own datasets
+ * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the
+ * example "StonesAndChips", datasets can be found in in this project in the
+ * documentation directory.
+ */
+ VuforiaTrackables stonesAndChips = vuforia.loadTrackablesFromAsset("StonesAndChips");
+ VuforiaTrackable redTarget = stonesAndChips.get(0);
+ redTarget.setName("RedTarget"); // Stones
+
+ VuforiaTrackable blueTarget = stonesAndChips.get(1);
+ blueTarget.setName("BlueTarget"); // Chips
+
+ /** For convenience, gather together all the trackable objects in one easily-iterable collection */
+ List allTrackables = new ArrayList();
+ allTrackables.addAll(stonesAndChips);
+
+ /**
+ * We use units of mm here because that's the recommended units of measurement for the
+ * size values specified in the XML for the ImageTarget trackables in data sets. E.g.:
+ *
+ * You don't *have to* use mm here, but the units here and the units used in the XML
+ * target configuration files *must* correspond for the math to work out correctly.
+ */
+ float mmPerInch = 25.4f;
+ float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot
+ float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels
+
+ /**
+ * In order for localization to work, we need to tell the system where each target we
+ * wish to use for navigation resides on the field, and we need to specify where on the robot
+ * the camera resides. These specifications are in the form of transformation matrices.
+ * Transformation matrices are a central, important concept in the math here involved in localization.
+ * See Transformation Matrix
+ * for detailed information. Commonly, you'll encounter transformation matrices as instances
+ * of the {@link OpenGLMatrix} class.
+ *
+ * For the most part, you don't need to understand the details of the math of how transformation
+ * matrices work inside (as fascinating as that is, truly). Just remember these key points:
+ *
+ *
+ *
You can put two transformations together to produce a third that combines the effect of
+ * both of them. If, for example, you have a rotation transform R and a translation transform T,
+ * then the combined transformation matrix RT which does the rotation first and then the translation
+ * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the
+ * reverse of the chronological order in which they applied.
+ *
+ *
A common way to create useful transforms is to use methods in the {@link OpenGLMatrix}
+ * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float,
+ * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and
+ * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}.
+ * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit,
+ * float, float, float, float)}, are syntactic shorthands for creating a new transform and
+ * then immediately multiplying the receiver by it, which can be convenient at times.
+ *
+ *
If you want to break open the black box of a transformation matrix to understand
+ * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the
+ * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF,
+ * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform
+ * will impart. See {@link #format(OpenGLMatrix)} below for an example.
+ *
+ *
+ *
+ * This example places the "stones" image on the perimeter wall to the Left
+ * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q
+ *
+ * This example places the "chips" image on the perimeter wall to the Right
+ * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q
+ *
+ * See the doc folder of this project for a description of the Field Coordinate System
+ * conventions.
+ *
+ * Initially the target is conceptually lying at the origin of the Field Coordinate System
+ * (the center of the field), facing up.
+ *
+ * In this configuration, the target's coordinate system aligns with that of the field.
+ *
+ * In a real situation we'd also account for the vertical (Z) offset of the target,
+ * but for simplicity, we ignore that here; for a real robot, you'll want to fix that.
+ *
+ * To place the Stones Target on the Red Audience wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright
+ * - Then we rotate it 90 around the field's Z access to face it away from the audience.
+ * - Finally, we translate it back along the X axis towards the red audience wall.
+ */
+ OpenGLMatrix redTargetLocationOnField = OpenGLMatrix
+ /* Then we translate the target off to the RED WALL. Our translation here
+ is a negative translation in X.*/
+ .translation(-mmFTCFieldWidth/2, 0, 0)
+ .multiplied(Orientation.getRotationMatrix(
+ /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */
+ AxesReference.EXTRINSIC, AxesOrder.XZX,
+ AngleUnit.DEGREES, 90, 90, 0));
+ redTarget.setLocationFtcFieldFromTarget(redTargetLocationOnField);
+ RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField));
+
+ /*
+ * To place the Stones Target on the Blue Audience wall:
+ * - First we rotate it 90 around the field's X axis to flip it upright
+ * - Finally, we translate it along the Y axis towards the blue audience wall.
+ */
+ OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix
+ /* Then we translate the target off to the Blue Audience wall.
+ Our translation here is a positive translation in Y.*/
+ .translation(0, mmFTCFieldWidth/2, 0)
+ .multiplied(Orientation.getRotationMatrix(
+ /* First, in the fixed (field) coordinate system, we rotate 90deg in X */
+ AxesReference.EXTRINSIC, AxesOrder.XZX,
+ AngleUnit.DEGREES, 90, 0, 0));
+ blueTarget.setLocationFtcFieldFromTarget(blueTargetLocationOnField);
+ RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField));
+
+ /**
+ * We also need to tell Vuforia where the cameras are relative to the robot.
+ *
+ * Just as there is a Field Coordinate System, so too there is a Robot Coordinate System.
+ * The two share many similarities. The origin of the Robot Coordinate System is wherever
+ * you choose to make it on the robot, but typically you'd choose somewhere in the middle
+ * of the robot. From that origin, the Y axis is horizontal and positive out towards the
+ * "front" of the robot (however you choose "front" to be defined), the X axis is horizontal
+ * and positive out towards the "right" of the robot (i.e.: 90deg horizontally clockwise from
+ * the positive Y axis), and the Z axis is vertical towards the sky.
+ *
+ * Similarly, for each camera there is a Camera Coordinate System. The origin of a Camera
+ * Coordinate System lies in the middle of the sensor inside of the camera. The Z axis is
+ * positive coming out of the lens of the camera in a direction perpendicular to the plane
+ * of the sensor. When looking at the face of the lens of the camera (down the positive Z
+ * axis), the X axis is positive off to the right in the plane of the sensor, and the Y axis
+ * is positive out the top of the lens in the plane of the sensor at 90 horizontally
+ * counter clockwise from the X axis.
+ *
+ * Next, there is Phone Coordinate System (for robots that have phones, of course), though
+ * with the advent of Vuforia support for Webcams, this coordinate system is less significant
+ * than it was previously. The Phone Coordinate System is defined thusly: with the phone in
+ * flat front of you in portrait mode (i.e. as it is when running the robot controller app)
+ * and you are staring straight at the face of the phone,
+ * * X is positive heading off to your right,
+ * * Y is positive heading up through the top edge of the phone, and
+ * * Z is pointing out of the screen, toward you.
+ * The origin of the Phone Coordinate System is at the origin of the Camera Coordinate System
+ * of the front-facing camera on the phone.
+ *
+ * Finally, it is worth noting that trackable Vuforia Image Targets have their own
+ * coordinate system (see {@link VuforiaTrackable}. This is sometimes referred to as the
+ * Target Coordinate System. In keeping with the above, when looking at the target in its
+ * natural orientation, in the Target Coodinate System
+ * * X is positive heading off to your right,
+ * * Y is positive heading up through the top edge of the target, and
+ * * Z is pointing out of the target, toward you.
+ *
+ * One can observe that the Camera Coordinate System of the front-facing camera on a phone
+ * coincides with the Phone Coordinate System. Further, when a phone is placed on its back
+ * at the origin of the Robot Coordinate System and aligned appropriately, those coordinate
+ * systems also coincide with the Robot Coordinate System. Got it?
+ *
+ * In this example here, we're going to assume that we put the camera on the right side
+ * of the robot (facing outwards, of course). To determine the transformation matrix that
+ * describes that location, first consider the camera as lying on its back at the origin
+ * of the Robot Coordinate System such that the Camera Coordinate System and Robot Coordinate
+ * System coincide. Then the transformation we need is
+ * * first a rotation of the camera by +90deg along the robot X axis,
+ * * then a rotation of the camera by +90deg along the robot Z axis, and
+ * * finally a translation of the camera to the side of the robot.
+ *
+ * When determining whether a rotation is positive or negative, consider yourself as looking
+ * down the (positive) axis of rotation from the positive towards the origin. Positive rotations
+ * are then CCW, and negative rotations CW. An example: consider looking down the positive Z
+ * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y
+ * plane) is then CCW, as one would normally expect from the usual classic 2D geometry.
+ */
+
+ OpenGLMatrix robotFromCamera = OpenGLMatrix
+ .translation(mmBotWidth/2,0,0)
+ .multiplied(Orientation.getRotationMatrix(
+ AxesReference.EXTRINSIC, AxesOrder.XZY,
+ AngleUnit.DEGREES, 90, 90, 0));
+ RobotLog.ii(TAG, "camera=%s", format(robotFromCamera));
+
+ /**
+ * Let the trackable listeners we care about know where the camera is. We know that each
+ * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because
+ * we have not ourselves installed a listener of a different type.
+ */
+ ((VuforiaTrackableDefaultListener)redTarget.getListener()).setCameraLocationOnRobot(parameters.cameraName, robotFromCamera);
+ ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setCameraLocationOnRobot(parameters.cameraName, robotFromCamera);
+
+ /**
+ * A brief tutorial: here's how all the math is going to work:
+ *
+ * C = robotFromCamera maps camera coords -> robot coords
+ * P = tracker.getPose() maps image target coords -> camera coords
+ * L = redTargetLocationOnField maps image target coords -> field coords
+ *
+ * So
+ *
+ * C.inverted() maps robot coords -> camera coords
+ * P.inverted() maps camera coords -> imageTarget coords
+ *
+ * Putting that all together,
+ *
+ * L x P.inverted() x C.inverted() maps robot coords to field coords.
+ *
+ * @see VuforiaTrackableDefaultListener#getRobotLocation()
+ */
+
+ /** Wait for the game to begin */
+ telemetry.addData(">", "Press Play to start tracking");
+ telemetry.update();
+ waitForStart();
+
+ /** Start tracking the data sets we care about. */
+ stonesAndChips.activate();
+
+ boolean buttonPressed = false;
+ while (opModeIsActive()) {
+
+ if (gamepad1.a && !buttonPressed) {
+ captureFrameToFile();
+ }
+ buttonPressed = gamepad1.a;
+
+ for (VuforiaTrackable trackable : allTrackables) {
+ /**
+ * getUpdatedRobotLocation() will return null if no new information is available since
+ * the last time that call was made, or if the trackable is not currently visible.
+ * getRobotLocation() will return null if the trackable is not currently visible.
+ */
+ telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); //
+
+ OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
+ if (robotLocationTransform != null) {
+ lastLocation = robotLocationTransform;
+ }
+ }
+ /**
+ * Provide feedback as to where the robot was last located (if we know).
+ */
+ if (lastLocation != null) {
+ // RobotLog.vv(TAG, "robot=%s", format(lastLocation));
+ telemetry.addData("Pos", format(lastLocation));
+ } else {
+ telemetry.addData("Pos", "Unknown");
+ }
+ telemetry.update();
+ }
+ }
+
+ /**
+ * A simple utility that extracts positioning information from a transformation matrix
+ * and formats it in a form palatable to a human being.
+ */
+ String format(OpenGLMatrix transformationMatrix) {
+ return transformationMatrix.formatAsTransform();
+ }
+
+ /**
+ * Sample one frame from the Vuforia stream and write it to a .PNG image file on the robot
+ * controller in the /sdcard/FIRST/data directory. The images can be downloaded using Android
+ * Studio's Device File Explorer, ADB, or the Media Transfer Protocol (MTP) integration into
+ * Windows Explorer, among other means. The images can be useful during robot design and calibration
+ * in order to get a sense of what the camera is actually seeing and so assist in camera
+ * aiming and alignment.
+ */
+ void captureFrameToFile() {
+ vuforia.getFrameOnce(Continuation.create(ThreadPool.getDefault(), new Consumer()
+ {
+ @Override public void accept(Frame frame)
+ {
+ Bitmap bitmap = vuforia.convertFrameToBitmap(frame);
+ if (bitmap != null) {
+ File file = new File(captureDirectory, String.format(Locale.getDefault(), "VuforiaFrame-%d.png", captureCounter++));
+ try {
+ FileOutputStream outputStream = new FileOutputStream(file);
+ try {
+ bitmap.compress(Bitmap.CompressFormat.PNG, 100, outputStream);
+ } finally {
+ outputStream.close();
+ telemetry.log().add("captured %s", file.getName());
+ }
+ } catch (IOException e) {
+ RobotLog.ee(TAG, e, "exception in captureFrameToFile()");
+ }
+ }
+ }
+ }));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java
new file mode 100644
index 0000000..337b703
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2018 Craig MacFarlane
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification, are permitted
+ * (subject to the limitations in the disclaimer below) provided that the following conditions are
+ * met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list of conditions
+ * and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions
+ * and the following disclaimer in the documentation and/or other materials provided with the
+ * distribution.
+ *
+ * Neither the name of Craig MacFarlane nor the names of its contributors may be used to
+ * endorse or promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS
+ * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * Display patterns of a REV Robotics Blinkin LED Driver.
+ * AUTO mode cycles through all of the patterns.
+ * MANUAL mode allows the user to manually change patterns using the
+ * left and right bumpers of a gamepad.
+ *
+ * Configure the driver on a servo port, and name it "blinkin".
+ *
+ * Displays the first pattern upon init.
+ */
+@TeleOp(name="BlinkinExample")
+@Disabled
+public class SampleRevBlinkinLedDriver extends OpMode {
+
+ /*
+ * Change the pattern every 10 seconds in AUTO mode.
+ */
+ private final static int LED_PERIOD = 10;
+
+ /*
+ * Rate limit gamepad button presses to every 500ms.
+ */
+ private final static int GAMEPAD_LOCKOUT = 500;
+
+ RevBlinkinLedDriver blinkinLedDriver;
+ RevBlinkinLedDriver.BlinkinPattern pattern;
+
+ Telemetry.Item patternName;
+ Telemetry.Item display;
+ DisplayKind displayKind;
+ Deadline ledCycleDeadline;
+ Deadline gamepadRateLimit;
+
+ protected enum DisplayKind {
+ MANUAL,
+ AUTO
+ }
+
+ @Override
+ public void init()
+ {
+ displayKind = DisplayKind.AUTO;
+
+ blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "blinkin");
+ pattern = RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE;
+ blinkinLedDriver.setPattern(pattern);
+
+ display = telemetry.addData("Display Kind: ", displayKind.toString());
+ patternName = telemetry.addData("Pattern: ", pattern.toString());
+
+ ledCycleDeadline = new Deadline(LED_PERIOD, TimeUnit.SECONDS);
+ gamepadRateLimit = new Deadline(GAMEPAD_LOCKOUT, TimeUnit.MILLISECONDS);
+ }
+
+ @Override
+ public void loop()
+ {
+ handleGamepad();
+
+ if (displayKind == DisplayKind.AUTO) {
+ doAutoDisplay();
+ } else {
+ /*
+ * MANUAL mode: Nothing to do, setting the pattern as a result of a gamepad event.
+ */
+ }
+ }
+
+ /*
+ * handleGamepad
+ *
+ * Responds to a gamepad button press. Demonstrates rate limiting for
+ * button presses. If loop() is called every 10ms and and you don't rate
+ * limit, then any given button press may register as multiple button presses,
+ * which in this application is problematic.
+ *
+ * A: Manual mode, Right bumper displays the next pattern, left bumper displays the previous pattern.
+ * B: Auto mode, pattern cycles, changing every LED_PERIOD seconds.
+ */
+ protected void handleGamepad()
+ {
+ if (!gamepadRateLimit.hasExpired()) {
+ return;
+ }
+
+ if (gamepad1.a) {
+ setDisplayKind(DisplayKind.MANUAL);
+ gamepadRateLimit.reset();
+ } else if (gamepad1.b) {
+ setDisplayKind(DisplayKind.AUTO);
+ gamepadRateLimit.reset();
+ } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.left_bumper)) {
+ pattern = pattern.previous();
+ displayPattern();
+ gamepadRateLimit.reset();
+ } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.right_bumper)) {
+ pattern = pattern.next();
+ displayPattern();
+ gamepadRateLimit.reset();
+ }
+ }
+
+ protected void setDisplayKind(DisplayKind displayKind)
+ {
+ this.displayKind = displayKind;
+ display.setValue(displayKind.toString());
+ }
+
+ protected void doAutoDisplay()
+ {
+ if (ledCycleDeadline.hasExpired()) {
+ pattern = pattern.next();
+ displayPattern();
+ ledCycleDeadline.reset();
+ }
+ }
+
+ protected void displayPattern()
+ {
+ blinkinLedDriver.setPattern(pattern);
+ patternName.setValue(pattern.toString());
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java
index f3f5276..6a94e25 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java
@@ -33,9 +33,9 @@
import android.graphics.Color;
import android.view.View;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
import com.qualcomm.robotcore.hardware.DigitalChannel;
@@ -67,7 +67,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Sensor: AdafruitRGB", group = "Sensor")
+@TeleOp(name = "Sensor: AdafruitRGB", group = "Sensor")
@Disabled // Comment this out to add to the opmode list
public class SensorAdafruitRGB extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
index b52d658..9102a10 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
@@ -31,9 +31,9 @@
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
@@ -54,7 +54,7 @@
*
* @see Adafruit IMU
*/
-@Autonomous(name = "Sensor: BNO055 IMU", group = "Sensor")
+@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
@Disabled // Comment this out to add to the opmode list
public class SensorBNO055IMU extends LinearOpMode
{
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
index 8da2afb..6bb6480 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
@@ -29,9 +29,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DigitalChannel;
/*
@@ -43,7 +43,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
*/
-@Autonomous(name = "Sensor: Digital touch", group = "Sensor")
+@TeleOp(name = "Sensor: Digital touch", group = "Sensor")
@Disabled
public class SensorDigitalTouch extends LinearOpMode {
/**
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTColor.java
index e0de829..e872273 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTColor.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTColor.java
@@ -33,9 +33,9 @@
import android.graphics.Color;
import android.view.View;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.ColorSensor;
/*
@@ -49,7 +49,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Sensor: HT color", group = "Sensor")
+@TeleOp(name = "Sensor: HT color", group = "Sensor")
@Disabled
public class SensorHTColor extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTGyro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTGyro.java
index d4ad94c..0e853ca 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTGyro.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHTGyro.java
@@ -30,9 +30,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.hitechnic.HiTechnicNxtGyroSensor;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gyroscope;
import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
@@ -45,7 +45,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
*/
-@Autonomous(name = "Sensor: HT Gyro", group = "Sensor")
+@TeleOp(name = "Sensor: HT Gyro", group = "Sensor")
@Disabled
public class SensorHTGyro extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
index 40e4a94..583f525 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
@@ -30,9 +30,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gyroscope;
import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -50,7 +50,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
-@Autonomous(name = "Sensor: KL navX Micro", group = "Sensor")
+@TeleOp(name = "Sensor: KL navX Micro", group = "Sensor")
@Disabled
public class SensorKLNavxMicro extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
index 90ab704..583ac1a 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
@@ -30,9 +30,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cCompassSensor;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CompassSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -49,7 +49,7 @@
*
* @see MR Compass Sensor
*/
-@Autonomous(name = "Sensor: MR compass", group = "Sensor")
+@TeleOp(name = "Sensor: MR compass", group = "Sensor")
@Disabled // comment out or remove this line to enable this opmode
public class SensorMRCompass extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
index 5acbd10..d7de1ff 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
@@ -30,9 +30,9 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -47,7 +47,7 @@
*
* @see MR Range Sensor
*/
-@Autonomous(name = "Sensor: MR range sensor", group = "Sensor")
+@TeleOp(name = "Sensor: MR range sensor", group = "Sensor")
@Disabled // comment out or remove this line to enable this opmode
public class SensorMRRangeSensor extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
new file mode 100644
index 0000000..880bd84
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
@@ -0,0 +1,89 @@
+/*
+Copyright (c) 2018 FIRST
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of FIRST nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * {@link SensorREV2mDistance} illustrates how to use the REV Robotics
+ * Time-of-Flight Range Sensor.
+ *
+ * The op mode assumes that the range sensor is configured with a name of "sensor_range".
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
+ *
+ * @see REV Robotics Web Page
+ */
+@TeleOp(name = "Sensor: REV2mDistance", group = "Sensor")
+@Disabled
+public class SensorREV2mDistance extends LinearOpMode {
+
+ private DistanceSensor sensorRange;
+
+ @Override
+ public void runOpMode() {
+ // you can use this as a regular DistanceSensor.
+ sensorRange = hardwareMap.get(DistanceSensor.class, "sensor_range");
+
+ // you can also cast this to a Rev2mDistanceSensor if you want to use added
+ // methods associated with the Rev2mDistanceSensor class.
+ Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor)sensorRange;
+
+ telemetry.addData(">>", "Press start to continue");
+ telemetry.update();
+
+ waitForStart();
+ while(opModeIsActive()) {
+ // generic DistanceSensor methods.
+ telemetry.addData("deviceName",sensorRange.getDeviceName() );
+ telemetry.addData("range", String.format("%.01f mm", sensorRange.getDistance(DistanceUnit.MM)));
+ telemetry.addData("range", String.format("%.01f cm", sensorRange.getDistance(DistanceUnit.CM)));
+ telemetry.addData("range", String.format("%.01f m", sensorRange.getDistance(DistanceUnit.METER)));
+ telemetry.addData("range", String.format("%.01f in", sensorRange.getDistance(DistanceUnit.INCH)));
+
+ // Rev2mDistanceSensor specific methods.
+ telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID()));
+ telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur()));
+
+ telemetry.update();
+ }
+ }
+
+}
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREVColorDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREVColorDistance.java
index 11a8880..37910ca 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREVColorDistance.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREVColorDistance.java
@@ -33,9 +33,9 @@
import android.graphics.Color;
import android.view.View;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
@@ -52,7 +52,7 @@
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
*/
-@Autonomous(name = "Sensor: REVColorDistance", group = "Sensor")
+@TeleOp(name = "Sensor: REVColorDistance", group = "Sensor")
@Disabled // Comment this out to add to the opmode list
public class SensorREVColorDistance extends LinearOpMode {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
index 4ec32fa..d5d6872 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
@@ -43,10 +43,13 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
import android.hardware.usb.UsbDevice;
import android.hardware.usb.UsbManager;
import android.net.wifi.WifiManager;
+import android.os.Build;
import android.os.Bundle;
import android.os.IBinder;
import android.preference.PreferenceManager;
import android.support.annotation.NonNull;
+import android.support.annotation.Nullable;
+import android.support.annotation.StringRes;
import android.view.Menu;
import android.view.MenuItem;
import android.view.MotionEvent;
@@ -54,6 +57,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
import android.webkit.WebView;
import android.widget.ImageButton;
import android.widget.LinearLayout;
+import android.widget.PopupMenu;
import android.widget.TextView;
import com.google.blocks.ftcrobotcontroller.BlocksActivity;
@@ -61,8 +65,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
import com.google.blocks.ftcrobotcontroller.ProgrammingModeControllerImpl;
import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers;
import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode;
-import com.qualcomm.ftccommon.AboutActivity;
import com.qualcomm.ftccommon.ClassManagerFactory;
+import com.qualcomm.ftccommon.FtcAboutActivity;
import com.qualcomm.ftccommon.FtcEventLoop;
import com.qualcomm.ftccommon.FtcEventLoopIdle;
import com.qualcomm.ftccommon.FtcRobotControllerService;
@@ -84,20 +88,26 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
import com.qualcomm.robotcore.hardware.configuration.LynxConstants;
import com.qualcomm.robotcore.hardware.configuration.Utility;
+import com.qualcomm.robotcore.util.Device;
import com.qualcomm.robotcore.util.Dimmer;
import com.qualcomm.robotcore.util.ImmersiveMode;
import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.wifi.NetworkConnection;
import com.qualcomm.robotcore.wifi.NetworkConnectionFactory;
import com.qualcomm.robotcore.wifi.NetworkType;
-import com.qualcomm.robotcore.wifi.WifiDirectAssistant;
import org.firstinspires.ftc.ftccommon.external.SoundPlayingRobotMonitor;
import org.firstinspires.ftc.ftccommon.internal.FtcRobotControllerWatchdogService;
import org.firstinspires.ftc.ftccommon.internal.ProgramAndManageActivity;
+import org.firstinspires.ftc.robotcore.external.navigation.MotionDetection;
import org.firstinspires.ftc.robotcore.internal.hardware.DragonboardLynxDragonboardIsPresentPin;
import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManager;
+import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManagerFactory;
+import org.firstinspires.ftc.robotcore.internal.network.WifiDirectDeviceNameManager;
import org.firstinspires.ftc.robotcore.internal.network.PreferenceRemoterRC;
import org.firstinspires.ftc.robotcore.internal.network.StartResult;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteEvent;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteStateMachine;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.firstinspires.ftc.robotcore.internal.system.Assert;
import org.firstinspires.ftc.robotcore.internal.system.PreferencesHelper;
@@ -130,8 +140,8 @@ public class FtcRobotControllerActivity extends Activity
protected UpdateUI.Callback callback;
protected Context context;
protected Utility utility;
- protected StartResult deviceNameManagerStartResult = new StartResult();
protected StartResult prefRemoterStartResult = new StartResult();
+ protected StartResult deviceNameStartResult = new StartResult();
protected PreferencesHelper preferencesHelper;
protected final SharedPreferencesListener sharedPreferencesListener = new SharedPreferencesListener();
@@ -154,6 +164,9 @@ public class FtcRobotControllerActivity extends Activity
protected FtcEventLoop eventLoop;
protected Queue receivedUsbAttachmentNotifications;
+ protected WifiMuteStateMachine wifiMuteStateMachine;
+ protected MotionDetection motionDetection;
+
protected class RobotRestarter implements Restarter {
public void requestRestart() {
@@ -220,6 +233,8 @@ protected void onCreate(Bundle savedInstanceState) {
RobotLog.vv(TAG, "onCreate()");
ThemedActivity.appAppThemeToActivity(getTag(), this); // do this way instead of inherit to help AppInventor
+ // Oddly, sometimes after a crash & restart the root activity will be something unexpected, like from the before crash? We don't yet understand
+ RobotLog.vv(TAG, "rootActivity is of class %s", AppUtil.getInstance().getRootActivity().getClass().getSimpleName());
Assert.assertTrue(FtcRobotControllerWatchdogService.isFtcRobotControllerActivity(AppUtil.getInstance().getRootActivity()));
Assert.assertTrue(AppUtil.getInstance().isRobotController());
@@ -240,7 +255,9 @@ protected void onCreate(Bundle savedInstanceState) {
context = this;
utility = new Utility(this);
- DeviceNameManager.getInstance().start(deviceNameManagerStartResult);
+
+ DeviceNameManagerFactory.getInstance().start(deviceNameStartResult);
+
PreferenceRemoterRC.getInstance().start(prefRemoterStartResult);
receivedUsbAttachmentNotifications = new ConcurrentLinkedQueue();
@@ -257,7 +274,15 @@ protected void onCreate(Bundle savedInstanceState) {
buttonMenu.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View v) {
- AppUtil.getInstance().openOptionsMenuFor(FtcRobotControllerActivity.this);
+ PopupMenu popupMenu = new PopupMenu(FtcRobotControllerActivity.this, v);
+ popupMenu.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() {
+ @Override
+ public boolean onMenuItemClick(MenuItem item) {
+ return onOptionsItemSelected(item); // Delegate to the handler for the hardware menu button
+ }
+ });
+ popupMenu.inflate(R.menu.ftc_robot_controller);
+ popupMenu.show();
}
});
@@ -300,11 +325,17 @@ public void onClick(View v) {
hittingMenuButtonBrightensScreen();
wifiLock.acquire();
- callback.networkConnectionUpdate(WifiDirectAssistant.Event.DISCONNECTED);
+ callback.networkConnectionUpdate(NetworkConnection.NetworkEvent.DISCONNECTED);
readNetworkType();
ServiceController.startService(FtcRobotControllerWatchdogService.class);
bindToService();
logPackageVersions();
+ logDeviceSerialNumber();
+ RobotLog.logDeviceInfo();
+
+ if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ }
}
protected UpdateUI createUpdateUI() {
@@ -374,8 +405,8 @@ protected void onDestroy() {
shutdownRobot(); // Ensure the robot is put away to bed
if (callback != null) callback.close();
- PreferenceRemoterRC.getInstance().start(prefRemoterStartResult);
- DeviceNameManager.getInstance().stop(deviceNameManagerStartResult);
+ PreferenceRemoterRC.getInstance().stop(prefRemoterStartResult);
+ DeviceNameManagerFactory.getInstance().stop(deviceNameStartResult);
unbindFromService();
// If the app manually (?) is stopped, then we don't need the auto-starting function (?)
@@ -408,6 +439,10 @@ protected void logPackageVersions() {
RobotLog.logBuildConfig(org.firstinspires.inspection.BuildConfig.class);
}
+ protected void logDeviceSerialNumber() {
+ RobotLog.ii(TAG, "410c serial number: " + Build.SERIAL);
+ }
+
protected void readNetworkType() {
// The code here used to defer to the value found in a configuration file
@@ -418,10 +453,17 @@ protected void readNetworkType() {
// Moreover, the non-Wifi-Direct networking is end-of-life, so the simplest and most robust
// (e.g.: no one can screw things up by messing with the contents of the config file) fix is
// to do away with configuration file entirely.
- networkType = NetworkType.WIFIDIRECT;
+ //
+ // Control hubs are always running the access point model. Everything else, for the time
+ // being always runs the wifi direct model.
+ if (Device.isRevControlHub() == true) {
+ networkType = NetworkType.RCWIRELESSAP;
+ } else {
+ networkType = NetworkType.fromString(preferencesHelper.readString(context.getString(R.string.pref_pairing_kind), NetworkType.globalDefaultAsString()));
+ }
// update the app_settings
- preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_network_connection_type), networkType.toString());
+ preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_pairing_kind), networkType.toString());
}
@Override
@@ -498,8 +540,7 @@ else if (id == R.id.action_settings) {
return true;
}
else if (id == R.id.action_about) {
- Intent intent = new Intent(AppUtil.getDefContext(), AboutActivity.class);
- intent.putExtra(LaunchActivityConstantsList.ABOUT_ACTIVITY_CONNECTION_TYPE, networkType);
+ Intent intent = new Intent(AppUtil.getDefContext(), FtcAboutActivity.class);
startActivity(intent);
return true;
}
@@ -555,14 +596,20 @@ private void updateUIAndRequestRobotSetup() {
if (controllerService != null) {
callback.networkConnectionUpdate(controllerService.getNetworkConnectionStatus());
callback.updateRobotStatus(controllerService.getRobotStatus());
- requestRobotSetup();
+ // Only show this first-time toast on headless systems: what we have now on non-headless suffices
+ requestRobotSetup(LynxConstants.isRevControlHub()
+ ? new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRobotSetupComplete);
+ }
+ }
+ : null);
}
}
- private void requestRobotSetup() {
+ private void requestRobotSetup(@Nullable Runnable runOnComplete) {
if (controllerService == null) return;
- HardwareFactory factory;
RobotConfigFile file = cfgFileMgr.getActiveConfigAndUpdateUI();
HardwareFactory hardwareFactory = new HardwareFactory(context);
try {
@@ -572,14 +619,13 @@ private void requestRobotSetup() {
hardwareFactory.setXmlPullParser(file.getXml());
cfgFileMgr.setActiveConfigAndUpdateUI(false, file);
}
- factory = hardwareFactory;
OpModeRegister userOpModeRegister = createOpModeRegister();
- eventLoop = new FtcEventLoop(factory, userOpModeRegister, callback, this, programmingModeController);
- FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(factory, userOpModeRegister, callback, this, programmingModeController);
+ eventLoop = new FtcEventLoop(hardwareFactory, userOpModeRegister, callback, this, programmingModeController);
+ FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(hardwareFactory, userOpModeRegister, callback, this, programmingModeController);
controllerService.setCallback(callback);
- controllerService.setupRobot(eventLoop, idleLoop);
+ controllerService.setupRobot(eventLoop, idleLoop, runOnComplete);
passReceivedUsbAttachmentsToEventLoop();
}
@@ -595,10 +641,18 @@ private void shutdownRobot() {
private void requestRobotRestart() {
AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartingRobot));
//
+ RobotLog.clearGlobalErrorMsg();
+ RobotLog.clearGlobalWarningMsg();
shutdownRobot();
- requestRobotSetup();
- //
- AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartRobotComplete));
+ requestRobotSetup(new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRestartRobotComplete);
+ }
+ });
+ }
+
+ private void showRestartRobotCompleteToast(@StringRes int resid) {
+ AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(resid));
}
protected void hittingMenuButtonBrightensScreen() {
@@ -619,7 +673,44 @@ protected class SharedPreferencesListener implements SharedPreferences.OnSharedP
@Override public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
if (key.equals(context.getString(R.string.pref_app_theme))) {
ThemedActivity.restartForAppThemeChange(getTag(), getString(R.string.appThemeChangeRestartNotifyRC));
+ } else if (key.equals(context.getString(R.string.pref_wifi_automute))) {
+ if (preferencesHelper.readBoolean(context.getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ } else {
+ initWifiMute(false);
+ }
}
}
}
+
+ protected void initWifiMute(boolean enable) {
+ if (enable) {
+ wifiMuteStateMachine = new WifiMuteStateMachine();
+ wifiMuteStateMachine.initialize();
+ wifiMuteStateMachine.start();
+
+ motionDetection = new MotionDetection(2.0, 10);
+ motionDetection.startListening();
+ motionDetection.registerListener(new MotionDetection.MotionDetectionListener() {
+ @Override
+ public void onMotionDetected(double vector)
+ {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ });
+ } else {
+ wifiMuteStateMachine.stop();
+ wifiMuteStateMachine = null;
+ motionDetection.stopListening();
+ motionDetection.purgeListeners();
+ motionDetection = null;
+ }
+ }
+
+ @Override
+ public void onUserInteraction() {
+ if (wifiMuteStateMachine != null) {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ }
}
diff --git a/FtcRobotController/src/main/res/xml/app_settings.xml b/FtcRobotController/src/main/res/xml/app_settings.xml
index adc3223..58d3aa9 100644
--- a/FtcRobotController/src/main/res/xml/app_settings.xml
+++ b/FtcRobotController/src/main/res/xml/app_settings.xml
@@ -64,7 +64,7 @@ See https://developer.android.com/guide/topics/ui/settings.html
diff --git a/FtcRobotController/src/main/res/xml/device_filter.xml b/FtcRobotController/src/main/res/xml/device_filter.xml
index 6f72819..7677dad 100644
--- a/FtcRobotController/src/main/res/xml/device_filter.xml
+++ b/FtcRobotController/src/main/res/xml/device_filter.xml
@@ -31,8 +31,20 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
+
+
+
+
+
+
+
+
diff --git a/README.md b/README.md
index 544c7db..704c730 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,5 @@
+[](https://www.codacy.com/app/michael_47/ARC-Core?utm_source=github.com&utm_medium=referral&utm_content=Andover-Robotics/ARC-Core&utm_campaign=Badge_Grade)
+
## Welcome!
This is the common code base for the Andover Robotics teams. All three teams will fork from this repository to program their own robots.
## Downloading the Project
@@ -9,7 +11,8 @@ Ask Lake Yin, Michael Peng, or Justin Zhu for help with any coding problems.
### User Documentation and Tutorials
Each TeamCode written by each team should be documented with the standard specified [here](documentation.md).
-### Javadoc Reference Material
+### JavaDoc Reference Material
+Our JavaDocs are located [here](https://Andover-Robotics.github.io/ARC-Core).
### Online User Forum
-The programming Discord can be found here:
+The programming Discord can be found [here](https://discord.gg/mut9kTr).
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index 7b4fa18..95e271e 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -14,4 +14,8 @@ dependencies {
compile project(':DogeCV')
compile project(':openCVLibrary320')
compile project(':FtcRobotController')
+ compile project(':ARCCore')
+}
+android {
+ buildToolsVersion '27.0.3'
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumAutonomousExample.java b/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumAutonomousExample.java
new file mode 100644
index 0000000..2009860
--- /dev/null
+++ b/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumAutonomousExample.java
@@ -0,0 +1,37 @@
+package com.andoverrobotics.core.examples;
+
+import com.andoverrobotics.core.drivetrain.MecanumDrive;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+@Autonomous(name = "Mecanum Autonomous Example", group = "ARC")
+public class MecanumAutonomousExample extends LinearOpMode {
+
+ // Change these values if necessary
+ private static final int ticksPerInch = (int) (1120 / (4 * Math.PI)),
+ ticksPer360 = 4000;
+
+ @Override
+ public void runOpMode() {
+
+ DcMotor motorFL = hardwareMap.dcMotor.get("motorFL");
+ DcMotor motorFR = hardwareMap.dcMotor.get("motorFR");
+ DcMotor motorBL = hardwareMap.dcMotor.get("motorBL");
+ DcMotor motorBR = hardwareMap.dcMotor.get("motorBR");
+
+ motorFL.setDirection(Direction.REVERSE);
+ motorBL.setDirection(Direction.REVERSE);
+
+ MecanumDrive drivetrain = MecanumDrive.fromOctagonalMotors(
+ motorFL, motorFR, motorBL, motorBR, this, ticksPerInch, ticksPer360);
+
+ waitForStart();
+
+ drivetrain.strafeInches(-5, 10, 1);
+ drivetrain.strafeInches(5, 10, 1);
+ drivetrain.strafeInches(5, -10, 1);
+ drivetrain.strafeInches(-5, -10, 1);
+ }
+}
diff --git a/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumTeleOpExample.java b/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumTeleOpExample.java
new file mode 100644
index 0000000..fc1d5ee
--- /dev/null
+++ b/TeamCode/src/main/java/com/andoverrobotics/core/examples/MecanumTeleOpExample.java
@@ -0,0 +1,44 @@
+package com.andoverrobotics.core.examples;
+
+import com.andoverrobotics.core.drivetrain.MecanumDrive;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+@TeleOp(name = "Mecanum TeleOp Example", group = "ARC")
+public class MecanumTeleOpExample extends OpMode {
+
+ private static final int ticksPerInch = 20, ticksPer360 = 200;
+
+ private MecanumDrive mecanumDrive;
+
+ @Override
+ public void init() {
+ DcMotor motorFL = hardwareMap.dcMotor.get("motorFL");
+ DcMotor motorFR = hardwareMap.dcMotor.get("motorFR");
+ DcMotor motorBL = hardwareMap.dcMotor.get("motorBL");
+ DcMotor motorBR = hardwareMap.dcMotor.get("motorBR");
+
+ motorFL.setDirection(Direction.REVERSE);
+ motorBL.setDirection(Direction.REVERSE);
+
+ mecanumDrive = MecanumDrive.fromOctagonalMotors(
+ motorFL, motorFR, motorBL, motorBR, this, ticksPerInch, ticksPer360);
+ }
+
+ @Override
+ public void loop() {
+
+ if (Math.abs(gamepad1.right_stick_x) > 0.1) {
+ mecanumDrive.setRotationPower(gamepad1.right_stick_x);
+ } else {
+ mecanumDrive.setStrafe(gamepad1.left_stick_x, -gamepad1.left_stick_y, 1);
+ }
+
+ telemetry.addData("Left stick X", gamepad1.left_stick_x);
+ telemetry.addData("Left stick Y", -gamepad1.left_stick_y);
+ telemetry.addData("Right stick X", gamepad1.right_stick_x);
+ telemetry.update();
+ }
+}
diff --git a/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveAutonomousExample.java b/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveAutonomousExample.java
new file mode 100644
index 0000000..f7f8b5f
--- /dev/null
+++ b/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveAutonomousExample.java
@@ -0,0 +1,35 @@
+package com.andoverrobotics.core.examples;
+
+import com.andoverrobotics.core.drivetrain.TankDrive;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+@Autonomous(name = "TankDrive Autonomous Example", group = "ARC")
+public class TankDriveAutonomousExample extends LinearOpMode {
+
+ private static final int ticksPerInch = 20, ticksPer360 = 200;
+
+ @Override
+ public void runOpMode() {
+
+ DcMotor motorL = hardwareMap.dcMotor.get("motorL");
+ DcMotor motorR = hardwareMap.dcMotor.get("motorR");
+ motorL.setDirection(Direction.REVERSE);
+
+ TankDrive tankDrive = TankDrive.fromMotors(motorL, motorR, this, ticksPerInch, ticksPer360);
+
+ waitForStart();
+
+ for (int i = 0; i < 4; i++) {
+ tankDrive.driveForwards(36, 1);
+ telemetry.addLine("Traveled 36 inches");
+ telemetry.update();
+
+ tankDrive.rotateClockwise(90, 1);
+ telemetry.addLine("Turned 90 degrees clockwise");
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveTeleOpExample.java b/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveTeleOpExample.java
new file mode 100644
index 0000000..0f759c0
--- /dev/null
+++ b/TeamCode/src/main/java/com/andoverrobotics/core/examples/TankDriveTeleOpExample.java
@@ -0,0 +1,34 @@
+package com.andoverrobotics.core.examples;
+
+import com.andoverrobotics.core.drivetrain.TankDrive;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+@TeleOp(name = "TankDrive TeleOp Example", group = "ARC")
+public class TankDriveTeleOpExample extends OpMode {
+
+ private static final int ticksPerInch = 1000, ticksPer360 = 1440 * 4;
+ private TankDrive tankDrive;
+
+ @Override
+ public void init() {
+ DcMotor motorL = hardwareMap.dcMotor.get("motorL");
+ DcMotor motorR = hardwareMap.dcMotor.get("motorR");
+ motorL.setDirection(Direction.REVERSE);
+
+ tankDrive = TankDrive.fromMotors(
+ motorL, motorR, this, ticksPerInch, ticksPer360);
+ }
+
+ @Override
+ public void loop() {
+ // The Y axis for gamepad sticks is reversed, negate it in use
+ tankDrive.setMovementAndRotation(-gamepad1.left_stick_y, gamepad1.left_stick_x);
+
+ telemetry.addData("Left stick X", gamepad1.left_stick_x);
+ telemetry.addData("Left stick Y", -gamepad1.left_stick_y);
+ telemetry.update();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bottest/TankDriveTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bottest/TankDriveTest.java
new file mode 100644
index 0000000..e12cda7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bottest/TankDriveTest.java
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.bottest;
+
+import com.andoverrobotics.core.drivetrain.DriveTrain;
+import com.andoverrobotics.core.drivetrain.TankDrive;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+@TeleOp(name = "TankDriveTest_TeleOp", group = "ARC")
+public class TankDriveTest extends OpMode {
+
+ private DriveTrain drive;
+
+ @Override
+ public void init() {
+ DcMotor motorL = hardwareMap.dcMotor.get("motorL");
+ DcMotor motorR = hardwareMap.dcMotor.get("motorR");
+ motorR.setDirection(Direction.REVERSE);
+
+ drive = TankDrive.fromMotors(motorL, motorR, this, 1, 1);
+ }
+
+ @Override
+ public void loop() {
+ drive.setMovementAndRotation(-gamepad1.left_stick_y, gamepad1.left_stick_x);
+
+ telemetry.addData("Left stick y", -gamepad1.left_stick_y);
+ telemetry.addData("Left stick x", gamepad1.left_stick_x);
+ telemetry.update();
+ }
+}
diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
new file mode 100644
index 0000000..e2819d2
--- /dev/null
+++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
@@ -0,0 +1,149 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/build.gradle b/build.gradle
index a267db5..a8dcfb5 100644
--- a/build.gradle
+++ b/build.gradle
@@ -6,10 +6,22 @@
buildscript {
repositories {
jcenter()
+ maven {
+ url 'https://maven.google.com/'
+ name 'Google'
+ }
+ google()
}
dependencies {
- classpath 'com.android.tools.build:gradle:2.3.3'
+ classpath 'com.android.tools.build:gradle:3.1.3'
}
}
+
+repositories {
+ maven {
+ url 'https://maven.google.com/'
+ name 'Google'
+ }
+}
\ No newline at end of file
diff --git a/doc/javadoc/allclasses-frame.html b/doc/javadoc/allclasses-frame.html
index eb75ffd..adcebda 100644
--- a/doc/javadoc/allclasses-frame.html
+++ b/doc/javadoc/allclasses-frame.html
@@ -11,8 +11,6 @@
public <T> T getHardwareDevice(java.lang.Class<? extends T> classOrInterface,
+ SerialNumber serialNumber,
+ <any> usbScanManagerSupplier)
+
Returns the device whose serial number is the one indicated, from the hardware map if possible
+ but instantiating / opening it if necessary. null is returned if the object cannot be
+ accessed.
+
+
Parameters:
+
classOrInterface - the interface to retrieve on the returned object
+
serialNumber - the serial number of the object to retrieve
+
usbScanManagerSupplier - how to get a USBScanManager if it ends up we need one
Returns an array containing the constants of this enum type, in
+the order they are declared. This method may be used to iterate
+over the constants as follows:
+
+for (FtcLynxFirmwareUpdateActivity.FwResponseStatus c : FtcLynxFirmwareUpdateActivity.FwResponseStatus.values())
+ System.out.println(c);
+
+
+
Returns:
+
an array containing the constants of this enum type, in the order they are declared
Returns the enum constant of this type with the specified name.
+The string must match exactly an identifier used to declare an
+enum constant in this type. (Extraneous whitespace characters are
+not permitted.)
+
+
Parameters:
+
name - the name of the enum constant to be returned.
+
Returns:
+
the enum constant with the specified name
+
Throws:
+
java.lang.IllegalArgumentException - if this enum type has no constant with the specified name
+
java.lang.NullPointerException - if the argument is null
The intent extra name for JSON encoded org.firstinspires.ftc.robotcore.internal.webserver.RobotControllerWebInfo used when launching
RemoteProgrammingModeActivity on the driver station.
protected class SoundPlayer.LoadedSoundCache
-extends java.util.LinkedHashMap<java.lang.Integer,SoundPlayer.SoundInfo>
+extends java.lang.Object
SoundPlayer.LoadedSoundCache keeps track of loaded sounds, mapping sound resource id to loaded
sound id. It keeps track of which sounds have been recently used, and unloads neglected
songs when a configured capacity of loaded sounds has been reached.
Returns an array containing the constants of this enum type, in
+the order they are declared. This method may be used to iterate
+over the constants as follows:
+
+for (SoundPlayer.StopWhat c : SoundPlayer.StopWhat.values())
+ System.out.println(c);
+
+
+
Returns:
+
an array containing the constants of this enum type, in the order they are declared
Returns the enum constant of this type with the specified name.
+The string must match exactly an identifier used to declare an
+enum constant in this type. (Extraneous whitespace characters are
+not permitted.)
+
+
Parameters:
+
name - the name of the enum constant to be returned.
+
Returns:
+
the enum constant with the specified name
+
Throws:
+
java.lang.IllegalArgumentException - if this enum type has no constant with the specified name
+
java.lang.NullPointerException - if the argument is null
Asynchronously loads the indicated sound from its resource (if not already loaded), then
- initiates its play once any current sound is finished playing.
play(Context context,
int resId,
boolean waitForCompletion)
-
Asynchronously loads the indicated sound from its resource (if not already loaded), then
- initiates its play, optionally waiting for any currently playing sound to finish first.
+
Deprecated.
+
use #startPlaying(Context, int, PlaySoundParams, Consumer, Runnable) instead
Asynchronously loads the indicated sound from its resource (if not already loaded), then
+ initiates its play once any current non-looping sound is finished playing.
Asynchronously loads the indicated sound from its resource (if not already loaded), then
+ initiates its play, optionally waiting for any currently non-looping playing sounds to finish first.
public void startPlaying(Context context,
+ int resId)
+
Asynchronously loads the indicated sound from its resource (if not already loaded), then
+ initiates its play once any current non-looping sound is finished playing.
+
+
Parameters:
+
context - the context in which resId is to be interpreted
+
resId - the resource id of the raw resource containing the sound.
public void startPlaying(Context context,
+ int resId,
+ SoundPlayer.PlaySoundParams params,
+ <any> runWhenStarted,
+ java.lang.Runnable runWhenFinished)
Asynchronously loads the indicated sound from its resource (if not already loaded), then
- initiates its play once any current sound is finished playing.
+ initiates its play, optionally waiting for any currently non-looping playing sounds to finish first.
Parameters:
context - the context in which resId is to be interpreted
resId - the resource id of the raw resource containing the sound.
+
params - controls how the playback proceeds
+
runWhenStarted - executed when the stream starts to play
+
runWhenFinished - executed when the stream finishes playing
public void play(Context context,
- int resId,
- boolean waitForCompletion)
-
Asynchronously loads the indicated sound from its resource (if not already loaded), then
- initiates its play, optionally waiting for any currently playing sound to finish first.
protected void loadAndPlay(Context context,
- int resourceId,
- boolean waitForCompletion)
-
Loads the requested sound if necessary, then (eventually) plays it.
- Note: this always runs on our dedicate executor thread. We do that because
- that allows us to have a looper that *only* sees load completions, which will
- prevent us from accepting new play requests while we're waiting for loads to
- complete.