From 30cd180c42f3967fc66ac7d7a94423a61c84e3e7 Mon Sep 17 00:00:00 2001 From: ExultantExalted Date: Sat, 2 Dec 2023 10:19:45 -0500 Subject: [PATCH 1/4] Added TeleOp, Control Button Things, Axon Servo Changes --- .../ftc/teamcode/hardware/AnalogTest.java | 8 +- .../ftc/teamcode/hardware/AxonServo.java | 7 +- .../ftc/teamcode/opmode/TeleOp.java | 74 +++++++++++++++++++ 3 files changed, 82 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java index 06f9c04e..adaea1e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java @@ -5,17 +5,17 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @TeleOp -@Disabled +//@Disabled public class AnalogTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - AxonServo servoTest = new AxonServo("servo test", "servoadc", hardwareMap); + AxonServo servoTest = new AxonServo("servo", "a0", hardwareMap); waitForStart(); while (opModeIsActive()) { if (gamepad1.a){ - servoTest.innerServo.setPosition(0.6);} + servoTest.innerServo.setPower(0.6);} else - {servoTest.innerServo.setPosition(0.5);} + {servoTest.innerServo.setPower(0.2);} servoTest.innerAnalog.getVoltage(); servoTest.getCurrentPosition(); telemetry.addData("Degrees",servoTest.getCurrentPosition()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java index 1e2581fa..a14d6530 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java @@ -2,14 +2,14 @@ import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.CRServo; public class AxonServo { - public Servo innerServo; + public CRServo innerServo; public AnalogInput innerAnalog; public AxonServo(String servoName, String analogName, HardwareMap hwMap) { - this.innerServo = hwMap.get(Servo.class, servoName); + this.innerServo = hwMap.get(CRServo.class, servoName); this.innerAnalog = hwMap.get(AnalogInput.class, analogName); } @@ -18,4 +18,5 @@ public double getCurrentPosition() { } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java new file mode 100644 index 00000000..e8804269 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.drive.testbot.TestBotDrive; +import org.firstinspires.ftc.teamcode.util.GlobalOpMode; +import org.firstinspires.ftc.teamcode.vision.AprilTagCamera; + +@com.qualcomm.robotcore.eventloop.opmode.TeleOp +public class TeleOp extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + GlobalOpMode.opMode = this; + TestBotDrive drive = new TestBotDrive(hardwareMap); + + AprilTagCamera[] cameras = new AprilTagCamera[3]; + cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(70), Math.toRadians(-45)); + cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0)); + cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45)); + + waitForStart(); + + + while (!isStopRequested()) { + // Read pose + Pose2d poseEstimate = drive.getPoseEstimate(); + + // Create a vector from the gamepad x/y inputs + // Then, rotate that vector by the inverse of that heading + Vector2d input = new Vector2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x).rotated(-poseEstimate.getHeading()); + + // Pass in the rotated input + right stick value for rotation + // Rotation is not part of the rotated input thus must be passed in separately + drive.setWeightedDrivePower(new Pose2d(input.getX(), input.getY(), -gamepad1.right_stick_x)); + + if (gamepad1.a) { + //turn on intake or else >:( + + } + else { + //turn off intake motor or i come for you + } + if (gamepad1.b) { + //make the release or i EAT YOU + } + else { + //close the pixel hand again + } + if (gamepad1.right_bumper && gamepad1.left_bumper) { + //launch the airplane + } + else { + //keep airplane closed or something idk + } + if (gamepad1.right_trigger>0.5) { + //raise bar hang + } + + if (gamepad1.left_trigger > 0.5) { + //lower bar hang + } + + } + + + + } +} +//hodie christus natus est terra canunt angeli laetantur archangeli alleluia hodie exultant justi dicentes gloria in excelsis deo et in terra pax homnibus bonnae voluntatis alleluia +//et in terra pax homnibus bonnae voluntatis gloria in excelsis deo lauda muste glorifica muste benedicimuste \ No newline at end of file From 1b695e663decafbc1e45c96d783d3f027d7b3590 Mon Sep 17 00:00:00 2001 From: ExultantExalted Date: Sat, 2 Dec 2023 12:12:00 -0500 Subject: [PATCH 2/4] added axon servo wraparound counter --- .../ftc/teamcode/hardware/AnalogTest.java | 19 +++++--- .../ftc/teamcode/hardware/AxonServo.java | 43 ++++++++++++++++++- 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java index adaea1e3..3fe16ad0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java @@ -12,13 +12,18 @@ public void runOpMode() throws InterruptedException { AxonServo servoTest = new AxonServo("servo", "a0", hardwareMap); waitForStart(); while (opModeIsActive()) { - if (gamepad1.a){ - servoTest.innerServo.setPower(0.6);} - else - {servoTest.innerServo.setPower(0.2);} - servoTest.innerAnalog.getVoltage(); - servoTest.getCurrentPosition(); - telemetry.addData("Degrees",servoTest.getCurrentPosition()); +// if (gamepad1.a) { +// servoTest.innerServo.setPower(0.6); +// } else { +// servoTest.innerServo.setPower(0); +// } + servoTest.innerServo.setPower(gamepad1.left_stick_x); + //servoTest.innerAnalog.getVoltage(); + //servoTest.getCurrentPosition(); + telemetry.addData("Degrees", servoTest.getCurrentPosition()); + telemetry.addData("Power", gamepad1.left_stick_x); + telemetry.addData("Last Pos", servoTest.getLastPosDebug()); + telemetry.addData("Adjusted Degrees", servoTest.getAdjustedPosition()); telemetry.addData("Voltage", servoTest.innerAnalog.getVoltage()); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java index a14d6530..beda660b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java @@ -8,13 +8,54 @@ public class AxonServo { public CRServo innerServo; public AnalogInput innerAnalog; + // Create a new background thread to count rotations + private ServoCounter counter; + + private class ServoCounter extends Thread { + private int count = 0; + + public double lastPosition = getCurrentPosition(); + + public void run() { + //double lastPosition = getCurrentPosition(); + while (true) { + android.util.Log.w("AXON_THREAD", "LOOP"); + double currentPosition = getCurrentPosition(); + // Negative wrap-around from 360 -> 0 + if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) { + count--; + } + // Positive wrap-around from 0 -> 360 + if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) { + count++; + } + lastPosition = currentPosition; + } + } + + public int getCount() { + return count; + } + } + public AxonServo(String servoName, String analogName, HardwareMap hwMap) { this.innerServo = hwMap.get(CRServo.class, servoName); this.innerAnalog = hwMap.get(AnalogInput.class, analogName); + + counter = new ServoCounter(); + counter.start(); } public double getCurrentPosition() { - return this.innerAnalog.getVoltage()/3.3 *360; + return this.innerAnalog.getVoltage()/3.3 * 360; + } + + public double getAdjustedPosition() { + return getCurrentPosition() + counter.getCount() * 360; + } + + public double getLastPosDebug() { + return this.counter.lastPosition; } From aa013e1cd0f5e355bbc25d65fb0530d7c35794d9 Mon Sep 17 00:00:00 2001 From: ExultantExalted Date: Sat, 2 Dec 2023 12:24:24 -0500 Subject: [PATCH 3/4] added more TeleOp comments --- .../ftc/teamcode/opmode/TeleOp.java | 38 ++++++++----------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java index e8804269..04c688e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java @@ -3,6 +3,8 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.drive.testbot.TestBotDrive; @@ -37,38 +39,30 @@ public void runOpMode() throws InterruptedException { // Rotation is not part of the rotated input thus must be passed in separately drive.setWeightedDrivePower(new Pose2d(input.getX(), input.getY(), -gamepad1.right_stick_x)); + // TODO: Add controls to gamepad 2 as well if (gamepad1.a) { - //turn on intake or else >:( - - } - else { - //turn off intake motor or i come for you + // Intake running while A is held + } else { + // Turn off intake when A released } + if (gamepad1.b) { - //make the release or i EAT YOU - } - else { - //close the pixel hand again + // Move pixel hand servo to release 1 pixel + // TODO: Do we need to debounce and count presses? Svit mentioned a "pixel cassette" that drops multiple pixels + } else { + // Close pixel hand servo } + if (gamepad1.right_bumper && gamepad1.left_bumper) { - //launch the airplane - } - else { - //keep airplane closed or something idk - } - if (gamepad1.right_trigger>0.5) { - //raise bar hang + // Launch the airplane + // TODO: Check for endgame? } - if (gamepad1.left_trigger > 0.5) { - //lower bar hang - } + // Set bar hang motor power to gamepad1.right_trigger - gamepad1.left_trigger for analog control } } -} -//hodie christus natus est terra canunt angeli laetantur archangeli alleluia hodie exultant justi dicentes gloria in excelsis deo et in terra pax homnibus bonnae voluntatis alleluia -//et in terra pax homnibus bonnae voluntatis gloria in excelsis deo lauda muste glorifica muste benedicimuste \ No newline at end of file +} \ No newline at end of file From db371867575b9c14f33027f8fc630d5c7f65a663 Mon Sep 17 00:00:00 2001 From: JJTech0130 Date: Sat, 2 Dec 2023 12:42:07 -0500 Subject: [PATCH 4/4] remove last pos debugging --- .../ftc/teamcode/hardware/AnalogTest.java | 3 +-- .../firstinspires/ftc/teamcode/hardware/AxonServo.java | 10 ++-------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java index 3fe16ad0..52c56ef7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.hardware; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -22,7 +21,7 @@ public void runOpMode() throws InterruptedException { //servoTest.getCurrentPosition(); telemetry.addData("Degrees", servoTest.getCurrentPosition()); telemetry.addData("Power", gamepad1.left_stick_x); - telemetry.addData("Last Pos", servoTest.getLastPosDebug()); + //telemetry.addData("Last Pos", servoTest.getLastPosDebug()); telemetry.addData("Adjusted Degrees", servoTest.getAdjustedPosition()); telemetry.addData("Voltage", servoTest.innerAnalog.getVoltage()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java index beda660b..74d37559 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java @@ -9,15 +9,13 @@ public class AxonServo { public AnalogInput innerAnalog; // Create a new background thread to count rotations - private ServoCounter counter; + private final ServoCounter counter; private class ServoCounter extends Thread { private int count = 0; - public double lastPosition = getCurrentPosition(); - public void run() { - //double lastPosition = getCurrentPosition(); + double lastPosition = getCurrentPosition(); while (true) { android.util.Log.w("AXON_THREAD", "LOOP"); double currentPosition = getCurrentPosition(); @@ -54,10 +52,6 @@ public double getAdjustedPosition() { return getCurrentPosition() + counter.getCount() * 360; } - public double getLastPosDebug() { - return this.counter.lastPosition; - } - }