Add AutoOpMode base class

AutoOpMode.java

@@ -0,0 +1,48 @@
+package org.redshiftrobotics.lib;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.redshiftrobotics.lib.encoder.EncoderPositioner;
+import org.redshiftrobotics.lib.pid.FakePIDManager;
+import org.redshiftrobotics.lib.pid.PIDManager;
+
+import java.util.Date;
+
+abstract public class AutoOpMode extends OpMode {
+    static double SPEED = 50;
+    static Position TOLERANCE = new Position(DistanceUnit.CM, 0.5, 0.5, 0.5, 0);
+    static double TICKS_PER_CM = 1500;
+
+    DcMotor m0;
+    DcMotor m1;
+    DcMotor m2;
+    DcMotor m3;
+
+    PIDManager pid;
+    Positioner positioner;
+
+    @Override
+    public void init() {
+        m0 = hardwareMap.dcMotor.get("m0");
+        m1 = hardwareMap.dcMotor.get("m1");
+        m2 = hardwareMap.dcMotor.get("m2");
+        m3 = hardwareMap.dcMotor.get("m3");
+
+        pid = new FakePIDManager(m0, m1, m2, m3);
+        positioner = new EncoderPositioner(new DoubleDcMotorEncoder(m0, m1), new DoubleDcMotorEncoder(m2, m3), TICKS_PER_CM);
+
+    }
+
+    private Position diff(Position a, Position b) {
+        b = b.toUnit(a.unit);
+        return new Position(a.unit, a.x - b.x, a.y - b.y, a.z - b.z, (new Date()).getTime());
+    }
+
+    private void driveToPosition(Position pos) {
+        pid.move(diff(positioner.getPosition(), pos), SPEED, TOLERANCE);
+    }
+
+}

DoubleDcMotorEncoder.java

@@ -0,0 +1,31 @@
+package org.redshiftrobotics.lib;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.redshiftrobotics.lib.encoder.Encoder;
+
+/**
+ * Created by ariporad on 2017-10-06.
+ */
+public class DoubleDcMotorEncoder implements Encoder {
+    private DcMotor m1;
+    private DcMotor m2;
+
+    public DoubleDcMotorEncoder(DcMotor m1, DcMotor m2) {
+        this.m1 = m1;
+        this.m2 = m2;
+        m1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        m2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        m1.setPower(0);
+        m2.setPower(0);
+    }
+
+    // Because Java is a shitty language
+    private double average(double a, double b) {
+        return (a + b) / 2;
+    }
+
+    @Override
+    public double getValue() { return average(m1.getCurrentPosition(), m2.getCurrentPosition()); }
+}

Positioner.java

@@ -0,0 +1,8 @@
+package org.redshiftrobotics.lib;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+
+public interface Positioner {
+    Position getPosition();
+    void reset();
+}

encoder/DcMotorEncoder.java

@@ -0,0 +1,19 @@
+package org.redshiftrobotics.lib.encoder;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+/**
+ * Created by ariporad on 2017-10-06.
+ */
+public class DcMotorEncoder implements Encoder {
+    private DcMotor motor;
+
+    public DcMotorEncoder(DcMotor motor) {
+        this.motor = motor;
+        motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        motor.setPower(0);
+    }
+
+    @Override
+    public double getValue() { return motor.getCurrentPosition(); }
+}

encoder/Encoder.java

@@ -0,0 +1,9 @@
+package org.redshiftrobotics.lib.encoder;
+
+/**
+ * Created by ariporad on 2017-10-03.
+ */
+
+public interface Encoder {
+    double getValue();
+}

encoder/EncoderPositioner.java

@@ -0,0 +1,56 @@
+package org.redshiftrobotics.lib.encoder;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.redshiftrobotics.lib.Positioner;
+import org.redshiftrobotics.lib.encoder.Encoder;
+
+public class EncoderPositioner implements Positioner {
+    private class EncoderTracker {
+        private Encoder encoder;
+
+        private double startPos;
+
+        double ticksPerCM;
+
+        EncoderTracker(Encoder encoder, double ticksPerCM) {
+            this.encoder = encoder;
+            this.ticksPerCM = ticksPerCM;
+            reset();
+        }
+
+        void reset() {
+            startPos = encoder.getValue();
+        }
+
+        double getValue() {
+            return encoder.getValue() - startPos;
+        }
+
+        double getPos() {
+            return getValue() * ticksPerCM;
+        }
+    }
+
+    EncoderTracker leftRight;
+    EncoderTracker frontBack;
+
+    public EncoderPositioner(Encoder leftRight, Encoder frontBack, double ticksPerCM) {
+        this.leftRight = new EncoderTracker(leftRight, ticksPerCM);
+        this.frontBack = new EncoderTracker(frontBack, ticksPerCM);
+    }
+
+    public void reset() {
+        leftRight.reset();
+        frontBack.reset();
+    }
+
+    public Position getPosition() {
+        return new Position(DistanceUnit.CM, this.leftRight.getPos(), this.frontBack.getPos(), 0, 0);
+    }
+
+    public void setTicksPerCM(double ticksPerCM) {
+        this.leftRight.ticksPerCM = ticksPerCM;
+        this.frontBack.ticksPerCM = ticksPerCM;
+    }
+}

pid/FakePIDManager.java

@@ -0,0 +1,25 @@
+package org.redshiftrobotics.lib.pid;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+
+/**
+ * Created by ariporad on 2017-10-11.
+ */
+
+public class FakePIDManager implements PIDManager {
+    public FakePIDManager(DcMotor m0, DcMotor m1, DcMotor m2, DcMotor m3) {
+
+    }
+
+    @Override
+    public void move(Position position, double speed, Position tolerance) {
+
+    }
+
+    @Override
+    public void turn(double degrees) {
+
+    }
+}

pid/PIDManager.java

@@ -0,0 +1,12 @@
+package org.redshiftrobotics.lib.pid;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+
+/**
+ * Created by ariporad on 2017-10-11.
+ */
+
+public interface PIDManager {
+    void move(Position position, double speed, Position tolerance);
+    void turn(double degrees);
+}