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

starighten added, center added, tweakes too both backstage code made but undtestested for some

Browse Source
This commit is contained in:
robotics2
2024-02-15 20:17:22 -08:00
parent 4dc9b182a7
commit a27996882b
4 changed files with 360 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java
View File

@ -100,6 +100,8 @@ public class Blue extends LinearOpMode {
(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 DRIVE_SPEED_SLOW = 0.2;
static final double TURN_SPEED = 0.4;
static final double LONG_TIMEOUT = 1000;
@ -143,7 +145,7 @@ public class Blue extends LinearOpMode {
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
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false); // S1: Forward 47 Inches with 5 Sec timeout
}
public void straightLeft(double distance)
@ -152,16 +154,28 @@ public class Blue extends LinearOpMode {
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
}
public void straightLeftOnPower(double speed) {
leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setPower(speed * 1.05);
rightDrive.setPower(speed * 1.05);
backrightDrive.setPower(speed);
backleftDrive.setPower(speed);
}
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);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
}
public void turnLeft(double degrees)
@ -171,7 +185,7 @@ public class Blue extends LinearOpMode {
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);
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
}
public void turnRight(double degrees) {
@ -180,9 +194,36 @@ public class Blue extends LinearOpMode {
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);
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
}
public void straighten(Double distance)
{
driveForward(0);
double D1 = distanceLeft.getDistance(DistanceUnit.INCH);
driveForward(distance);
double D2 = distanceLeft.getDistance(DistanceUnit.INCH);
double rad = Math.atan2(D1 - D2, distance);
double degrees = Math.toDegrees(rad);
turnRight(degrees);
telemetry.addData("d1", D1);
telemetry.addData("d2", D2);
telemetry.addData("Calibration deg", degrees);
telemetry.update();
sleep(1000);
}
public void centerLeft()
{
double leftDistance = distanceLeft.getDistance(DistanceUnit.INCH);
straightLeft(leftDistance - 3);
telemetry.addData("leftDistance",leftDistance);
telemetry.addData("moving left x inches",leftDistance - 3);
telemetry.update();
straightLeft(0.0);
sleep(1000);
}
public void raisearm(int degrees) {
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
@ -238,61 +279,109 @@ public class Blue extends LinearOpMode {
@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)
while (true)
{
telemetry.addData("postion","left");
int distright = (int)distanceRight.getDistance(DistanceUnit.INCH);
telemetry.addData("right dist", distright);
telemetry.update();
turnLeft(90);
straightLeft(2);
driveForward(6.5);
raisearm(45);
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();
// 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();
}
else
telemetry.addData("postion","center");
telemetry.update();
driveForward(3.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
terminateOpModeNow();
// if (distanceleft < 7)
// {
// telemetry.addData("postion","left");
// telemetry.update();
// turnLeft(90);
// straightLeft(2);
// driveForward(6.5);
// raisearm(45);
// arm.setPower(0);
// sleep(500);
// driveForward(-20);
//
// do {
// straightLeftOnPower(DRIVE_SPEED_SLOW);
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
//
// } while (distanceleft >= 4);
// straighten(12.0);
// centerLeft();
// driveForward(88);
// sleep(1000);
// wrist.setPosition(.465);
// gripper.setPosition(1);
// sleep(1000);
// driveForward(-3);
// terminateOpModeNow();
//
//
//
//
// }
// if (distanceright < 7)
// {
// telemetry.addData("postion", "right");
// telemetry.update();
// straightRight(12);
// raisearm(80);
// arm.setPower(0);
// driveForward(-22);
// turnLeft(90);
// do {
// straightLeftOnPower(DRIVE_SPEED_SLOW);
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
//
// } while (distanceleft >= 6);
// straighten(12.0);
// centerLeft();
// driveForward(98);
// sleep(1000);
// wrist.setPosition(.465);
// gripper.setPosition(1);
// sleep(1000);
// driveForward(-3);
// terminateOpModeNow();
//
//
// }
// else
// telemetry.addData("postion","center");
// telemetry.update();
// driveForward(3.5);
// raisearm(80);
// arm.setPower(0);
// driveForward(-8);
// turnLeft(90);
// driveForward(-12);
// do {
// straightLeftOnPower(DRIVE_SPEED_SLOW);
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
//
// } while (distanceleft >= 6);
// straighten(12.0);
// centerLeft();
// driveForward(88);
// sleep(1000);
// wrist.setPosition(.465);
// gripper.setPosition(1);
// sleep(1000);
// driveForward(-3);
// 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.
// pause to display final telemetry message.
}
@ -311,7 +400,7 @@ public class Blue extends LinearOpMode {
public void encoderDrive(double speed,
double leftInches, double rightInches,
double timeoutS) {
double timeoutS, boolean addJuice) {
int newLeftTarget;
int newRightTarget;
int newBackLeftTarget;
@ -338,8 +427,13 @@ public class Blue extends LinearOpMode {
// reset the timeout time and start motion.
runtime.reset();
leftDrive.setPower(Math.abs(speed));
rightDrive.setPower(Math.abs(speed));
if(addJuice) {
leftDrive.setPower(Math.abs(speed * 1.05));
rightDrive.setPower(Math.abs(speed * 1.05));
} else {
leftDrive.setPower(Math.abs(speed));
rightDrive.setPower(Math.abs(speed));
}
backrightDrive.setPower(Math.abs(speed));
backleftDrive.setPower(Math.abs(speed));

169
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java
View File

@ -94,7 +94,9 @@ public class BlueBackStage extends LinearOpMode {
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;
double DRIVE_SPEED = 0.5;
static final double DRIVE_SPEED_SLOW = .25;
static final double TURN_SPEED = 0.4;
static final double LONG_TIMEOUT = 1000;
@ -103,8 +105,7 @@ public class BlueBackStage extends LinearOpMode {
static final double TICKS_TO_DEGREES = 0.07462686567;
@Override
public void runOpMode()
{
public void runOpMode() {
hardwareinit();
// Send telemetry message to indicate successful Encoder reset
@ -129,63 +130,56 @@ public class BlueBackStage extends LinearOpMode {
}
//
public void driveForward(double distance)
{
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
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false); // S1: Forward 47 Inches with 5 Sec timeout
}
public void straightLeft(double distance)
{
public void driveForwardSpeed(double distance, double speed) {
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(speed, distance, distance, LONG_TIMEOUT, false); // 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);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
}
public void straightRight(double distance)
{
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);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
}
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)
{
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);
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
}
public void raisearm(int degrees) {
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
armEncoder(ARM_SPEED, degrees * TICKS_TO_DEGREES, LONG_TIMEOUT);
}
public void hardwareinit()
{
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");
@ -204,6 +198,10 @@ public class BlueBackStage extends LinearOpMode {
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
arm.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@ -218,30 +216,28 @@ public class BlueBackStage extends LinearOpMode {
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
public void testWrist()
{
public void testWrist() {
wrist.setPosition(0);
sleep(3000);
wrist.setPosition(1);
sleep(3000);
}
public void testGripper()
{
public void testGripper() {
gripper.setPosition(0);
sleep(3000);
gripper.setPosition(1);
sleep(3000);
}
public void executeAuto()
{
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)
{
int distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
int distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
if (distanceleft < 7) {
telemetry.addData("position", "left");
telemetry.update();
straightLeft(13.5);
@ -251,7 +247,8 @@ public class BlueBackStage extends LinearOpMode {
turnRight(90);
straightRight(15);
driveForward(-18);
straightLeft(20.25);
DRIVE_SPEED = .25;
straightLeft(22.25);
driveForward(-.30);
raisearm(80);
wrist.setPosition(0);
@ -262,17 +259,14 @@ public class BlueBackStage extends LinearOpMode {
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
straightLeft(29);
driveForward(-10);
straightLeft(35);
driveForward(-22);
terminateOpModeNow();
}
if (distanceright < 7)
{
telemetry.addData("position","right");
if (distanceright < 7) {
telemetry.addData("position", "right");
telemetry.update();
turnRight(88);
driveForward(5.25);
@ -293,34 +287,33 @@ public class BlueBackStage extends LinearOpMode {
terminateOpModeNow();
} else {
telemetry.addData("position", "center");
telemetry.update();
driveForward(5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
straightLeft(11.5);
driveForward(-15);
turnRight(90);
straightRight(15);
driveForward(-18);
DRIVE_SPEED = .25;
straightLeft(29);
driveForward(-2);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
driveForward(6);
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
straightLeft(28);
driveForward(-25);
terminateOpModeNow();
}
else
telemetry.addData("position","center");
telemetry.update();
driveForward(3.5);
raisearm(80);;
arm.setPower(0);
driveForward(-8);
straightLeft(11.5);
driveForward(-15);
turnRight(90);
straightRight(15);
driveForward(-18);
straightLeft(29);
driveForward(-2);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
driveForward(6);
raisearm(-50);
wrist.setPosition(1);
raisearm(-70);
straightLeft(19);
driveForward(-10);
terminateOpModeNow();
//Values were created from robot with wheel issues 9/28/23
@ -343,16 +336,28 @@ public class BlueBackStage extends LinearOpMode {
* 3) Driver stops the opmode running.
*/
// public void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean addJuice) {
//
// if(leftInches < 4) {
// encodedDriver(speed, leftInches, rightInches, timeout, addJuice);
//
// } else {
// // first n-4 is 50% speed (regional tournament speed)
// encodedDriver(DRIVE_SPEED, leftInches-10, rightInches-4, timeout, addJuice);
// // trailing result is 30% speed (base speed)
// encodedDriver(.3, 10, 10, timeout, addJuice);
//
// }
//}
public void encoderDrive(double speed,
double leftInches, double rightInches,
double timeoutS) {
double timeoutS, boolean addJuice) {
int newLeftTarget;
int newRightTarget;
int newBackLeftTarget;
int newbackRightTarget;
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller
@ -373,8 +378,13 @@ public class BlueBackStage extends LinearOpMode {
// reset the timeout time and start motion.
runtime.reset();
leftDrive.setPower(Math.abs(speed));
rightDrive.setPower(Math.abs(speed));
if(addJuice) {
leftDrive.setPower(Math.abs(speed * 1.05));
rightDrive.setPower(Math.abs(speed * 1.05));
} else {
leftDrive.setPower(Math.abs(speed));
rightDrive.setPower(Math.abs(speed));
}
backrightDrive.setPower(Math.abs(speed));
backleftDrive.setPower(Math.abs(speed));
@ -395,7 +405,6 @@ public class BlueBackStage extends LinearOpMode {
telemetry.update();
}
leftDrive.setPower(0);
rightDrive.setPower(0);
backrightDrive.setPower(0);
@ -408,7 +417,7 @@ public class BlueBackStage extends LinearOpMode {
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
sleep(250); // optional pause after each move.
// sleep(250); // optional pause after each move.
}
}

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java
View File

@ -97,6 +97,8 @@ public class Red extends LinearOpMode {
(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 DRIVE_SPEED_SLOW = 0.25;
static final double TURN_SPEED = 0.4;
static final double LONG_TIMEOUT = 1000;
@ -219,6 +221,47 @@ public class Red extends LinearOpMode {
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
public void straightRightOnPower(double speed) {
speed *= -1;
leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setPower(speed * 1.05);
rightDrive.setPower(speed * 1.05);
backrightDrive.setPower(speed);
backleftDrive.setPower(speed);
}
public void straighten(Double distance)
{
driveForward(0);
double D1 = distanceRight.getDistance(DistanceUnit.INCH);
driveForward(distance);
double D2 = distanceRight.getDistance(DistanceUnit.INCH);
double rad = Math.atan2(D1 - D2, distance);
double degrees = Math.toDegrees(rad);
turnRight(-degrees);
telemetry.addData("d1", D1);
telemetry.addData("d2", D2);
telemetry.addData("Calibration deg", degrees);
telemetry.update();
sleep(3000);
}
public void centerRight()
{
double rightDistance = distanceRight.getDistance(DistanceUnit.INCH);
straightRight(rightDistance - 3);
telemetry.addData("rightDistance",rightDistance);
telemetry.addData("moving left x inches",rightDistance - 3);
telemetry.update();
straightRight(0.0);
sleep(3000);
}
public void testWrist()
{
wrist.setPosition(0);
@ -235,6 +278,13 @@ public class Red extends LinearOpMode {
}
public void executeAuto()
{
// while (true)
// {
// int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
// telemetry.addData("right", distanceright);
// telemetry.update();
// sleep(500);
// }
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26);
sleep(500);
@ -247,14 +297,27 @@ public class Red extends LinearOpMode {
straightLeft(12);
raisearm(80);
arm.setPower(0);
sleep(500);
turnRight(90);
driveForward(-10);
do {
straightRightOnPower(DRIVE_SPEED_SLOW);
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
} while (distanceright >= 4);
straighten(12.0);
centerRight();
driveForward(88);
sleep(1000);
wrist.setPosition(.465);
gripper.setPosition(1);
sleep(1000);
driveForward(-3);
terminateOpModeNow();
}
if (distanceright < 7)
if (distanceright < 7) //right
{
telemetry.addData("position","right");
telemetry.update();
@ -263,7 +326,21 @@ public class Red extends LinearOpMode {
driveForward(6.5);
raisearm(45);
arm.setPower(0);
driveForward(-10);
sleep(500);
driveForward(-6);
do {
straightRightOnPower(DRIVE_SPEED_SLOW);
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
} while (distanceright >= 6);
straighten(12.0);
centerRight();
driveForward(88);
sleep(1000);
wrist.setPosition(.465);
gripper.setPosition(1);
sleep(1000);
driveForward(-3);
terminateOpModeNow();
@ -275,11 +352,28 @@ public class Red extends LinearOpMode {
raisearm(80);
arm.setPower(0);
driveForward(-8);
turnRight(90);
driveForward(-12);
do {
straightRightOnPower(DRIVE_SPEED_SLOW);
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
} while (distanceright >= 6);
straighten(12.0);
centerRight();
//straightLeft(3);
driveForward(88);
sleep(1000);
wrist.setPosition(.465);
gripper.setPosition(1);
sleep(1000);
driveForward(-3);
terminateOpModeNow();
//Values were created from robot with wheel issues 9/28/23
telemetry.addData("Path", "Complete");

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java
View File

@ -97,7 +97,7 @@ public class RedBackStage extends LinearOpMode {
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;
double DRIVE_SPEED = 0.5;
static final double TURN_SPEED = 0.4;
static final double LONG_TIMEOUT = 1000;
@ -234,7 +234,10 @@ public class RedBackStage extends LinearOpMode {
@SuppressLint("SuspiciousIndentation")
public void executeAuto()
{
backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26);
sleep(500);
@ -257,6 +260,7 @@ public class RedBackStage extends LinearOpMode {
driveForward(-21);
straightLeft(34);
driveForward(-10);
DRIVE_SPEED = .3;
straightRight(35);
driveForward(-5);
raisearm(80);
@ -267,6 +271,11 @@ public class RedBackStage extends LinearOpMode {
driveForward(4.5);
sleep(500);
driveForward(1.5);
raisearm(-50);
sleep(500);
wrist.setPosition(1);
raisearm(-70);
driveForward(-6);
terminateOpModeNow();
@ -284,13 +293,20 @@ public class RedBackStage extends LinearOpMode {
turnLeft(90);
straightLeft(15);
driveForward(-20.5);
DRIVE_SPEED = .3;
straightRight(19);
driveForward(-1.5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
sleep(500);
driveForward(8.5);
raisearm(-50);
sleep(500);
wrist.setPosition(1);
raisearm(-70);
driveForward(-6);
terminateOpModeNow();
@ -307,14 +323,20 @@ else
turnLeft(90);
straightLeft(15);
driveForward(-18);
DRIVE_SPEED = .3;
straightRight(29);
turnRight(10);
turnRight(5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(1);
sleep(500);
driveForward(5);
raisearm(-50);
sleep(500);
wrist.setPosition(1);
raisearm(-70);
driveForward(-6);
terminateOpModeNow();