Bug fixes

TeamCode/src/main/java/codebase/Constants.java

@@ -0,0 +1,12 @@
+package codebase;
+
+import com.qualcomm.robotcore.hardware.PIDCoefficients;
+
+public class Constants {
+    public static final PIDCoefficients MOVEMENT_PID_COEFFICIENTS = new PIDCoefficients(0, 0, 0);
+    public static final PIDCoefficients DIRECTION_PID_COEFFICIENTS = new PIDCoefficients(0, 0, 0);
+
+    public static class MotorConstants {
+        public static double GOBILDA_5202_TICKS_PER_ROTATION = 384.5;
+    }
+}

TeamCode/src/main/java/codebase/actions/MoveToAction.java

@@ -1,5 +1,8 @@
 package codebase.actions;
 
+import static codebase.Constants.DIRECTION_PID_COEFFICIENTS;
+import static codebase.Constants.MOVEMENT_PID_COEFFICIENTS;
+
 import com.qualcomm.robotcore.hardware.PIDCoefficients;
 
 import codebase.controllers.PIDController;
@@ -10,9 +13,6 @@
 import codebase.pathing.Localizer;
 
 public class MoveToAction implements Action {
-
-    private static final PIDCoefficients MOVEMENT_PID_COEFFICIENTS = new PIDCoefficients(1, 0, 0);
-    private static final PIDCoefficients DIRECTION_PID_COEFFICIENTS = new PIDCoefficients(1, 0, 0);
     private final MecanumDriver driver;
     private final Localizer localizer;
 
@@ -53,9 +53,7 @@ public MoveToAction(MecanumDriver driver, Localizer localizer, FieldPosition des
     }
 
     @Override
-    public void init() {
-
-    }
+    public void init() {}
 
     @Override
     public void loop() {
@@ -64,18 +62,29 @@ public void loop() {
         double powerRotational = directionPID.getPower();
 
         MovementVector vector = new MovementVector(
-                movementSpeed * powerY,
                 movementSpeed * powerX,
+                movementSpeed * powerY,
                 rotationalSpeed * powerRotational);
 
-        this.driver.setAbsoluteVelocity(localizer.getCurrentPosition(), vector);
+        this.driver.setAbsolutePower(localizer.getCurrentPosition(), vector);
     }
 
     @Override
     public boolean isComplete() {
         double distanceError = Math.sqrt(Math.pow(localizer.getCurrentPosition().x - destination.x, 2) + Math.pow(localizer.getCurrentPosition().y - destination.y, 2));
         double rotationalError = Angles.angleDifference(localizer.getCurrentPosition().direction, destination.direction);
 
-        return (distanceError <= maxDistanceError) && (rotationalError <= maxRotationalError);
+        if ((distanceError <= maxDistanceError) && (rotationalError <= maxRotationalError)) {
+            driver.stop();
+            return true;
+        }
+
+        return false;
+    }
+
+    public void setPIDCoefficients(PIDCoefficients movementCoefficients, PIDCoefficients rotationCoefficients) {
+        this.xPID.setCoefficients(movementCoefficients);
+        this.yPID.setCoefficients(movementCoefficients);
+        this.directionPID.setCoefficients(rotationCoefficients);
     }
 }

TeamCode/src/main/java/codebase/actions/SimultaneousAction.java

@@ -18,6 +18,10 @@ public SimultaneousAction(Action first, Action... rest) {
         }
     }
 
