SCDS/FtcRobotController
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

All autos made and backstage area conistantly getting 45 points, other than Red (Backstage) all untested.

Browse Source
This commit is contained in:
robotics2
2023-11-27 11:34:59 -08:00
parent a6ea0fc529
commit 0d131c1b1e
8 changed files with 980 additions and 438 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java
View File

@ -29,6 +29,8 @@
package org.firstinspires.ftc.teamcode; package org.firstinspires.ftc.teamcode;
import android.annotation.SuppressLint;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.ColorSensor;
@ -66,7 +68,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/ */
@Autonomous(name="red front", group="Robot") @Autonomous(name="red (backstage)", group="Robot")
//@Disabled //@Disabled
public class Autonomoustest extends LinearOpMode { public class Autonomoustest extends LinearOpMode {
@ -109,8 +111,6 @@ public class Autonomoustest extends LinearOpMode {
public void runOpMode() public void runOpMode()
{ {
hardwareinit(); hardwareinit();
gripper.setPosition(1);
sleep(1000);
// Send telemetry message to indicate successful Encoder reset // Send telemetry message to indicate successful Encoder reset
/* telemetry.addData("Starting at", "%7d :%7d", /* telemetry.addData("Starting at", "%7d :%7d",
@ -236,68 +236,83 @@ public class Autonomoustest extends LinearOpMode {
gripper.setPosition(0.5); gripper.setPosition(0.5);
} }
@SuppressLint("SuspiciousIndentation")
public void executeAuto() public void executeAuto()
{ {
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26); driveForward(26);
sleep(500);
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.CM); int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
int distanceright = (int)distanceRight.getDistance(DistanceUnit.CM); int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
telemetry.addData("color left sensor",distanceleft); telemetry.addData("color left sensor",distanceleft);
telemetry.addData("color right sensor",distanceright); telemetry.addData("color right sensor",distanceright);
telemetry.update(); telemetry.update();
sleep(500);
if (distanceleft < 30) if (distanceleft < 7)
{ {
//telemetry.addData("color sensor","left"); telemetry.addData("postion","left");
telemetry.update();
turnLeft(90); turnLeft(90);
straightLeft(2); straightLeft(2);
driveForward(6.5); driveForward(6.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-23); driveForward(-21);
straightLeft(32); straightLeft(32);
turnLeft(10);
driveForward(18); driveForward(18);
driveForward(-40); driveForward(-28);
// straightRight(31.5); straightRight(33);
// raisearm(80); driveForward(-1.5);
// wrist.setPosition(0); raisearm(80);
// raisearm(100); wrist.setPosition(0);
// driveForward(-1); raisearm(100);
// gripper.setPosition(0.25); gripper.setPosition(0);
sleep(500);
driveForward(5);
raisearm(-270);
raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow(); terminateOpModeNow();
} }
if (distanceright < 30) if (distanceright < 7)
{ {
straightRight(13.5); telemetry.addData("postion", "right");
telemetry.update();
straightRight(12);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-15.5); driveForward(-15.5);
turnLeft(90); turnLeft(90);
straightLeft(15); straightLeft(15);
driveForward(8); driveForward(8);
driveForward(-38); driveForward(-28.5);
// straightRight(14.5); straightRight(19);
// raisearm(80); driveForward(-1.5);
// wrist.setPosition(0); raisearm(80);
// raisearm(100); wrist.setPosition(0);
// gripper.setPosition(.25); raisearm(100);
// driveForward(5); gripper.setPosition(0);
// raisearm(-200); sleep(500);
driveForward(5);
raisearm(-270);
raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow(); terminateOpModeNow();
} }
else
driveForward(6.5); telemetry.addData("postion","center");
telemetry.update();
driveForward(3.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-8); driveForward(-8);
@ -312,13 +327,14 @@ public class Autonomoustest extends LinearOpMode {
raisearm(80); raisearm(80);
wrist.setPosition(0); wrist.setPosition(0);
raisearm(100); raisearm(100);
gripper.setPosition(.25); gripper.setPosition(0);
sleep(500); sleep(500);
driveForward(5); driveForward(5);
raisearm(-270); raisearm(-270);
telemetry.update(); raisearm(50);
sleep(250); wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow();
@ -326,7 +342,7 @@ public class Autonomoustest extends LinearOpMode {
telemetry.addData("Path", "Complete"); telemetry.addData("Path", "Complete");
telemetry.update(); telemetry.update();
sleep(1000); // pause to display final telemetry message. // sleep(1000); // pause to display final telemetry message.
} }

443
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java Normal file
View File

@ -0,0 +1,443 @@
/* 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 android.annotation.SuppressLint;
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.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
/**
* 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", group="Robot")
//@Disabled
public class Blue extends LinearOpMode {
/* Declare OpMode members. */
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
private DcMotor backrightDrive = null;
private DcMotor backleftDrive = null;
private DistanceSensor distanceRight = null;
private DistanceSensor distanceLeft = null;
private Servo wrist = null;
private Servo gripper = null;
private DcMotor arm = null;
private DistanceSensor distance = 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.3;
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 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");
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class, "arm raise");
wrist = hardwareMap.get(Servo.class, "wrist");
distance = hardwareMap.get(DistanceSensor.class, "distance");
wrist.setPosition(1);
sleep(1000);
// 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.5);
}
@SuppressLint("SuspiciousIndentation")
public void executeAuto()
{
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26);
sleep(500);
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
telemetry.addData("color left sensor",distanceleft);
telemetry.addData("color right sensor",distanceright);
telemetry.update();
if (distanceleft < 7)
{
telemetry.addData("postion","left");
telemetry.update();
turnLeft(90);
straightLeft(2);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
terminateOpModeNow();
}
if (distanceright < 7)
{
telemetry.addData("postion", "right");
telemetry.update();
straightRight(12);
raisearm(80);
arm.setPower(0);
driveForward(-10);
terminateOpModeNow();
}
else
telemetry.addData("postion","center");
telemetry.update();
driveForward(3.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
straightRight(11.5);
driveForward(-15);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-26);
straightRight(29);
driveForward(-1.5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(0);
sleep(500);
driveForward(5);
raisearm(-270);
raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow();
//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) (leftInches * 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);
}
}
}

51
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Motor_Test.java
View File

@ -1,51 +0,0 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import java.util.concurrent.TimeUnit;
@Autonomous(name="Motor_test")
public class Motor_Test extends OpMode {
DcMotor hwMotorDriveFrontLeft;
DcMotor hwMotorDriveFrontRight;
DcMotor hwMotorDriveBackLeft;
DcMotor hwMotorDriveBackRight;
public void init() {
hwMotorDriveFrontLeft = hardwareMap.dcMotor.get("Drive front lt");
hwMotorDriveFrontRight = hardwareMap.dcMotor.get("Drive front rt");
hwMotorDriveBackLeft = hardwareMap.dcMotor.get("Drive back lt");
hwMotorDriveBackRight = hardwareMap.dcMotor.get("Drive back rt");
}
public void sleepSec(int iSecs){
try {
Thread.sleep(iSecs*1000);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
public void loop() {
hwMotorDriveFrontLeft.setPower(1);
sleepSec(1);
hwMotorDriveFrontRight.setPower(1);
sleepSec(1);
hwMotorDriveBackLeft.setPower(1);
sleepSec(1);
hwMotorDriveBackRight.setPower(1);
sleepSec(1);
hwMotorDriveFrontLeft.setPower(0);
hwMotorDriveFrontRight.setPower(0);
hwMotorDriveBackLeft.setPower(0);
hwMotorDriveBackRight.setPower(0);
sleepSec(10);
}
}

417
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java Normal file
View File

@ -0,0 +1,417 @@
/* 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.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
/**
* 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="Red", group="Robot")
//@Disabled
public class Red extends LinearOpMode {
/* Declare OpMode members. */
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
private DcMotor backrightDrive = null;
private DcMotor backleftDrive = null;
private DistanceSensor distanceRight = null;
private DistanceSensor distanceLeft = 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.3;
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 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");
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class, "arm raise");
wrist = hardwareMap.get(Servo.class, "wrist");
wrist.setPosition(1);
sleep(1000);
// 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(26);
sleep(500);
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
if (distanceleft < 7)
{
telemetry.addData("position", "left");
telemetry.update();
straightLeft(12);
raisearm(80);
arm.setPower(0);
driveForward(-10);
terminateOpModeNow();
}
if (distanceright < 7)
{
telemetry.addData("position","right");
telemetry.update();
turnRight(90);
straightLeft(2);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-10);
terminateOpModeNow();
}
else
telemetry.addData("position","center");
telemetry.update();
driveForward(3.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
terminateOpModeNow();
//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);
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java
View File

@ -129,7 +129,7 @@ public class arm extends OpMode {
} }
if(gamepad2.left_trigger > 0.35) if(gamepad2.left_trigger > 0.35)
{ {
gripper.setPosition(0.5); gripper.setPosition(0);
} }
if(gamepad2.right_trigger > 0.35){ if(gamepad2.right_trigger > 0.35){
gripper.setPosition(1); gripper.setPosition(1);

111
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java
View File

@ -34,9 +34,12 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
/** /**
* This file illustrates the concept of driving a path based on encoder counts. * This file illustrates the concept of driving a path based on encoder counts.
* The code is structured as a LinearOpMode * The code is structured as a LinearOpMode
@ -63,7 +66,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/ */
@Autonomous(name="blue front", group="Robot") @Autonomous(name="Blue (Backstage)", group="Robot")
//@Disabled //@Disabled
public class bluefront extends LinearOpMode { public class bluefront extends LinearOpMode {
@ -72,8 +75,8 @@ public class bluefront extends LinearOpMode {
private DcMotor rightDrive = null; private DcMotor rightDrive = null;
private DcMotor backrightDrive = null; private DcMotor backrightDrive = null;
private DcMotor backleftDrive = null; private DcMotor backleftDrive = null;
private ColorSensor colorRight = null; private DistanceSensor distanceRight = null;
private ColorSensor colorLeft = null; private DistanceSensor distanceLeft = null;
private Servo wrist = null; private Servo wrist = null;
private Servo gripper = null; private Servo gripper = null;
private DcMotor arm = null; private DcMotor arm = null;
@ -177,27 +180,6 @@ public class bluefront extends LinearOpMode {
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); 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) { public void raisearm(int degrees) {
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT); armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
@ -209,8 +191,8 @@ public class bluefront extends LinearOpMode {
rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt"); rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
colorRight = hardwareMap.get(ColorSensor.class, "color right"); distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left"); distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
gripper = hardwareMap.get(Servo.class, "gripper"); gripper = hardwareMap.get(Servo.class, "gripper");
arm = hardwareMap.get(DcMotor.class, "arm raise"); arm = hardwareMap.get(DcMotor.class, "arm raise");
wrist = hardwareMap.get(Servo.class, "wrist"); wrist = hardwareMap.get(Servo.class, "wrist");
@ -256,62 +238,70 @@ public class bluefront extends LinearOpMode {
{ {
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26); driveForward(26);
int blueleft = readColorLeft(); sleep(500);
int blueright = readColorRight(); int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
double backboard = 29; int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
if (blueleft > 75) if (distanceleft < 7)
{ {
//telemetry.addData("color sensor","left"); telemetry.addData("position", "left");
if(blueleft > blueright) telemetry.update();
telemetry.addData("color sensor","left"); straightLeft(12);
straightLeft(13.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-15.5); driveForward(-15.5);
turnRight(90); turnRight(90);
straightRight(15); straightRight(15);
driveForward(8); driveForward(8);
driveForward(-38); driveForward(-28.5);
// straightLeft(22.5); straightLeft(19);
// raisearm(80); raisearm(80);
// wrist.setPosition(0); wrist.setPosition(0);
// raisearm(100); raisearm(100);
// driveForward(-5); gripper.setPosition(0);
// gripper.setPosition(.25); sleep(500);
driveForward(5);
raisearm(-270);
raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow(); terminateOpModeNow();
} }
if (blueright > 75) if (distanceright < 7)
{ {
if(blueleft < blueright) telemetry.addData("position","right");
telemetry.addData("color sensor","right"); telemetry.update();
turnRight(90); turnRight(90);
straightLeft(2); straightLeft(2);
driveForward(6.5); driveForward(6.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-23); driveForward(-21);
straightRight(32); straightRight(32);
turnRight(10);
driveForward(18); driveForward(18);
driveForward(-40); driveForward(-28);
// straightLeft(34); straightLeft(33);
// raisearm(80); raisearm(80);
// wrist.setPosition(0); wrist.setPosition(0);
// raisearm(100); raisearm(100);
// driveForward(-1); gripper.setPosition(0);
// gripper.setPosition(0.25); sleep(500);
// terminateOpModeNow(); driveForward(5);
raisearm(-270);
raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow();
} }
else else
telemetry.addData("position","center"); telemetry.addData("position","center");
driveForward(6.5); telemetry.update();
driveForward(3.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-8); driveForward(-8);
@ -325,11 +315,14 @@ public class bluefront extends LinearOpMode {
raisearm(80); raisearm(80);
wrist.setPosition(0); wrist.setPosition(0);
raisearm(100); raisearm(100);
gripper.setPosition(.25); gripper.setPosition(0);
sleep(500); sleep(500);
driveForward(5); driveForward(5);
telemetry.update(); raisearm(-270);
sleep(250); raisearm(50);
wrist.setPosition(1);
driveForward(-5);
terminateOpModeNow();

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorRead.java
View File

@ -1,93 +0,0 @@
/* 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;
@Autonomous(name="Robot: colorRead", group="Robot")
//@Disabled
public class colorRead extends LinearOpMode {
/* Declare OpMode members. */
private ColorSensor colorRight = null;
private ColorSensor colorLeft = null;
// 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.
@Override
public void runOpMode() {
hardwareinit();
readColorRight();
readColorLeft();
}
//
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 hardwareinit() {
colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left");
}
}

183
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java
View File

@ -1,183 +0,0 @@
/* 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="color", group="Robot")
//@Disabled
public class colorsense extends LinearOpMode {
/* Declare OpMode members. */
private ColorSensor colorRight = null;
private ColorSensor colorLeft = 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 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.red();
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.red();
return bluenumber;
}
public void hardwareinit()
{
colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "left color");
}
public void executeAuto()
{
while(opModeIsActive())
{
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();
}
}
/*
* 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.
*/
}