From ff1b1b8d144e8d9f36bf82dab1f51bfd96b4d8e9 Mon Sep 17 00:00:00 2001 From: sadie-datoo Date: Thu, 14 Dec 2023 14:45:29 -0700 Subject: [PATCH 1/3] Motor Encoder Test --- .../auton/{ => Tests}/DistanceSensorTest.java | 2 +- .../auton/Tests/MotorEncoderTest.java | 94 +++++++++++++++++++ .../auton/{ => Tests}/TouchSensorTest.java | 2 +- .../auton/vision/AprilTagAlignment.java | 1 - 4 files changed, 96 insertions(+), 3 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/{ => Tests}/DistanceSensorTest.java (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/{ => Tests}/TouchSensorTest.java (94%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/DistanceSensorTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/DistanceSensorTest.java index 53a784d1..5ecab72e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java new file mode 100644 index 00000000..0fa35827 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java @@ -0,0 +1,94 @@ +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.*; + +@Autonomous(name="Motor Encoder Test") + +public class MotorEncoderTest extends OpMode { + float wheelEncoderPPR = 537.7f; // PPR + int wheelDiameter = 96; // mm + double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm + double distance; + float motorRunPower = 0.4f; + private Hardware robot; + + @Override + public void init(){ + robot = new Hardware(hardwareMap, telemetry); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront")); + 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")); + } + + @Override + public void start() { + distance = inchesToMM(12); + + setTargetPosition(robot.get("leftFront"), distance); + setTargetPosition(robot.get("rightFront"), distance); + setTargetPosition(robot.get("leftRear"), distance); + setTargetPosition(robot.get("rightRear"), distance); + + while (wheelsInPosition()) { + updateMotors(); + } + } + + public double inchesToMM(double inches) { + return inches*25.4; + } + + public void loop(){} + public void setTargetPosition(DcMotor motor, double mm) { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setTargetPosition((int) Math.round(mm/mmPerEncoderTick)); + } + + public boolean wheelsInPosition() { + return robot.get("leftFront").getTargetPosition() != + robot.get("leftFront").getCurrentPosition() && + robot.get("rightFront").getTargetPosition() != + robot.get("rightFront").getCurrentPosition() && + robot.get("leftRear").getTargetPosition() != + robot.get("leftRear").getCurrentPosition() && + robot.get("rightRear").getTargetPosition() != + robot.get("rightRear").getCurrentPosition(); + } + + public void updateMotors() { + showMotorStatus(robot.get("leftFront")); + showMotorStatus(robot.get("rightFront")); + showMotorStatus(robot.get("leftRear")); + showMotorStatus(robot.get("rightRear")); + + updateMotorPower(robot.get("leftFront")); + updateMotorPower(robot.get("rightFront")); + updateMotorPower(robot.get("leftRear")); + updateMotorPower(robot.get("rightRear")); + } + + public void showMotorStatus(DcMotor motor) { + if (motor.getTargetPosition() == motor.getCurrentPosition()) { + telemetry.addLine(motor.getDeviceName() + " in Position"); + } else { + telemetry.addLine("Running " + motor.getDeviceName() + " Motor"); + telemetry.addData(motor.getDeviceName() + " Target Position", + motor.getTargetPosition()); + telemetry.addData(motor.getDeviceName() + " Current Position", + motor.getTargetPosition()); + } + } + + public void updateMotorPower(DcMotor motor) { + if (motor.getTargetPosition() == motor.getCurrentPosition()) { + motor.setPower(0); + } else { + motor.setPower(motorRunPower); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/TouchSensorTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/TouchSensorTest.java index 06e22156..1fc79bbe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/vision/AprilTagAlignment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/vision/AprilTagAlignment.java index 6bcb909c..2fc001fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/vision/AprilTagAlignment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/vision/AprilTagAlignment.java @@ -56,7 +56,6 @@ public class AprilTagAlignment extends OpMode { float wheelEncoderPPR = 537.7f; // PPR int wheelDiameter = 96; // mm double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm - // TODO: Measure and record turning radius. Should be the same as the distance between wheels. float turningRadius = 228.6f; // mm int targetTagID = 2; From deb72e8cdfd09b050bb6c2fc5a80ac2e54255e12 Mon Sep 17 00:00:00 2001 From: sadie-datoo Date: Tue, 9 Jan 2024 16:20:41 -0700 Subject: [PATCH 2/3] Fixed comments --- .../auton/Tests/MotorEncoderTest.java | 54 +++++++++++-------- 1 file changed, 31 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java index 0fa35827..fbc52b9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java @@ -9,11 +9,13 @@ @Autonomous(name="Motor Encoder Test") public class MotorEncoderTest extends OpMode { - float wheelEncoderPPR = 537.7f; // PPR - int wheelDiameter = 96; // mm - double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm - double distance; - float motorRunPower = 0.4f; + private float wheelEncoderPPR = 537.7f; // PPR + private int wheelDiameter = 96; // mm + private double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm + private double distance; + private float motorRunPower = 0.4f; + private float tolerance = 1; + // TODO: Figure out actual value of tolerance. private Hardware robot; @Override @@ -23,41 +25,42 @@ public void init(){ 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")); + + distance = inchesToMM(12); } @Override public void start() { - distance = inchesToMM(12); setTargetPosition(robot.get("leftFront"), distance); setTargetPosition(robot.get("rightFront"), distance); setTargetPosition(robot.get("leftRear"), distance); setTargetPosition(robot.get("rightRear"), distance); - while (wheelsInPosition()) { - updateMotors(); - } } public double inchesToMM(double inches) { return inches*25.4; } - public void loop(){} + public void loop() { + checkMotors(); + } + public void setTargetPosition(DcMotor motor, double mm) { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setTargetPosition((int) Math.round(mm/mmPerEncoderTick)); } public boolean wheelsInPosition() { - return robot.get("leftFront").getTargetPosition() != - robot.get("leftFront").getCurrentPosition() && - robot.get("rightFront").getTargetPosition() != - robot.get("rightFront").getCurrentPosition() && - robot.get("leftRear").getTargetPosition() != - robot.get("leftRear").getCurrentPosition() && - robot.get("rightRear").getTargetPosition() != - robot.get("rightRear").getCurrentPosition(); + return Math.abs(robot.get("leftFront").getTargetPosition() - + robot.get("leftFront").getCurrentPosition()) > tolerance && + Math.abs(robot.get("rightFront").getTargetPosition() - + robot.get("rightFront").getCurrentPosition()) > tolerance && + Math.abs(robot.get("leftRear").getTargetPosition() - + robot.get("leftRear").getCurrentPosition()) > tolerance && + Math.abs(robot.get("rightRear").getTargetPosition() - + robot.get("rightRear").getCurrentPosition()) > tolerance; } public void updateMotors() { @@ -72,20 +75,25 @@ public void updateMotors() { updateMotorPower(robot.get("rightRear")); } + public void checkMotors(){ + while (!wheelsInPosition()) { + updateMotors(); + } + } public void showMotorStatus(DcMotor motor) { - if (motor.getTargetPosition() == motor.getCurrentPosition()) { - telemetry.addLine(motor.getDeviceName() + " in Position"); + if (Math.abs(motor.getTargetPosition() - motor.getCurrentPosition()) < tolerance) { + telemetry.addLine(motor.getDeviceName() + " in Position."); } else { telemetry.addLine("Running " + motor.getDeviceName() + " Motor"); - telemetry.addData(motor.getDeviceName() + " Target Position", + telemetry.addData(motor.getDeviceName() + " Target Position.", motor.getTargetPosition()); - telemetry.addData(motor.getDeviceName() + " Current Position", + telemetry.addData(motor.getDeviceName() + " Current Position.", motor.getTargetPosition()); } } public void updateMotorPower(DcMotor motor) { - if (motor.getTargetPosition() == motor.getCurrentPosition()) { + if (Math.abs(motor.getTargetPosition() - motor.getCurrentPosition()) < tolerance) { motor.setPower(0); } else { motor.setPower(motorRunPower); From 43f14c1288fd07b339db08a90fa01d7dbede2821 Mon Sep 17 00:00:00 2001 From: sadie-datoo Date: Wed, 17 Jan 2024 21:06:14 -0700 Subject: [PATCH 3/3] Fixed TODO line placement. --- .../ftc/teamcode/auton/Tests/MotorEncoderTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java index fbc52b9a..93120aba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Tests/MotorEncoderTest.java @@ -14,8 +14,8 @@ public class MotorEncoderTest extends OpMode { private double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm private double distance; private float motorRunPower = 0.4f; - private float tolerance = 1; // TODO: Figure out actual value of tolerance. + private float tolerance = 1; private Hardware robot; @Override