origin/drive_testing

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointSpirit.java

@@ -0,0 +1,116 @@
+/*
+*   Copyright (c) 2025 Alan Smith
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy
+*   of this software and associated documentation files (the "Software"), to deal
+*   in the Software without restriction, including without limitation the rights
+*   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+*   copies of the Software, and to permit persons to whom the Software is
+*   furnished to do so, subject to the following conditions:
+
+*   The above copyright notice and this permission notice shall be included in all
+*   copies or substantial portions of the Software.
+
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+*   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+*   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+*   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+*   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+*   SOFTWARE.
+*/
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+
+/*
+ * This OpMode illustrates how to use the GoBildaPinpoint
+ *
+ * The OpMode assumes that the sensor is configured with a name of "pinpoint".
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * See the sensor's product page: https://www.gobilda.com/pinpoint-odometry-computer-imu-sensor-fusion-for-2-wheel-odometry/
+ */
+@TeleOp(name = "Sensor: GoBilda Pinpoint", group = "Sensor")
+//@Disabled
+public class SensorGoBildaPinpointSpirit extends OpMode {
+    // Create an instance of the sensor
+    GoBildaPinpointDriver pinpoint;
+
+    @Override
+    public void init() {
+        // Get a reference to the sensor
+        pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+
+        // Configure the sensor
+        configurePinpoint();
+
+        // Set the location of the robot - this should be the place you are starting the robot from
+        pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0));
+    }
+
+    @Override
+    public void loop() {
+        telemetry.addLine("Push your robot around to see it track");
+        telemetry.addLine("Press A to reset the position");
+        if(gamepad1.a){
+            // You could use readings from April Tags here to give a new known position to the pinpoint
+            pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0));
+        }
+        pinpoint.update();
+        Pose2D pose2D = pinpoint.getPosition();
+
+        telemetry.addData("X coordinate (IN)", pose2D.getX(DistanceUnit.INCH));
+        telemetry.addData("Y coordinate (IN)", pose2D.getY(DistanceUnit.INCH));
+        telemetry.addData("Heading angle (DEGREES)", pose2D.getHeading(AngleUnit.DEGREES));
+    }
+
+    public void configurePinpoint(){
+       /*
+        *  Set the odometry pod positions relative to the point that you want the position to be measured from.
+        *
+        *  The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is.
+        *  Left of the center is a positive number, right of center is a negative number.
+        *
+        *  The Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is.
+        *  Forward of center is a positive number, backwards is a negative number.
+        */
+        pinpoint.setOffsets(-84.0, -168.0, DistanceUnit.MM); //these are tuned for 3110-0002-0001 Product Insight #1
+
+        /*
+         * Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
+         * the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
+         * If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
+         * number of ticks per unit of your odometry pod.  For example: 
+         *     pinpoint.setEncoderResolution(13.26291192, DistanceUnit.MM);
+         */
+        pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD);
+
+        /*
+         * Set the direction that each of the two odometry pods count. The X (forward) pod should
+         * increase when you move the robot forward. And the Y (strafe) pod should increase when
+         * you move the robot to the left.
+         */
+        pinpoint.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD,
+                                      GoBildaPinpointDriver.EncoderDirection.FORWARD);
+
+        /*
+         * Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
+         * The IMU will automatically calibrate when first powered on, but recalibrating before running
+         * the robot is a good idea to ensure that the calibration is "good".
+         * resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
+         * This is recommended before you run your autonomous, as a bad initial calibration can cause
+         * an incorrect starting value for x, y, and heading.
+         */
+        pinpoint.resetPosAndIMU();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorLimelight3ASpirit.java