+    public SimultaneousAction() {
+        this.actions = new ArrayList<>();
+    }
+
     @Override
     public void init() {
         for (Action action : actions) {
@@ -47,15 +51,25 @@ public void loop() {
     public void add(@NonNull Action action, boolean init) {
         String actionName = action.getClass().getName();
 
+        Action toDelete = null;
+
         for (Action a : actions) {
-            if (a.getClass().getName().equals(actionName)) {
-                throw new IllegalArgumentException("You can't add multiple of the same class of Action to SimultaneousAction.");
+            if (a.getClass().getName().equals(actionName) && !a.isComplete()) {
+                toDelete = a;
             }
         }
+        if (toDelete != null) {
+            actions.remove(toDelete);
+        }
+
         actions.add(action);
 
         if (init) {
             action.init();
         }
     }
+
+    public ArrayList<Action> getActions() {
+        return actions;
+    }
 }

TeamCode/src/main/java/codebase/controllers/PIDController.java

@@ -6,12 +6,13 @@
 
 public class PIDController {
 
-    private final PIDCoefficients coefficients;
+    private PIDCoefficients coefficients;
     private final Supplier<Double> errorSupplier;
 
     private double integralSum = 0;
     private double lastError = 0;
     private double lastTime = 0;
+    private double lastDerivative = 0;
 
     public PIDController(PIDCoefficients coefficients, Supplier<Double> currentPositionSupplier, Supplier<Double> targetPositionSupplier) {
         this(coefficients, () -> targetPositionSupplier.get() - currentPositionSupplier.get());
@@ -33,15 +34,23 @@ public double getPower() {
 
         double error = errorSupplier.get();
 
-        double derivative = (error - lastError) / deltaTimeSeconds;
+        double rawDerivative = (error - lastError) / deltaTimeSeconds;
+
+        double alpha = 0.01;
+        double derivative = (alpha * rawDerivative) + ((1 - alpha) * lastDerivative);
 
         integralSum += (error * deltaTimeSeconds);
 
         double result = (coefficients.p * error) + (coefficients.i * integralSum) + (coefficients.d * derivative);
 
         lastError = error;
         lastTime = System.currentTimeMillis();
+        lastDerivative = derivative;
 
         return result;
     }
+
+    public void setCoefficients(PIDCoefficients coefficients) {
+        this.coefficients = coefficients;
+    }
 }

TeamCode/src/main/java/codebase/gamepad/Gamepad.java

@@ -43,8 +43,9 @@ public Gamepad(com.qualcomm.robotcore.hardware.Gamepad gamepad) {
         this.dpadUp = new Button(this, () -> gamepad.dpad_up);
         this.dpadDown = new Button(this, () -> gamepad.dpad_down);
 
-        this.leftJoystick = new Joystick(this, () -> gamepad.left_stick_x, () -> gamepad.left_stick_y);
-        this.rightJoystick = new Joystick(this, () -> gamepad.right_stick_x, () -> gamepad.right_stick_y);
+        // y values from the robotcore gamepad class are inverted from what is expected: + is up, - is down. This corrects it to + is up, - is down
+        this.leftJoystick = new Joystick(this, () -> gamepad.left_stick_x, () -> -gamepad.left_stick_y);
+        this.rightJoystick = new Joystick(this, () -> gamepad.right_stick_x, () -> -gamepad.right_stick_y);
 
         this.leftJoystickButton = new Button(this, () -> gamepad.left_stick_button);
         this.rightJoystickButton = new Button(this, () -> gamepad.right_stick_button);

TeamCode/src/main/java/codebase/geometry/FieldPosition.java

@@ -1,9 +1,9 @@
 package codebase.geometry;
 
 
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import android.annotation.SuppressLint;
+
+import androidx.annotation.NonNull;
 
 public class FieldPosition extends Point {
 
@@ -16,4 +16,11 @@ public FieldPosition(double x, double y, double direction) {
         super(x, y);
         this.direction = direction;
     }
+
+    @NonNull
+    @SuppressLint("DefaultLocale")
+    @Override
+    public String toString() {
+        return String.format("(%f, %f, %f) (in, in, rad)", x, y, direction);
+    }
 }

TeamCode/src/main/java/codebase/hardware/Motor.java

@@ -16,6 +16,14 @@ public Motor(DcMotorEx motor, double ticksPerRotation, double wheelDiameter) {
         this.wheelDiameter = wheelDiameter;
     }
 
+    public Motor(DcMotorEx motor) {
+        this(motor, 1, 1);
+    }
+
+    public void setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior behavior) {
+        this.motor.setZeroPowerBehavior(behavior);
+    }
+
     /**
      * Sets the velocity in relation to the diameter of the wheel and the ticks per rotation of the motor.
      * @param velocity The desired velocity measured in inches per second.