RedTop.java

/* 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.
 */

 /**
 *  This file is used to control the robot autonomously when on the Red Top,
 *  which is the Red Cryptobox beside the opposing Blue Cryptobox.
 **/

package org.firstinspires.ftc.teamcode;

import android.app.Activity;
import android.graphics.Color;
import android.view.View;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

import java.util.Locale;


@Autonomous(name = "RedTop", group = "Sensor")
                            // Comment this out to add to the opmode list
public class RedTop extends LinearOpMode {


    ColorSensor sensorColor;
    DistanceSensor sensorDistance;

    @Override
    public void runOpMode() {

        // get a reference to the color sensor.
        sensorColor = hardwareMap.get(ColorSensor.class, "sensor_color_distance");

        // get a reference to the distance sensor that shares the same name.
        sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_color_distance");

        // hsvValues is an array that will hold the hue, saturation, and value information.
        float hsvValues[] = {0F, 0F, 0F};

        // values is a reference to the hsvValues array.
        final float values[] = hsvValues;

        // sometimes it helps to multiply the raw RGB values with a scale factor
        // to amplify/attentuate the measured values.
        final double SCALE_FACTOR = 255;

        // get a reference to the RelativeLayout so we can change the background
        // color of the Robot Controller app to match the hue detected by the RGB sensor.
        int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
        final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);



        // wait for the start button to be pressed.
        waitForStart();

        //Create an instance of AutoSenor
        AutoSenor autoSenor = new AutoSenor();

        //Create elapsed time Object
        ElapsedTime timer = new ElapsedTime();

        // loop and read the RGB and distance data.
        // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
        while (opModeIsActive()) {
            // convert the RGB values to HSV values.
            // multiply by the SCALE_FACTOR.
            // then cast it back to int (SCALE_FACTOR is a double)
            Color.RGBToHSV((int) (sensorColor.red() * SCALE_FACTOR),
                    (int) (sensorColor.green() * SCALE_FACTOR),
                    (int) (sensorColor.blue() * SCALE_FACTOR),
                    hsvValues);

            // send the info back to driver station using telemetry function.
            telemetry.addData("Distance (cm)",
                    String.format(Locale.US, "%.02f", sensorDistance.getDistance(DistanceUnit.CM)));
            telemetry.addData("Alpha", sensorColor.alpha());
            telemetry.addData("Red  ", sensorColor.red());
            telemetry.addData("Green", sensorColor.green());
            telemetry.addData("Blue ", sensorColor.blue());
            telemetry.addData("Hue", hsvValues[0]);

            // change the background color to match the color detected by the RGB sensor.
            // pass a reference to the hue, saturation, and value array as an argument
            // to the HSVToColor method.
            relativeLayout.post(new Runnable() {
                public void run() {
                    relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
                }
            });

            telemetry.update();

        autoSenor.frontRight = hardwareMap.get(DcMotor.class, "frontRight");
        autoSenor.backRight = hardwareMap.get(DcMotor.class, "backRight");
        autoSenor.backLeft = hardwareMap.get(DcMotor.class, "backLeft");
        autoSenor.frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
        autoSenor.rightClaw = hardwareMap.get(Servo.class, "rightClaw");
        autoSenor.leftClaw = hardwareMap.get(Servo.class, "leftClaw");
        autoSenor.rightArm = hardwareMap.get(Servo.class, "rightArm");
        autoSenor.leftArm = hardwareMap.get(Servo.class, "leftArm");
        autoSenor.jewelArmHorizontal = hardwareMap.get(Servo.class, "jewelArmHorizontal");
        autoSenor.jewelArmVertical = hardwareMap.get(Servo.class, "jewelArmVertical");

        //Raises the arm to the center position.
        while(timer.time() < 1){
        autoSenor.jewelArmUp();
        autoSenor.jewelArmCenter();
        }

        //Places the sensor in between the jewels.
        while((timer.time() > 2) && (timer.time() < 3)){
            autoSenor.jewelArmDown();
        }

        //Decides which jewel needs to be removed and removes it.
        while((timer.time() > 4) && (timer.time() < 5)) {
            if((hsvValues[0] < 210) && (hsvValues[0] > 190)){
                autoSenor.jewelArmLeft();
            }else{
                autoSenor.jewelArmRight();
            }
        }

        while((timer.time() > 5) && (timer.time() < 6)) {
            autoSenor.grab();
            autoSenor.jewelArmUp();
            autoSenor.jewelArmCenter();
        }

        while((timer.time() > 7) && (timer.time() < 8)) {
            autoSenor.liftArm();
        }

        while((timer.time() > 9) && (timer.time() < 9.75)) {
            autoSenor.driveForward();
        }

        while((timer.time() > 11) && (timer.time() < 11.32)) {
            autoSenor.rotateRight();
        }

        while((timer.time() > 13) && (timer.time() < 14.30)) {
            autoSenor.driveForward();
            autoSenor.lowerArm();
        }

        while((timer.time() > 15) && (timer.time() < 16)) {
            autoSenor.release();
        }

        while((timer.time() > 17) && (timer.time() < 17.15)) {
            autoSenor.driveBackward();
        }

        while((timer.time() > 18) && (timer.time() < 18.50)) {
            autoSenor.rotateLeft();
        }
        autoSenor.brake();


        // Set the panel back to the default color
        relativeLayout.post(new Runnable() {
            public void run() {
                relativeLayout.setBackgroundColor(Color.WHITE);
            }
        });




        }


    }
}