Add Demo for goBILDA Indicator Light

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

@@ -0,0 +1,92 @@
+/*
+        Copyright (c) 2024 Alan Smith
+        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 Alan Smith 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 FITNESSFOR 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;
+
+/*
+ * This OpMode illustrates how to use the goBILDA Indicator Light
+ *
+ * To make this work, this Light uses a Servo motor and setPositions.
+ * Valid values are from .277 to .722.
+ *
+ * If it's less than .277 then no light
+ * If it's greater than .722 then white light
+ *
+ * Spec Sheet is here: https://cdn11.bigcommerce.com/s-x56mtydx1w/images/stencil/original/products/2275/13464/3118-0808-0002-Product-Insight-4__03940__01348.1728056113.png?c=1
+ */
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@TeleOp(name = "Demo: goBILDA Indicator Light", group = "Concept")
+public class DemoGoBildaIndicatorLight extends LinearOpMode {
+    Servo front_led;
+    private final double buffer = .277;
+    @Override
+    public void runOpMode() {
+        front_led = hardwareMap.get(Servo.class, "test_servo");
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            double left_x = Math.abs(gamepad1.left_stick_x) / 4;
+            double left_y = Math.abs(gamepad1.left_stick_y) / 4;
+            double right_x = Math.abs(gamepad1.right_stick_x) / 4;
+            double right_y = Math.abs(gamepad1.right_stick_y) / 4;
+            double total_position = Math.min(left_x + left_y + right_x + right_y + buffer, 1.0);
+
+            telemetry.addData("left_x", left_x);
+            telemetry.addData("left_y", left_y);
+            telemetry.addData("right_x", right_x);
+            telemetry.addData("right_y", right_y);
+            telemetry.addData("total (plus buffer)", total_position);
+            telemetry.update();
+
+            front_led.setPosition(total_position);
+
+            if (gamepad1.cross) {
+                front_led.setPosition(.611);
+                sleep(2000);
+            }
+            if (gamepad1.triangle) {
+                front_led.setPosition(.444);
+                sleep(2000);
+            }
+            if (gamepad1.square) {
+                front_led.setPosition(.7);
+                sleep(2000);
+            }
+            if (gamepad1.circle) {
+                front_led.setPosition(.3);
+                sleep(2000);
+            }
+
+
+        }
+    }
+
+}

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

@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+
+public class PedroConstants {
+
+    /*
+        Robot parameters
+    */
+
+    // Robot motor configurations
+    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
+    public static final String BACK_LEFT_MOTOR  = "Drive back lt";
+    public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";
+    public static final String BACK_RIGHT_MOTOR  = "Drive back rt";
+
+    // Robot motor direction
+    public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
+    public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
+    public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
+    public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
+
+    // Robot IMU configuration
+    public static final String IMU = "imu";
+
+    // Robot IMU placement
+    public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
+            = RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
+    public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
+            = RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
+
+    // Robot encoders
+    public static final String LEFT_ENCODER = "encoder left";
+    public static final String RIGHT_ENCODER = "encoder right";
+    public static final String BACK_ENCODER = "encoder back";
+
+    // Robot encoder direction
+    public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
+    public static final double BACK_ENCODER_DIRECTION = Encoder.REVERSE;
+
+    /*
+        Pedro's parameters
+    */
+
+    // The weight of the robot in Kilograms
+    public static final double ROBOT_WEIGHT_IN_KG = 10.5;
+
+    // Maximum velocity of the robot going forward
+    public static final double ROBOT_SPEED_FORWARD = 72.0693;
+
+    // Maximum velocity of the robot going right
+    public static final double ROBOT_SPEED_LATERAL = 24.1401;
+
+    // Rate of deceleration when power is cut-off when the robot is moving forward
+    public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779;
+
+    // Rate of deceleration when power is cut-off when the robot is moving to the right
+    public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409;
+
+    // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
+    public static final double ZERO_POWER_ACCEL_MULT = 4;
+
+    /* Centripetal force correction - increase if robot is correcting into the path
+                                    - decrease if robot is correcting away from the path */
+    public static final double CENTRIPETAL_SCALING = 0.0005;
+}

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

