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")
Configuration Name: **cometBoTsChassis2023**
Configuration Name: **CometBoTsChassis2023**
There are two robots: 14493-DS, and FTC-992M.
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 |
| 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 |
| 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 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 | control | Color sensor V3 | Left outside leg frame | color left |
| I2C B1 | control | Color sensor V3 | Right outside leg frame | color right |
| I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance |
| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise |
| motor1 | expansion | Core Hex Motor | right back arm frame | hang |
| motor3 | expansion | Digital device | arm frame back right | axle encoder |
| Servo 0 | expansion | Servo | on arm | wrist |
| Servo 1 | expansion | Servo | on arm | gripper |
| motor1 | expansion | Core Hex Motor | right back arm frame | hang |
| motor3 | expansion | Digital device | arm frame back right | axle encoder |
| Servo 0 | expansion | Servo | on arm | wrist |
| Servo 1 | expansion | Servo | on arm | gripper |

6
TeamCode/build.gradle
View File

@ -26,8 +26,4 @@ android {
dependencies {
implementation project(':FtcRobotController')
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.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
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
@ -70,13 +73,14 @@ public class Autonomoustest extends LinearOpMode {
/* Declare OpMode members. */
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
private DcMotor backRightDrive = null;
private DcMotor backLeftDrive = null;
private ColorSensor colorRight = null;
private ColorSensor colorLeft = 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();
@ -112,8 +116,8 @@ public class Autonomoustest extends LinearOpMode {
/* telemetry.addData("Starting at", "%7d :%7d",
leftDrive.getCurrentPosition(),
rightDrive.getCurrentPosition(),
backLeftDrive.getCurrentPosition(),
backRightDrive.getCurrentPosition());*/
backleftDrive.getCurrentPosition(),
backrightDrive.getCurrentPosition());*/
telemetry.update();
@ -137,8 +141,8 @@ public class Autonomoustest extends LinearOpMode {
{
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
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
}
@ -146,8 +150,8 @@ public class Autonomoustest extends LinearOpMode {
{
leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.FORWARD);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
}
@ -155,8 +159,8 @@ public class Autonomoustest extends LinearOpMode {
{
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.REVERSE);
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.FORWARD);
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
backleftDrive.setDirection(DcMotor.Direction.FORWARD);
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
}
@ -164,8 +168,8 @@ public class Autonomoustest extends LinearOpMode {
{
leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.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);
}
@ -173,49 +177,29 @@ public class Autonomoustest extends LinearOpMode {
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);
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 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) {
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");
colorRight = hardwareMap.get(ColorSensor.class, "color right");
colorLeft = hardwareMap.get(ColorSensor.class, "color left");
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");
wrist.setPosition(1);
sleep(1000);
// 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
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
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);
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);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
public void testWrist()
@ -253,51 +237,54 @@ public class Autonomoustest extends LinearOpMode {
}
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);
driveForward(26);
int blueleft = readColorLeft();
int blueright = readColorRight();
// double backboard = 29; -- not used
if (blueleft > 50 )
{
//telemetry.addData("color sensor","left");
if(blueleft > blueright)
telemetry.addData("color sensor","left");
turnLeft(90);
straightLeft(2);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-23);
straightLeft(32);
turnLeft(10);
driveForward(18);
driveForward(-40);
//telemetry.addData("color sensor","left");
turnLeft(90);
straightLeft(2);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-23);
straightLeft(32);
turnLeft(10);
driveForward(18);
driveForward(-40);
// straightRight(31.5);
// raisearm(80);
// wrist.setPosition(0);
// raisearm(100);
// driveForward(-1);
// gripper.setPosition(0.25);
terminateOpModeNow();
terminateOpModeNow();
}
if (blueright > 50)
{
if(blueleft < blueright)
telemetry.addData("color sensor","right");
straightRight(11);
raisearm(80);
arm.setPower(0);
driveForward(-15.5);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-38);
}
if (distanceright < 30)
{
straightRight(13.5);
raisearm(80);
arm.setPower(0);
driveForward(-15.5);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-38);
// straightRight(14.5);
// raisearm(80);
// wrist.setPosition(0);
@ -305,41 +292,41 @@ public class Autonomoustest extends LinearOpMode {
// gripper.setPosition(.25);
// driveForward(5);
// raisearm(-200);
terminateOpModeNow();
terminateOpModeNow();
}
else
telemetry.addData("position","center");
driveForward(2.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
straightRight(11.5);
driveForward(-15);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-26);
straightRight(29);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(.25);
sleep(500);
driveForward(5);
raisearm(-270);
telemetry.update();
sleep(250);
}
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);
straightRight(11.5);
driveForward(-15);
turnLeft(90);
straightLeft(15);
driveForward(8);
driveForward(-26);
straightRight(29);
driveForward(-1.5);
raisearm(80);
wrist.setPosition(0);
raisearm(100);
gripper.setPosition(.25);
sleep(500);
driveForward(5);
raisearm(-270);
telemetry.update();
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.update();
sleep(1000); // pause to display final telemetry message.
telemetry.addData("Path", "Complete");
telemetry.update();
sleep(1000); // pause to display final telemetry message.
}
@ -362,7 +349,7 @@ public class Autonomoustest extends LinearOpMode {
int newLeftTarget;
int newRightTarget;
int newBackLeftTarget;
int newBackRightTarget;
int newbackRightTarget;
if (opModeIsActive()) {
@ -370,25 +357,25 @@ public class Autonomoustest extends LinearOpMode {
// 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);
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);
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);
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));
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
@ -398,27 +385,27 @@ 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() && 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());
leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition());
telemetry.update();
}
leftDrive.setPower(0);
rightDrive.setPower(0);
backRightDrive.setPower(0);
backLeftDrive.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);
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
sleep(250); // optional pause after each move.
}
@ -426,14 +413,14 @@ public class Autonomoustest extends LinearOpMode {
public void armEncoder(double speed,
double Inches, double timeoutS) {
int newArmTarget;
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);
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
arm.setTargetPosition(newarmTarget);
// Turn On RUN_TO_POSITION
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@ -453,7 +440,7 @@ public class Autonomoustest extends LinearOpMode {
(arm.isBusy())) {
// Display it for the driver.
telemetry.addData("Running to", " %7d", newArmTarget);
telemetry.addData("Running to", " %7d", newarmTarget);
telemetry.addData("Currently at", " at %7d",
arm.getCurrentPosition());
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.
* 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)
{
gripper.setPosition(0.25);
gripper.setPosition(0.5);
}
if(gamepad2.right_trigger > 0.35){
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
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.
// Set up a variable for each drive wheel to save the power level for telemetry.
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)
telemetry.addData("color sensor","left");
straightLeft(11);
straightLeft(13.5);
raisearm(80);
arm.setPower(0);
driveForward(-15.5);
@ -311,7 +311,7 @@ public class bluefront extends LinearOpMode {
}
else
telemetry.addData("position","center");
driveForward(2.5);
driveForward(6.5);
raisearm(80);
arm.setPower(0);
driveForward(-8);