@@ -0,0 +1,158 @@
+/*
+Copyright (c) 2024 Limelight Vision
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of FIRST nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.LLStatus;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use the Limelight3A Vision Sensor.
+ *
+ * @see <a href="https://limelightvision.io/">Limelight</a>
+ *
+ * Notes on configuration:
+ *
+ *   The device presents itself, when plugged into a USB port on a Control Hub as an ethernet
+ *   interface.  A DHCP server running on the Limelight automatically assigns the Control Hub an
+ *   ip address for the new ethernet interface.
+ *
+ *   Since the Limelight is plugged into a USB port, it will be listed on the top level configuration
+ *   activity along with the Control Hub Portal and other USB devices such as webcams.  Typically
+ *   serial numbers are displayed below the device's names.  In the case of the Limelight device, the
+ *   Control Hub's assigned ip address for that ethernet interface is used as the "serial number".
+ *
+ *   Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight
+ *   and specify the Limelight's ip address.  Users should take care not to confuse the ip address of
+ *   the Limelight itself, which can be configured through the Limelight settings page via a web browser,
+ *   and the ip address the Limelight device assigned the Control Hub and which is displayed in small text
+ *   below the name of the Limelight on the top level configuration screen.
+ */
+@TeleOp(name = "Sensor: Limelight3A", group = "Sensor")
+//@Disabled
+public class SensorLimelight3ASpirit extends LinearOpMode {
+
+    private Limelight3A limelight;
+
+    @Override
+    public void runOpMode() throws InterruptedException
+    {
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+        telemetry.setMsTransmissionInterval(11);
+
+        limelight.pipelineSwitch(0);
+
+        /*
+         * Starts polling for data.  If you neglect to call start(), getLatestResult() will return null.
+         */
+        limelight.start();
+
+        telemetry.addData(">", "Robot Ready.  Press Play.");
+        telemetry.update();
+        waitForStart();
+
+        while (opModeIsActive()) {
+            LLStatus status = limelight.getStatus();
+            telemetry.addData("Name", "%s",
+                    status.getName());
+            telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d",
+                    status.getTemp(), status.getCpu(),(int)status.getFps());
+            telemetry.addData("Pipeline", "Index: %d, Type: %s",
+                    status.getPipelineIndex(), status.getPipelineType());
+
+            LLResult result = limelight.getLatestResult();
+            if (result.isValid()) {
+                // Access general information
+                Pose3D botpose = result.getBotpose();
+                double captureLatency = result.getCaptureLatency();
+                double targetingLatency = result.getTargetingLatency();
+                double parseLatency = result.getParseLatency();
+                telemetry.addData("LL Latency", captureLatency + targetingLatency);
+                telemetry.addData("Parse Latency", parseLatency);
+                telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput()));
+
+                telemetry.addData("tx", result.getTx());//angle in degrees on x axis from target, by default it's the center of the April tag
+                telemetry.addData("txnc", result.getTxNC());//not using this one
+                telemetry.addData("ty", result.getTy());//will need this if we can tilt the shooter
+                telemetry.addData("tync", result.getTyNC());//not using but tells you the number of the april tag it's looking at
+
+                telemetry.addData("Botpose", botpose.toString());
+
+                // Access barcode results
+                List<LLResultTypes.BarcodeResult> barcodeResults = result.getBarcodeResults();
+                for (LLResultTypes.BarcodeResult br : barcodeResults) {
+                    telemetry.addData("Barcode", "Data: %s", br.getData());
+                }
+
+                // Access classifier results
+                List<LLResultTypes.ClassifierResult> classifierResults = result.getClassifierResults();
+                for (LLResultTypes.ClassifierResult cr : classifierResults) {
+                    telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence());
+                }
+
+                // Access detector results
+                List<LLResultTypes.DetectorResult> detectorResults = result.getDetectorResults();
+                for (LLResultTypes.DetectorResult dr : detectorResults) {
+                    telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea());
+                }
+
+                // Access fiducial results
+                List<LLResultTypes.FiducialResult> fiducialResults = result.getFiducialResults();
+                for (LLResultTypes.FiducialResult fr : fiducialResults) {
+                    telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees());
+                    //21-23 are diferent oblisks (patterns) so 21 is gpp, 22 is pgp, 23 is ppg. 20 is blue and 25 is red (we think)
+                }
+
+                // Access color results
+                List<LLResultTypes.ColorResult> colorResults = result.getColorResults();
+                for (LLResultTypes.ColorResult cr : colorResults) {
+                    telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees());
+                }
+            } else {
+                telemetry.addData("Limelight", "No data available");
+            }
+
+            telemetry.update();
+        }
+        limelight.stop();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Carousel.java

@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.robot;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PwmControl;
+import com.qualcomm.robotcore.hardware.Servo;
+//import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class Carousel {
+
+   private final Servo shooterServo;//lifts the teardrop to feed the artifact to the shooter
+  // private final Servo carouselServo;//turns the carousel
+  //private final ServoImplEx carouselServo;//turns the carousel
+     private final Servo carouselServo;//turns the carousel
+    double downKickerPosition = .9;//value to position kicker down
+    double tinyLiftKickerPosition = .8;//value to cause kicker to lift a tiny bit to allow tilt
+    double fullLiftKickerPosition = .5;//value to life kicker up to shooter
+    double carouselHomePosition = 0;//starting position of carousel
+    double carouselPosition = .2;//position to which carousel will move to advance
+    double carouselPositionIncrement = .2;//value needed to advance carousel by 120 degrees
+
+
+    public Carousel(HardwareMap hardwareMap) {
+       shooterServo = hardwareMap.get(Servo.class, "shooterServo");
+        carouselServo = hardwareMap.get(Servo.class, "carouselServo");
+      // carouselServo = hardwareMap.get(ServoImplEx.class, "carouselServo");
+     // carouselServo.setPwmRange(new PwmRange(500, 2500));
+      //  carouselServo.setPwmRange(new PwmControl.PwmRange(500,2500));
+    }
+    //-------------------KICK SHOOTER SERVO POSITIONING ----------------------
+
+    //positions the shooter servo in the down position
+    public void setDownPositionKickerServo(){
+        shooterServo.setPosition(downKickerPosition);
+    }
+    //lifts the shooter servo up slightly so the carousel can move
+    public void tinyLiftKickerServo(){
+        shooterServo.setPosition(tinyLiftKickerPosition);
+    }
+    //makes the shooter servo feed the artifact to the shooters
+    public void fullLiftKickerServo(){
+        shooterServo.setPosition(fullLiftKickerPosition);
+    }
+
+    public void spinCarouselHome(){
+        carouselServo.setPosition(carouselHomePosition);
+}
+   public void spinCarousel() {
+       carouselServo.setPosition(carouselPosition);
+   }
+   // public void spinCarouselBackward(){
+   //    carouselServo.setPosition();
+   // }
+    }
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Intake.java

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.robot;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class Intake {
+    private final DcMotorEx intakeMotor;
+
+    public Intake(HardwareMap hardwareMap) {
+        intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor");
+    }
+
+    public void setPower(double power) {
+        intakeMotor.setPower(power);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/MecanumDrive.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.robot;
 
 import androidx.annotation.NonNull;
 
@@ -41,6 +41,8 @@
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.Drawing;
+import org.firstinspires.ftc.teamcode.Localizer;
 import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
 import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage;
 import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage;
@@ -58,9 +60,9 @@ public static class Params {
         // TODO: fill in these values based on
         //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
         public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
-                RevHubOrientationOnRobot.LogoFacingDirection.UP;
+                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
         public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+                RevHubOrientationOnRobot.UsbFacingDirection.UP;
 
         // drive model parameters
         public double inPerTick = 1;
@@ -236,7 +238,12 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
         rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         // TODO: reverse motor directions if needed
-        //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+       //leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
+     //   leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
+        //rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
+
+
 
         // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html