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

Updated code allows corner parking

Browse Source
This commit is contained in:
robotics3
2024-01-30 15:53:16 -08:00
parent 913544f4fd
commit 4855f69efc
1 changed files with 478 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStageClone.java Normal file
View File

@ -0,0 +1,478 @@
/* 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.DcMotor;
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 (Backstage) Clone", group="Robot")
//@Disabled
public class BlueBackStageClone 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");
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(5.5);
raisearm(80);
arm.setPower(0);
driveForward(-21);
straightLeft(34);
driveForward(-10);
straightRight(35);
driveForward(-5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
sleep(500);
driveForward(4.5);
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
/* need to review */
straightLeft(11);
driveForward(1.5);
terminateOpModeNow();
}
if (distanceright < 7)
{
telemetry.addData("postion", "right");
telemetry.update();
straightRight(12);
raisearm(80);
arm.setPower(0);
driveForward(-15.5);
turnLeft(90);
straightLeft(15);
driveForward(-20.5);
straightRight(19);
driveForward(-1.5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
driveForward(8.5);
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
/* need to review */
straightLeft(29);
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(-18);
straightRight(29);
turnRight(10);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
driveForward(5);
/* added from bluebackstage */
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
straightLeft(19);
driveForward(-10);
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);
}
}
}