diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java index b092fd5..6212476 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -31,7 +31,10 @@ 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; /** @@ -69,6 +72,12 @@ public class Autonomoustest extends LinearOpMode { 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; + @@ -92,11 +101,16 @@ public class Autonomoustest extends LinearOpMode { static final double DEGREE_TOO_DISTANCE = 0.21944444444; @Override public void runOpMode() { - // Initialize the drive system variables. + // Initialize the drive system variables 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 @@ -104,23 +118,26 @@ public class Autonomoustest extends LinearOpMode { 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); // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Starting at", "%7d :%7d", + /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition(), - backrightDrive.getCurrentPosition()); + backrightDrive.getCurrentPosition());*/ telemetry.update(); @@ -129,10 +146,67 @@ public class Autonomoustest extends LinearOpMode { // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) + raisearm(1); + arm.setPower(0.001); + sleep(10000); //driveForward(2); - //driveForward(80); - driveForward(28); - //turnRight(90); +// driveForward(28); +// { +// int blueleft = readColorLeft(); +// int blueright = readColorRight(); +// if (blueleft > 75) +// { +// //telemetry.addData("color sensor","left"); +// if(blueleft > blueright) +// telemetry.addData("color sensor","left"); +// turnLeft(90); +// straightLeft(2); +// driveForward(11); +// driveForward(-30); +// turnLeft(180); +// driveForward(19); +// straightLeft(7); +// //use arm to place pixel +// straightRight(34); +// driveForward(11); +// terminateOpModeNow(); +// +// +// +// +// } +// if (blueright > 75) +// { +// //telemetry.addData("color sensor", "right"); +// if(blueleft < blueright) +// telemetry.addData("color sensor","right"); +// straightRight(11); +// driveForward(-17); +// turnRight(90); +// straightLeft(8); +// driveForward(28.5); +// //use arm to place pixel +// straightRight(20); +// driveForward(11); +// terminateOpModeNow(); +// +// +// } +// else +// telemetry.addData("position","center"); +// driveForward(11); +// driveForward(-17); +// turnRight(90); +// driveForward(39); +// straightRight(3.5); +// //use arm to place pixel +// straightRight(23.5); +// driveForward(11); +// +// telemetry.update(); +// sleep(250); +// +// } @@ -185,6 +259,36 @@ public class Autonomoustest extends LinearOpMode { 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()); + 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()); + + int bluenumber = colorLeft.blue(); + return bluenumber; + + + + } + + public void raisearm(long seconds) + { + arm.setPower(.1); + sleep(seconds * 1000); + } + + /* * Method to perform a relative move, based on encoder counts. @@ -193,7 +297,9 @@ public class Autonomoustest extends LinearOpMode { * 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) { @@ -202,7 +308,6 @@ public class Autonomoustest extends LinearOpMode { int newBackLeftTarget; int newbackRightTarget; - // Ensure that the opmode is still active if (opModeIsActive()) { // Determine new target position, and pass to motor controller @@ -245,11 +350,13 @@ public class Autonomoustest extends LinearOpMode { telemetry.update(); } - // Stop all motion; - leftDrive.setPower(0); - rightDrive.setPower(0); - backrightDrive.setPower(0); - backleftDrive.setPower(0); + + + leftDrive.setPower(0); + rightDrive.setPower(0); + backrightDrive.setPower(0); + backleftDrive.setPower(0); + // Turn off RUN_TO_POSITION leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);