diff --git a/HARDWARE.md b/HARDWARE.md index 4996c4c..878acd1 100644 --- a/HARDWARE.md +++ b/HARDWARE.md @@ -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 | + diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 442471b..9364c04 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -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' -} +} \ No newline at end of file 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 8973fd5..8d3540f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -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 @@ -72,11 +75,12 @@ public class Autonomoustest extends LinearOpMode { private DcMotor rightDrive = null; private DcMotor backrightDrive = null; private DcMotor backleftDrive = null; - private ColorSensor colorRight = null; - private ColorSensor colorLeft = 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(); @@ -179,27 +183,6 @@ public class Autonomoustest extends LinearOpMode { 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); @@ -211,11 +194,12 @@ public class Autonomoustest extends LinearOpMode { 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"); + 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. @@ -254,16 +238,21 @@ public class Autonomoustest extends LinearOpMode { } public void executeAuto() { + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveForward(26); - int blueleft = readColorLeft(); - int blueright = readColorRight(); - double backboard = 29; - if (blueleft > 50 ) + + 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) { //telemetry.addData("color sensor","left"); - if(blueleft > blueright) - telemetry.addData("color sensor","left"); + turnLeft(90); straightLeft(2); driveForward(6.5); @@ -286,11 +275,9 @@ public class Autonomoustest extends LinearOpMode { } - if (blueright > 50) + if (distanceright < 30) { - if(blueleft < blueright) - telemetry.addData("color sensor","right"); - straightRight(11); + straightRight(13.5); raisearm(80); arm.setPower(0); driveForward(-15.5); @@ -309,9 +296,8 @@ public class Autonomoustest extends LinearOpMode { } - else - telemetry.addData("position","center"); - driveForward(2.5); + + driveForward(6.5); raisearm(80); arm.setPower(0); driveForward(-8); @@ -322,6 +308,7 @@ public class Autonomoustest extends LinearOpMode { driveForward(8); driveForward(-26); straightRight(29); + driveForward(-1.5); raisearm(80); wrist.setPosition(0); raisearm(100); @@ -370,7 +357,7 @@ 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) (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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java index ab75179..e952d5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java index fd91263..4d214f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -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);