Merge branch 'branch-cooper' of http://10.37.49.22:3000/SCDS/FtcRobotController into branch-cooper

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);
-        driveForward(28);
+//        {
-        //turnRight(90);
+//            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);
+                leftDrive.setPower(0);
-            backrightDrive.setPower(0);
+                rightDrive.setPower(0);
-            backleftDrive.setPower(0);
+                backrightDrive.setPower(0);
+                backleftDrive.setPower(0);
+
 
             // Turn off RUN_TO_POSITION
             leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);