Merge branch 'branch-cooper' of http://10.37.49.22:3000/SCDS/FtcRobotController into branch-cooper
This commit is contained in:
@ -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);
|
||||
|
||||
Reference in New Issue
Block a user