@@ -1,44 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-public class PedrosConstants {
-
-    // Robot motor configurations
-    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
-    public static final String BACK_LEFT_MOTOR  = "Drive back lt";
-    public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";
-    public static final String BACK_RIGHT_MOTOR  = "Drive back rt";
-
-    // Robot IMU configuration
-    public static final String IMU = "imu";
-
-    // Robot Encoders
-    public static final String LEFT_ENCODER = "encoder left";
-    public static final String RIGHT_ENCODER = "encoder right";
-    public static final String BACK_ENCODER = "encoder back";
-
-    /*
-        Pedro's parameters
-     */
-
-    // The weight of the robot in Kilograms
-    public static final double ROBOT_WEIGHT_IN_KG = 10.65942;
-
-    // Maximum velocity of the robot going forward
-    public static final double ROBOT_SPEED_FORWARD = 81.34056;
-
-    // Maximum velocity of the robot going right
-    public static final double ROBOT_SPEED_LATERAL = 65.43028;
-
-    // Rate of deceleration when power is cut-off when the robot is moving forward
-    public static final double FORWARD_ZERO_POWER_ACCEL = -34.62719;
-
-    // Rate of deceleration when power is cut-off when the robot is moving to the right
-    public static final double LATERAL_ZERO_POWER_ACCEL = -78.15554;
-
-    // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
-    public static final double ZERO_POWER_ACCEL_MULT = 4;
-
-    /* Centripetal force correction - increase if robot is correcting into the path
-                                    - decrease if robot is correcting away from the path */
-    public static final double CENTRIPETAL_SCALING = 0.0005;
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.follower;
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
@@ -168,8 +169,10 @@ public class Follower {
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
 
         // TODO: Make sure that this is the direction your motors need to be reversed in.
-        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
+        leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
+        rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
+        rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
 
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java

@@ -69,7 +69,7 @@ public class PoseUpdater {
      */
     public PoseUpdater(HardwareMap hardwareMap) {
         // TODO: replace the second argument with your preferred localizer
-        this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
+        this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap));
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
@@ -65,9 +65,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
     private double previousIMUOrientation;
     private double deltaRadians;
     private double totalHeading;
-    public static double FORWARD_TICKS_TO_INCHES = 0.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
-    public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
-    public static double TURN_TICKS_TO_RADIANS = 0.002995;//8192 * 1.37795 * 2 * Math.PI * 0.5;
+    public static double FORWARD_TICKS_TO_INCHES = 0.004;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    public static double STRAFE_TICKS_TO_INCHES = -0.0036;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+    public static double TURN_TICKS_TO_RADIANS = 0.0043;//8192 * 1.37795 * 2 * Math.PI * 0.5;
 
     public static boolean useIMU = true;
 
@@ -93,12 +93,12 @@ public class ThreeWheelIMULocalizer extends Localizer {
         imu = hardwareMap.get(IMU.class, IMU);
 
         // TODO: replace this with your IMU's orientation
-        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)));
+        imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION)));
 
         // TODO: replace these with your encoder positions
-        leftEncoderPose = new Pose(-3, 5.7, 0);
-        rightEncoderPose = new Pose(-3, -5.7, 0);
-        strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90));
+        leftEncoderPose = new Pose(-7.625, 6.19375, 0);
+        rightEncoderPose = new Pose(-7.625, -6.19375, 0);
+        strafeEncoderPose = new Pose(7, 1, Math.toRadians(90));
 
         // TODO: replace these with your encoder ports
         leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
@@ -106,9 +106,9 @@ public class ThreeWheelIMULocalizer extends Localizer {
         strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(Encoder.REVERSE);
-        rightEncoder.setDirection(Encoder.FORWARD);
-        strafeEncoder.setDirection(Encoder.FORWARD);
+        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
+        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
+        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer {
         strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(Encoder.REVERSE);
-        rightEncoder.setDirection(Encoder.REVERSE);
-        strafeEncoder.setDirection(Encoder.FORWARD);
+        //leftEncoder.setDirection(Encoder.REVERSE);
+       // rightEncoder.setDirection(Encoder.REVERSE);
+        //strafeEncoder.setDirection(Encoder.FORWARD);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java

@@ -1,7 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedrosConstants.LEFT_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.tuning;
 
-import static org.firstinspires.ftc.teamcode.PedrosConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.*;
 
 import com.acmerobotics.dashboard.config.Config;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java

@@ -52,7 +52,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
 
     private PoseUpdater poseUpdater;
 
-    public static double VELOCITY = 30;
+    public static double VELOCITY = 50;
 
     private double previousVelocity;