Rayne working teleop

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.auton;
 
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -11,6 +12,7 @@
 
 import org.firstinspires.ftc.teamcode.subsystems.robotMethods.*;
 
+@Autonomous(name = "Auton Park")
 public class autonPark extends OpMode {
     byte startingLocationIndex = 0;
     double[][] startingLocation = new double[][] {new double[] {0, 0, 0}, new double[] {0, 0, 0},

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/AutonWheelsTest.java

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.auton.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
+import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement;
+import org.firstinspires.ftc.teamcode.subsystems.robotMethods.RobotMethods;
+import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Context;
+
+@Autonomous(name = "Auton Wheels Test")
+public class AutonWheelsTest extends OpMode {
+    Hardware robot;
+    Context context;
+    double motorRunPower = 0.5;
+
+    @Override
+    public void init() {
+        telemetry.addData("Status", "Initializing");
+
+        robot = new Hardware(hardwareMap, telemetry);
+        context = new Context();
+
+        // Init DcMotors.
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront",
+                "setDirection:REVERSE"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear",
+                "setDirection:REVERSE"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront"));
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear"));
+
+        telemetry.addData("Status", "Initialized");
+    }
+
+    @Override
+    public void start() {
+        RobotMethods.setTargetPosition(robot.<DcMotor>get("leftFront"), 300);
+        RobotMethods.setTargetPosition(robot.<DcMotor>get("leftRear"), 300);
+        RobotMethods.setTargetPosition(robot.<DcMotor>get("rightFront"), 300);
+        RobotMethods.setTargetPosition(robot.<DcMotor>get("rightRear"), 300);
+    }
+
+    @Override
+    public void loop() {
+        if (gamepad1.a) {
+            robot.<DcMotor>get("leftFront").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.<DcMotor>get("leftRear").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.<DcMotor>get("rightFront").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.<DcMotor>get("rightRear").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        } else {
+            RobotMethods.updateMotor(telemetry, robot.<DcMotor>get("leftFront"), motorRunPower);
+            RobotMethods.updateMotor(telemetry, robot.<DcMotor>get("leftRear"), motorRunPower);
+            RobotMethods.updateMotor(telemetry, robot.<DcMotor>get("rightFront"), motorRunPower);
+            RobotMethods.updateMotor(telemetry, robot.<DcMotor>get("rightRear"), motorRunPower);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DcMotorTest.java

@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.auton.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
+import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement;
+
+@Autonomous(name = "DcMotor Test")
+public class DcMotorTest extends OpMode {
+    Hardware robot;
+
+    @Override
+    public void init() {
+        telemetry.addData("Status", "Initializing");
+
+        robot = new Hardware(hardwareMap, telemetry);
+
+        // Init DcMotors.
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "testMotor"));
+        robot.<DcMotor>get("testMotor").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        telemetry.addData("Status", "Initialized");
+    }
+
+    @Override
+    public void start() {
+        robot.<DcMotor>get("testMotor").setTargetPosition(-300);
+        robot.<DcMotor>get("testMotor").setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.<DcMotor>get("testMotor").setPower(-0.75);
+    }
+
+    @Override
+    public void loop() {
+        if (robot.<DcMotor>get("testMotor").getCurrentPosition() > 100) {
+            robot.<DcMotor>get("testMotor").setPower(0.75);
+        } else if (robot.<DcMotor>get("testMotor").getCurrentPosition() <=
+                robot.<DcMotor>get("testMotor").getTargetPosition()) {
+            robot.<DcMotor>get("testMotor").setPower(0);
+        }
+
+        telemetry.addData("target pos", robot.<DcMotor>get("testMotor").getTargetPosition());
+        telemetry.addData("current pos", robot.<DcMotor>get("testMotor").getCurrentPosition());
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DistanceSensorTest.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.auton;
+package org.firstinspires.ftc.teamcode.auton.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.auton.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
+import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement;
+import org.firstinspires.ftc.teamcode.subsystems.robotMethods.RobotMethods;
+import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Context;
+
+@Autonomous(name = "Robot Methods Test")
+public class RobotMethodsTest extends OpMode {
+    Hardware robot;
+    Context context;
+    double motorRunPower = 0.5;
+
+    @Override
+    public void init() {
+        telemetry.addData("Status", "Initializing");
+
+        robot = new Hardware(hardwareMap, telemetry);
+        context = new Context();
+
+        // Init DcMotors.
+        robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "testMotor"));
+
+        telemetry.addData("Status", "Initialized");
+    }
+
+    @Override
+    public void start() {
+        RobotMethods.setTargetPosition(robot.<DcMotor>get("testMotor"), 300);
+    }
+
+    @Override
+    public void loop() {
+        RobotMethods.updateMotor(telemetry, robot.<DcMotor>get("testMotor"), motorRunPower);
+
+        if (gamepad1.a) {
+            robot.<DcMotor>get("testMotor").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/TouchSensorTest.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.auton;
+package org.firstinspires.ftc.teamcode.auton.tests;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java

@@ -118,21 +118,34 @@ private static void setWheelTargets(Hardware robot, double mm) {
         setTargetPosition(robot.<DcMotor>get("rightRear"), mm);
     }
 
-    private static void setTargetPosition(DcMotor motor, double mm) {
+    public static void setTargetPosition(DcMotor motor, double mm) {
         motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         motor.setTargetPosition((int) Math.round(mm/mmPerEncoderTick));
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
-    private static void updateMotor(Telemetry telemetry, DcMotor motor, double motorRunPower) {
+    public static void updateMotor(Telemetry telemetry, DcMotor motor, double motorRunPower) {
         showMotorStatus(motor, telemetry);
         updateMotorPower(motor, motorRunPower);
     }
 
     private static void updateMotorPower(DcMotor motor, double motorRunPower) {
-        if (motor.getTargetPosition() == motor.getCurrentPosition()) {
+        // Is motor in position?
+        if ((motor.getTargetPosition() <= 0 && motor.getCurrentPosition() <= motor.getTargetPosition() ||
+                motor.getTargetPosition() >= 0 && motor.getCurrentPosition() >= motor.getTargetPosition())) {
             motor.setPower(0);
-        } else {
-            motor.setPower(motorRunPower);
+        } else if (motor.getPower() == 0) { // If not in position and power is zero, set power.
+            if (motor.getTargetPosition() > 0){
+                motor.setPower(motorRunPower);
+            } else if (motor.getTargetPosition() > 0) {
+                motor.setPower(-motorRunPower);
+            }
+        }
+
+        // Catch any motors running the wrong way.
+        if (motor.getTargetPosition() < motor.getCurrentPosition() && motor.getCurrentPosition() > 0 ||
+                motor.getTargetPosition() > motor.getCurrentPosition() && motor.getCurrentPosition() < 0) {
+            motor.setPower(-motor.getPower());
         }
     }