From 35dbc1cc15a7860cb6880c847bcd8dc187188ac2 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Tue, 10 Oct 2023 17:04:04 -0700 Subject: [PATCH] arm encoders made, not tested --- .../ftc/teamcode/Autonomoustest.java | 41 ++++++++++++++----- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java index 6212476..2932607 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -94,11 +94,13 @@ public class Autonomoustest extends LinearOpMode { 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; @Override + static final double DEGREE_TOO_DISTANCE = 0.21944444444; + @Override public void runOpMode() { // Initialize the drive system variables @@ -146,10 +148,8 @@ public class Autonomoustest extends LinearOpMode { // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) - raisearm(1); - arm.setPower(0.001); - sleep(10000); - //driveForward(2); + raiseArm(10); + // driveForward(28); // { // int blueleft = readColorLeft(); @@ -223,6 +223,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE); + arm.setPower(0); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout } public void straightLeft(double distance) @@ -231,6 +232,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.REVERSE); + arm.setPower(0); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } public void straightRight(double distance) @@ -239,6 +241,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.FORWARD); + arm.setPower(0); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } public void turnLeft(double degrees) @@ -247,6 +250,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.FORWARD); + arm.setPower(0); double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } @@ -256,6 +260,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.REVERSE); + arm.setPower(0); double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } @@ -281,13 +286,22 @@ public class Autonomoustest extends LinearOpMode { } - - public void raisearm(long seconds) + public void raiseArm(double distance) { - arm.setPower(.1); - sleep(seconds * 1000); + leftDrive.setPower(0); + rightDrive.setPower(0); + backleftDrive.setPower(0); + backrightDrive.setPower(0); + arm.setDirection(DcMotorSimple.Direction.REVERSE); + encoderDrive(DRIVE_SPEED, distance,distance,LONG_TIMEOUT ); } +// public void raisearm(long seconds) +// { +// arm.setPower(.1); +// sleep(seconds * 1000); +// } + /* @@ -306,6 +320,7 @@ public class Autonomoustest extends LinearOpMode { int newLeftTarget; int newRightTarget; int newBackLeftTarget; + int newArmTarget; int newbackRightTarget; if (opModeIsActive()) { @@ -315,16 +330,19 @@ public class Autonomoustest extends LinearOpMode { newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); newBackLeftTarget = backleftDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); newbackRightTarget = backrightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); + newArmTarget = backrightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_ARM_INCH); leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); backrightDrive.setTargetPosition(newbackRightTarget); backleftDrive.setTargetPosition(newBackLeftTarget); + arm.setTargetPosition(newArmTarget); // 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); + arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); // reset the timeout time and start motion. runtime.reset(); @@ -332,6 +350,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setPower(Math.abs(speed)); backrightDrive.setPower(Math.abs(speed)); backleftDrive.setPower(Math.abs(speed)); + 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 @@ -341,7 +360,7 @@ public class Autonomoustest extends LinearOpMode { // onto the next step, use (isBusy() || isBusy()) in the loop test. while (opModeIsActive() && (runtime.seconds() < timeoutS) && - (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy())) { + (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && arm.isBusy())) { // Display it for the driver. telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); @@ -356,6 +375,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setPower(0); backrightDrive.setPower(0); backleftDrive.setPower(0); + arm.setPower(0); // Turn off RUN_TO_POSITION @@ -363,6 +383,7 @@ public class Autonomoustest extends LinearOpMode { rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); sleep(250); // optional pause after each move. }