auto blue, and red front have been made not tested but values switched

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

@@ -63,7 +63,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
  * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
  */
 
-@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot")
+@Autonomous(name="red front", group="Robot")
 //@Disabled
 public class Autonomoustest extends LinearOpMode {
 
@@ -182,7 +182,8 @@ public class Autonomoustest extends LinearOpMode {
         telemetry.addData("Red  ", colorRight.red());
         telemetry.addData("Green", colorRight.green());
         telemetry.addData("Blue ", colorRight.blue());
-        int bluenumber = colorRight.blue();
+        //telemetry.update();
+        int bluenumber = colorRight.red();
         return bluenumber;
     }
 
@@ -191,8 +192,8 @@ public class Autonomoustest extends LinearOpMode {
         telemetry.addData("Red left ", colorLeft.red());
         telemetry.addData("Green left", colorLeft.green());
         telemetry.addData("Blue left", colorLeft.blue());
-
+        //telemetry.update();
-        int bluenumber = colorLeft.blue();
+        int bluenumber = colorLeft.red();
         return bluenumber;
 
 
@@ -255,35 +256,36 @@ public class Autonomoustest extends LinearOpMode {
             driveForward(28);
             int blueleft = readColorLeft();
             int blueright = readColorRight();
-           if (blueleft > 75)
+            double backboard = 29;
+           if (blueleft > 100)
            {
                //telemetry.addData("color sensor","left");
                if(blueleft > blueright)
                    telemetry.addData("color sensor","left");
                turnLeft(90);
                straightLeft(2);
-               driveForward(11);
+               driveForward(8);
-               raisearm(26);
+               raisearm(30);
                arm.setPower(0);
-               driveForward(-26);
+               driveForward(-23);
                straightLeft(32);
                turnLeft(10);
                driveForward(18);
                driveForward(-31);
-               double backboard = 34;
+               straightRight(34);
-               straightRight(backboard);
                raisearm(80);
+               wrist.setPosition(0);
+               raisearm(100);
                driveForward(-1);
-               gripper.setPosition(0);
+               gripper.setPosition(0.25);
                terminateOpModeNow();
 
 
 
 
            }
-           if (blueright > 75)
+           if (blueright > 100)
            {
-               //telemetry.addData("color sensor", "right");
                if(blueleft < blueright)
                    telemetry.addData("color sensor","right");
                straightRight(8);
@@ -294,32 +296,34 @@ public class Autonomoustest extends LinearOpMode {
                straightLeft(15);
                driveForward(8);
                driveForward(-26);
-               double backboard = 22.5;
+               straightRight(22.5);
-               straightRight(backboard);
                raisearm(80);
+               wrist.setPosition(0);
+               raisearm(100);
                driveForward(-5);
-               gripper.setPosition(1);
+               gripper.setPosition(.25);
                terminateOpModeNow();
 
 
            }
            else
                telemetry.addData("position","center");
-           driveForward(3.5);
+           driveForward(4);
            raisearm(30);
             arm.setPower(0);
-            driveForward(-3.5);
+            driveForward(-6);
             straightRight(11.5);
             driveForward(-17);
             turnLeft(90);
             straightLeft(15);
             driveForward(8);
             driveForward(-26);
-            double backboard = 29;
+            straightRight(29);
-            straightRight(backboard);
             raisearm(80);
+            wrist.setPosition(0);
+            raisearm(100);
             driveForward(-5);
-            gripper.setPosition(1);
+            gripper.setPosition(.25);
            telemetry.update();
             sleep(250);
 

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

@@ -0,0 +1,467 @@
+/* Copyright (c) 2017 FIRST. 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.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/**
+ * This file illustrates the concept of driving a path based on encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The code REQUIRES that you DO have encoders on the wheels,
+ *   otherwise you would use: RobotAutoDriveByTime;
+ *
+ *  This code ALSO requires that the drive Motors have been configured such that a positive
+ *  power command moves them forward, and causes the encoders to count UP.
+ *
+ *   The desired path in this example is:
+ *   - Drive forward for 48 inches
+ *   - Spin right for 12 Inches
+ *   - Drive Backward for 24 inches
+ *   - Stop and close the claw.
+ *
+ *  The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS)
+ *  that performs the actual movement.
+ *  This method assumes that each movement is relative to the last stopping place.
+ *  There are other ways to perform encoder based moves, but this method is probably the simplest.
+ *  This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
+ *
+ * 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
+ */
+
+@Autonomous(name="blue front", group="Robot")
+//@Disabled
+public class bluefront extends LinearOpMode {
+
+    /* Declare OpMode members. */
+    private DcMotor leftDrive = null;
+    private DcMotor rightDrive = null;
+    private DcMotor backrightDrive = null;
+    private DcMotor backleftDrive = null;
+    private ColorSensor colorRight = null;
+    private ColorSensor colorLeft = null;
+    private Servo wrist = null;
+    private Servo gripper = null;
+    private DcMotor arm = null;
+
+
+    private ElapsedTime runtime = new ElapsedTime();
+
+    // Calculate the COUNTS_PER_INCH for your specific drive train.
+    // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+    // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+    // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+    // This is gearing DOWN for less speed and more torque.
+    // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+    static final double COUNTS_PER_MOTOR_REV = 537.6;    // eg: TETRIX Motor Encoder
+    static final double DRIVE_GEAR_REDUCTION = 1.0;     // No External Gearing.
+    static final double WHEEL_DIAMETER_INCHES = 3.77953;     // For figuring circumference
+    static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+            (WHEEL_DIAMETER_INCHES * Math.PI);
+    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
+    static final double DRIVE_SPEED = 0.2;
+    static final double TURN_SPEED = 0.4;
+
+    static final double LONG_TIMEOUT = 1000;
+    static final double DEGREE_TOO_DISTANCE = 0.21944444444;
+    static final double ARM_SPEED = .1;
+    static final double TICKS_TO_DEGREES = 0.07462686567;
+
+    @Override
+    public void runOpMode()
+    {
+        hardwareinit();
+
+        // Send telemetry message to indicate successful Encoder reset
+       /* telemetry.addData("Starting at",  "%7d :%7d",
+                          leftDrive.getCurrentPosition(),
+                          rightDrive.getCurrentPosition(),
+                backleftDrive.getCurrentPosition(),
+        backrightDrive.getCurrentPosition());*/
+
+        telemetry.update();
+
+        // Wait for the game to start (driver presses PLAY)
+        waitForStart();
+        {
+            executeAuto();
+
+        }
+
+        // Step through each leg of the path,
+        // Note: Reverse movement is obtained by setting a negative distance (not speed)
+
+
+    }
+
+
+//
+
+    public void driveForward(double distance)
+    {
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);  // S1: Forward 47 Inches with 5 Sec timeout
+    }
+
+    public void straightLeft(double distance)
+    {
+        leftDrive.setDirection(DcMotor.Direction.FORWARD);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+    }
+
+    public void straightRight(double distance)
+    {
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backleftDrive.setDirection(DcMotor.Direction.FORWARD);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+    }
+
+    public void turnLeft(double degrees)
+    {
+        leftDrive.setDirection(DcMotor.Direction.FORWARD);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backleftDrive.setDirection(DcMotor.Direction.FORWARD);
+        double turning_distance = degrees * DEGREE_TOO_DISTANCE;
+        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
+    }
+
+    public void turnRight(double degrees) {
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backrightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        double turning_distance = degrees * DEGREE_TOO_DISTANCE;
+        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
+    }
+
+    public int readColorRight() {
+        telemetry.addData("Clear", colorRight.alpha());
+        telemetry.addData("Red  ", colorRight.red());
+        telemetry.addData("Green", colorRight.green());
+        telemetry.addData("Blue ", colorRight.blue());
+        //telemetry.update();
+        int bluenumber = colorRight.blue();
+        return bluenumber;
+    }
+
+    public int readColorLeft() {
+        telemetry.addData("Clear Left", colorLeft.alpha());
+        telemetry.addData("Red left ", colorLeft.red());
+        telemetry.addData("Green left", colorLeft.green());
+        telemetry.addData("Blue left", colorLeft.blue());
+        //telemetry.update();
+        int bluenumber = colorLeft.blue();
+        return bluenumber;
+
+
+    }
+
+    public void raisearm(int degrees) {
+        armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
+
+    }
+    public void hardwareinit()
+    {
+        leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
+        rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
+        backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
+        backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
+        colorRight = hardwareMap.get(ColorSensor.class, "color right");
+        colorLeft = hardwareMap.get(ColorSensor.class, "color left");
+        gripper = hardwareMap.get(Servo.class, "gripper");
+        arm = hardwareMap.get(DcMotor.class, "arm raise");
+        wrist = hardwareMap.get(Servo.class, "wrist");
+        // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+        // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
+        // Note: The settings here assume direct drive on left and right wheels.  Gear Reduction or 90 Deg drives may require direction flips
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        arm.setDirection(DcMotor.Direction.REVERSE);
+
+        leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+
+        leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+    public void testWrist()
+    {
+        wrist.setPosition(0);
+        sleep(3000);
+        wrist.setPosition(1);
+        sleep(3000);
+    }
+    public void testGripper()
+    {
+        gripper.setPosition(0);
+        sleep(3000);
+        gripper.setPosition(1);
+        sleep(3000);
+    }
+    public void executeAuto()
+    {
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveForward(28);
+        int blueleft = readColorLeft();
+        int blueright = readColorRight();
+        double backboard = 29;
+        if (blueleft > 100)
+        {
+            //telemetry.addData("color sensor","left");
+            if(blueleft > blueright)
+                telemetry.addData("color sensor","left");
+
+            straightLeft(8);
+            raisearm(30);
+            arm.setPower(0);
+            driveForward(-13.5);
+            turnRight(90);
+            straightRight(15);
+            driveForward(8);
+            driveForward(-26);
+            straightLeft(22.5);
+            raisearm(80);
+            wrist.setPosition(0);
+            raisearm(100);
+            driveForward(-5);
+            gripper.setPosition(.25);
+            terminateOpModeNow();
+
+
+
+
+        }
+        if (blueright > 100)
+        {
+            if(blueleft < blueright)
+                telemetry.addData("color sensor","right");
+            turnRight(90);
+            straightLeft(2);
+            driveForward(8);
+            raisearm(30);
+            arm.setPower(0);
+            driveForward(-23);
+            straightRight(32);
+            turnRight(10);
+            driveForward(18);
+            driveForward(-31);
+            straightLeft(34);
+            raisearm(80);
+            wrist.setPosition(0);
+            raisearm(100);
+            driveForward(-1);
+            gripper.setPosition(0.25);
+            terminateOpModeNow();
+
+
+        }
+        else
+            telemetry.addData("position","center");
+        driveForward(4);
+        raisearm(30);
+        arm.setPower(0);
+        driveForward(-6);
+        straightLeft(11.5);
+        driveForward(-17);
+        turnRight(90);
+        straightRight(15);
+        driveForward(8);
+        driveForward(-26);
+        straightLeft(29);
+        raisearm(80);
+        wrist.setPosition(0);
+        raisearm(100);
+        driveForward(-5);
+        gripper.setPosition(.25);
+        telemetry.update();
+        sleep(250);
+
+
+
+
+        //Values were created from robot with wheel issues 9/28/23
+
+        telemetry.addData("Path", "Complete");
+        telemetry.update();
+        sleep(1000);  // pause to display final telemetry message.
+    }
+
+
+
+
+
+    /*
+     *  Method to perform a relative move, based on encoder counts.
+     *  Encoders are not reset as the move is based on the current position.
+     *  Move will stop if any of three conditions occur:
+     *  1) Move gets to the desired position
+     *  2) Move runs out of time
+     *  3) Driver stops the opmode running.
+
+     */
+
+    public void encoderDrive(double speed,
+                             double leftInches, double rightInches,
+                             double timeoutS) {
+        int newLeftTarget;
+        int newRightTarget;
+        int newBackLeftTarget;
+        int newbackRightTarget;
+
+
+        if (opModeIsActive()) {
+
+            // Determine new target position, and pass to motor controller
+            newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
+            newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
+            newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
+            newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
+            leftDrive.setTargetPosition(newLeftTarget);
+            rightDrive.setTargetPosition(newRightTarget);
+            backrightDrive.setTargetPosition(newbackRightTarget);
+            backleftDrive.setTargetPosition(newBackLeftTarget);
+
+            // Turn On RUN_TO_POSITION
+            leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            // reset the timeout time and start motion.
+            runtime.reset();
+            leftDrive.setPower(Math.abs(speed));
+            rightDrive.setPower(Math.abs(speed));
+            backrightDrive.setPower(Math.abs(speed));
+            backleftDrive.setPower(Math.abs(speed));
+
+            // keep looping while we are still active, and there is time left, and both motors are running.
+            // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
+            // its target position, the motion will stop.  This is "safer" in the event that the robot will
+            // always end the motion as soon as possible.
+            // However, if you require that BOTH motors have finished their moves before the robot continues
+            // onto the next step, use (isBusy() || isBusy()) in the loop test.
+            while (opModeIsActive() &&
+                    (runtime.seconds() < timeoutS) &&
+                    (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) {
+
+                // Display it for the driver.
+                telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
+                telemetry.addData("Currently at", " at %7d :%7d",
+                        leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition());
+                telemetry.update();
+            }
+
+
+            leftDrive.setPower(0);
+            rightDrive.setPower(0);
+            backrightDrive.setPower(0);
+            backleftDrive.setPower(0);
+
+
+            // Turn off RUN_TO_POSITION
+            leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+            sleep(250);   // optional pause after each move.
+        }
+    }
+
+    public void armEncoder(double speed,
+                           double Inches, double timeoutS) {
+        int newarmTarget;
+
+
+        if (opModeIsActive()) {
+
+            // Determine new target position, and pass to motor controller
+            newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
+            arm.setTargetPosition(newarmTarget);
+
+            // Turn On RUN_TO_POSITION
+            arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            // reset the timeout time and start motion.
+            runtime.reset();
+            arm.setPower(Math.abs(speed));
+
+            // keep looping while we are still active, and there is time left, and both motors are running.
+            // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
+            // its target position, the motion will stop.  This is "safer" in the event that the robot will
+            // always end the motion as soon as possible.
+            // However, if you require that BOTH motors have finished their moves before the robot continues
+            // onto the next step, use (isBusy() || isBusy()) in the loop test.
+            while (opModeIsActive() &&
+                    (runtime.seconds() < timeoutS) &&
+                    (arm.isBusy())) {
+
+                // Display it for the driver.
+                telemetry.addData("Running to", " %7d", newarmTarget);
+                telemetry.addData("Currently at", " at %7d",
+                        arm.getCurrentPosition());
+                telemetry.update();
+            }
+
+
+            arm.setPower(0);
+
+
+            // Turn off RUN_TO_POSITION
+            arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        }
+    }
+}