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

Compare commits

base: SCDS:58f738e28d69c7fde8d44807b04bcd48a9d4e64b
Branches Tags
SCDS:branch-cooper-v9.0.1
...
compare: SCDS:a6ea0fc529c3fbfdd6ddeb7d4e8349c17750a4ba
Branches Tags
SCDS:branch-cooper-v9.0.1

4 Commits
58f738e28d ... a6ea0fc529

Author SHA1 Message Date
robotics2
a6ea0fc529 Merge in new changes 2023-11-16 17:14:12 -08:00
robotics2
e3a3bdfb3b Replaced color sensor with distance sensor 2023-11-16 17:07:36 -08:00
robotics2
81e0825fea Copy from 9.0.1 to here 2023-11-04 10:08:49 -07:00
robotics2
a9c0d443eb Copy from 9.0.1 to here 2023-11-04 10:07:31 -07:00
5 changed files with 133 additions and 149 deletions
Expand all files Collapse all files

21
HARDWARE.md
View File

@ -4,26 +4,27 @@
![Bird's eye view of robot.](/Robot.png "Bird's eye view of robot") ![Bird's eye view of robot.](/Robot.png "Bird's eye view of robot")
Configuration Name: **cometBoTsChassis2023** Configuration Name: **CometBoTsChassis2023**
There are two robots: 14493-DS, and FTC-992M. There are two robots: 14493-DS, and FTC-992M.
Below are the following configurations for our robots Below are the following configurations for our robots
| physical port | hub | robot part | robot part location | robot software config name | | physical port | hub | robot part | robot part location | robot software config name |
|---------------|-----------|----------------------------|-------------------------------|----------------------------| |---------------|-----------|-----------------------------|-------------------------------|----------------------------|
| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt | | motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt |
| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt | | motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt |
| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt | | motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt |
| motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt | | motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt |
| I2C B0 | control | Color sensor V3 | Left outside leg frame | color left | | I2C B0 | control | Color sensor V3 | Left outside leg frame | color left |
| I2C B1 | control | Color sensor V3 | Right outside leg frame | color right | | I2C B1 | control | Color sensor V3 | Right outside leg frame | color right |
| I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance | | I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance |
| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise | | motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise |
| motor1 | expansion | Core Hex Motor | right back arm frame | hang | | motor1 | expansion | Core Hex Motor | right back arm frame | hang |
| motor3 | expansion | Digital device | arm frame back right | axle encoder | | motor3 | expansion | Digital device | arm frame back right | axle encoder |
| Servo 0 | expansion | Servo | on arm | wrist | | Servo 0 | expansion | Servo | on arm | wrist |
| Servo 1 | expansion | Servo | on arm | gripper | | Servo 1 | expansion | Servo | on arm | gripper |

6
TeamCode/build.gradle
View File

@ -26,8 +26,4 @@ android {
dependencies { dependencies {
implementation project(':FtcRobotController') implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar') annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
}
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
}

245
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.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
@ -70,13 +73,14 @@ public class Autonomoustest extends LinearOpMode {
/* Declare OpMode members. */ /* Declare OpMode members. */
private DcMotor leftDrive = null; private DcMotor leftDrive = null;
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;
private DistanceSensor distance = null;
private ElapsedTime runtime = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime();
@ -112,8 +116,8 @@ public class Autonomoustest extends LinearOpMode {
/* telemetry.addData("Starting at", "%7d :%7d", /* telemetry.addData("Starting at", "%7d :%7d",
leftDrive.getCurrentPosition(), leftDrive.getCurrentPosition(),
rightDrive.getCurrentPosition(), rightDrive.getCurrentPosition(),
backLeftDrive.getCurrentPosition(), backleftDrive.getCurrentPosition(),
backRightDrive.getCurrentPosition());*/ backrightDrive.getCurrentPosition());*/
telemetry.update(); telemetry.update();
@ -137,8 +141,8 @@ public class Autonomoustest extends LinearOpMode {
{ {
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.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); // S1: Forward 47 Inches with 5 Sec timeout
} }
@ -146,8 +150,8 @@ public class Autonomoustest extends LinearOpMode {
{ {
leftDrive.setDirection(DcMotor.Direction.FORWARD); leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
} }
@ -155,8 +159,8 @@ public class Autonomoustest extends LinearOpMode {
{ {
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE);
backRightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.FORWARD);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
} }
@ -164,8 +168,8 @@ public class Autonomoustest extends LinearOpMode {
{ {
leftDrive.setDirection(DcMotor.Direction.FORWARD); leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.FORWARD);
double turning_distance = degrees * DEGREE_TOO_DISTANCE; 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);
} }
@ -173,49 +177,29 @@ public class Autonomoustest extends LinearOpMode {
public void turnRight(double degrees) { public void turnRight(double degrees) {
leftDrive.setDirection(DcMotor.Direction.REVERSE); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE);
backRightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
double turning_distance = degrees * DEGREE_TOO_DISTANCE; 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);
} }
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 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);
} }
public void hardwareinit() public void hardwareinit()
{ {
leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt"); leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
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");
distance = hardwareMap.get(DistanceSensor.class, "distance");
wrist.setPosition(1); wrist.setPosition(1);
sleep(1000); sleep(1000);
// To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
@ -223,21 +207,21 @@ public class Autonomoustest extends LinearOpMode {
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips // 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); leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE); backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE);
arm.setDirection(DcMotor.Direction.REVERSE); arm.setDirection(DcMotor.Direction.REVERSE);
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
} }
public void testWrist() public void testWrist()
@ -253,51 +237,54 @@ public class Autonomoustest extends LinearOpMode {
} }
public void executeAuto() public void executeAuto()
{
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveForward(26);
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.CM);
int distanceright = (int)distanceRight.getDistance(DistanceUnit.CM);
telemetry.addData("color left sensor",distanceleft);
telemetry.addData("color right sensor",distanceright);
telemetry.update();
sleep(500);
if (distanceleft < 30)
{ {
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //telemetry.addData("color sensor","left");
driveForward(26);
int blueleft = readColorLeft(); turnLeft(90);
int blueright = readColorRight(); straightLeft(2);
// double backboard = 29; -- not used driveForward(6.5);
if (blueleft > 50 ) raisearm(80);
{ arm.setPower(0);
//telemetry.addData("color sensor","left"); driveForward(-23);
if(blueleft > blueright) straightLeft(32);
telemetry.addData("color sensor","left"); turnLeft(10);
turnLeft(90); driveForward(18);
straightLeft(2); driveForward(-40);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-23);
straightLeft(32);
turnLeft(10);
driveForward(18);
driveForward(-40);
// straightRight(31.5); // straightRight(31.5);
// raisearm(80); // raisearm(80);
// wrist.setPosition(0); // wrist.setPosition(0);
// raisearm(100); // raisearm(100);
// driveForward(-1); // driveForward(-1);
// gripper.setPosition(0.25); // gripper.setPosition(0.25);
terminateOpModeNow(); terminateOpModeNow();
} }
if (blueright > 50) if (distanceright < 30)
{ {
if(blueleft < blueright) straightRight(13.5);
telemetry.addData("color sensor","right"); raisearm(80);
straightRight(11); arm.setPower(0);
raisearm(80); driveForward(-15.5);
arm.setPower(0); turnLeft(90);
driveForward(-15.5); straightLeft(15);
turnLeft(90); driveForward(8);
straightLeft(15); driveForward(-38);
driveForward(8);
driveForward(-38);
// straightRight(14.5); // straightRight(14.5);
// raisearm(80); // raisearm(80);
// wrist.setPosition(0); // wrist.setPosition(0);
@ -305,41 +292,41 @@ public class Autonomoustest extends LinearOpMode {
// gripper.setPosition(.25); // gripper.setPosition(.25);
// driveForward(5); // driveForward(5);
// raisearm(-200); // raisearm(-200);
terminateOpModeNow(); terminateOpModeNow();
} }
else
telemetry.addData("position","center"); driveForward(6.5);
driveForward(2.5); raisearm(80);
raisearm(80); arm.setPower(0);
arm.setPower(0); driveForward(-8);
driveForward(-8); straightRight(11.5);
straightRight(11.5); driveForward(-15);
driveForward(-15); turnLeft(90);
turnLeft(90); straightLeft(15);
straightLeft(15); driveForward(8);
driveForward(8); driveForward(-26);
driveForward(-26); straightRight(29);
straightRight(29); driveForward(-1.5);
raisearm(80); raisearm(80);
wrist.setPosition(0); wrist.setPosition(0);
raisearm(100); raisearm(100);
gripper.setPosition(.25); gripper.setPosition(.25);
sleep(500); sleep(500);
driveForward(5); driveForward(5);
raisearm(-270); raisearm(-270);
telemetry.update(); telemetry.update();
sleep(250); sleep(250);
//Values were created from robot with wheel issues 9/28/23 //Values were created from robot with wheel issues 9/28/23
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.
} }
@ -362,7 +349,7 @@ public class Autonomoustest extends LinearOpMode {
int newLeftTarget; int newLeftTarget;
int newRightTarget; int newRightTarget;
int newBackLeftTarget; int newBackLeftTarget;
int newBackRightTarget; int newbackRightTarget;
if (opModeIsActive()) { if (opModeIsActive()) {
@ -370,25 +357,25 @@ public class Autonomoustest extends LinearOpMode {
// Determine new target position, and pass to motor controller // Determine new target position, and pass to motor controller
newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
newBackLeftTarget = backLeftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
newBackRightTarget = backRightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
leftDrive.setTargetPosition(newLeftTarget); leftDrive.setTargetPosition(newLeftTarget);
rightDrive.setTargetPosition(newRightTarget); rightDrive.setTargetPosition(newRightTarget);
backRightDrive.setTargetPosition(newBackRightTarget); backrightDrive.setTargetPosition(newbackRightTarget);
backLeftDrive.setTargetPosition(newBackLeftTarget); backleftDrive.setTargetPosition(newBackLeftTarget);
// Turn On RUN_TO_POSITION // Turn On RUN_TO_POSITION
leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
backRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
backLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// reset the timeout time and start motion. // reset the timeout time and start motion.
runtime.reset(); runtime.reset();
leftDrive.setPower(Math.abs(speed)); leftDrive.setPower(Math.abs(speed));
rightDrive.setPower(Math.abs(speed)); rightDrive.setPower(Math.abs(speed));
backRightDrive.setPower(Math.abs(speed)); backrightDrive.setPower(Math.abs(speed));
backLeftDrive.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. // 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 // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
@ -398,27 +385,27 @@ public class Autonomoustest extends LinearOpMode {
// onto the next step, use (isBusy() || isBusy()) in the loop test. // onto the next step, use (isBusy() || isBusy()) in the loop test.
while (opModeIsActive() && while (opModeIsActive() &&
(runtime.seconds() < timeoutS) && (runtime.seconds() < timeoutS) &&
(leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) { (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) {
// Display it for the driver. // Display it for the driver.
telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
telemetry.addData("Currently at", " at %7d :%7d", telemetry.addData("Currently at", " at %7d :%7d",
leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backRightDrive.getCurrentPosition(), backLeftDrive.getCurrentPosition()); leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition());
telemetry.update(); telemetry.update();
} }
leftDrive.setPower(0); leftDrive.setPower(0);
rightDrive.setPower(0); rightDrive.setPower(0);
backRightDrive.setPower(0); backrightDrive.setPower(0);
backLeftDrive.setPower(0); backleftDrive.setPower(0);
// Turn off RUN_TO_POSITION // Turn off RUN_TO_POSITION
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.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.
} }
@ -426,14 +413,14 @@ public class Autonomoustest extends LinearOpMode {
public void armEncoder(double speed, public void armEncoder(double speed,
double Inches, double timeoutS) { double Inches, double timeoutS) {
int newArmTarget; int newarmTarget;
if (opModeIsActive()) { if (opModeIsActive()) {
// Determine new target position, and pass to motor controller // Determine new target position, and pass to motor controller
newArmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
arm.setTargetPosition(newArmTarget); arm.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION // Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -453,7 +440,7 @@ public class Autonomoustest extends LinearOpMode {
(arm.isBusy())) { (arm.isBusy())) {
// Display it for the driver. // Display it for the driver.
telemetry.addData("Running to", " %7d", newArmTarget); telemetry.addData("Running to", " %7d", newarmTarget);
telemetry.addData("Currently at", " at %7d", telemetry.addData("Currently at", " at %7d",
arm.getCurrentPosition()); arm.getCurrentPosition());
telemetry.update(); telemetry.update();

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

@ -100,7 +100,7 @@ public class arm extends OpMode {
} }
double num = 3; double num = 2.5;
/** /**
* User defined loop method. * User defined loop method.
* This method will be called repeatedly in a loop while this op mode is running * This method will be called repeatedly in a loop while this op mode is running
@ -129,7 +129,7 @@ public class arm extends OpMode {
} }
if(gamepad2.left_trigger > 0.35) if(gamepad2.left_trigger > 0.35)
{ {
gripper.setPosition(0.25); gripper.setPosition(0.5);
} }
if(gamepad2.right_trigger > 0.35){ if(gamepad2.right_trigger > 0.35){
gripper.setPosition(1); gripper.setPosition(1);
@ -161,7 +161,7 @@ public class arm extends OpMode {
} }
axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value
lateral = gamepad1.left_stick_x/num; lateral = gamepad1.left_stick_x/num;
yaw = gamepad1.right_stick_x/(num+0.5); yaw = gamepad1.right_stick_x/(num);
// Combine the joystick requests for each axis-motion to determine each wheel's power. // Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry. // Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw; double leftFrontPower = axial + lateral + yaw;

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

@ -265,7 +265,7 @@ public class bluefront extends LinearOpMode {
if(blueleft > blueright) if(blueleft > blueright)
telemetry.addData("color sensor","left"); telemetry.addData("color sensor","left");
straightLeft(11); straightLeft(13.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-15.5); driveForward(-15.5);
@ -311,7 +311,7 @@ public class bluefront extends LinearOpMode {
} }
else else
telemetry.addData("position","center"); telemetry.addData("position","center");
driveForward(2.5); driveForward(6.5);
raisearm(80); raisearm(80);
arm.setPower(0); arm.setPower(0);
driveForward(-8); driveForward(-8);