Merge remote-tracking branch 'origin/apriltag' into apriltag

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java

@@ -1,24 +1,28 @@
 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;
 
 @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);}
-             else
-                {servoTest.innerServo.setPosition(0.5);}
-            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();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java

@@ -2,20 +2,56 @@
 
 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;
 
+    // Create a new background thread to count rotations
+    private final ServoCounter counter;
+
+    private class ServoCounter extends Thread {
+        private int count = 0;
+
+        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(Servo.class, servoName);
+        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;
     }
 
 
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java

@@ -0,0 +1,68 @@
+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 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;
+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));
+
+            // TODO: Add controls to gamepad 2 as well
+            if (gamepad1.a) {
+                // Intake running while A is held
+            } else {
+                // Turn off intake when A released
+            }
+
+            if (gamepad1.b) {
+                // 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
+                // TODO: Check for endgame?
+            }
+
+            // Set bar hang motor power to gamepad1.right_trigger - gamepad1.left_trigger for analog control
+
+        }
+
+
+
+    }
+}