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

arm encoders made, not tested

Browse Source
This commit is contained in:
robotics2
2023-10-10 17:04:04 -07:00
parent 780f5fdde9
commit 35dbc1cc15
1 changed files with 31 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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.
}