Skip to content
This repository has been archived by the owner on Aug 31, 2024. It is now read-only.

Commit

Permalink
AutonomousV1 & AutonomousV2 (#101)
Browse files Browse the repository at this point in the history
* [fix] motor coefficients

* [av1] autonv1 basics

* code

* Complete Auton Rewrite & Simplification

* Manual Starting State Selection & TestingMode

* Trajectory fixes

* bad solution, but hopefully fixes

* boo

* im sorry

* [roadrunner] DriveConstants update

* Updates to roadrunner tuning

* auton fixes, hopefully

* Fix Auton Trajectories

* Add Moving after Yellow Pixel Delivery

* FTC Dashboard Telemetry

* Blue Side Trajectories

* Telemetry Fix

* Slow trajectory fixes

* things

* Merge Master

* Auton mostly works, we are conisdering a different method of detecting

* AutonV1 Rewrite + AutonV2 Red Only

* AutonV1 Edits + AutonV2 Massive Changes

* Fix blue side trajectories for autov1

* Intake Detection While Driving + New Experimental Sweeping Idea

* Pixel Stack -> Backboard Trajectory WORKS

* woops

* HWC Changes

* things

* Comp Changes!

---------

Co-authored-by: Milo Banks <milobanks@rowlandhall.org>
DragonDev07 and milobanks authored Feb 26, 2024
1 parent 2ec9fd2 commit 1c13880
Showing 14 changed files with 1,106 additions and 186 deletions.
4 changes: 2 additions & 2 deletions TeamCode/build.gradle
Original file line number Diff line number Diff line change
@@ -65,10 +65,10 @@ dependencies {

umlDoclet "nl.talsmasoftware:umldoclet:2.1.0"
implementation 'com.acmerobotics.roadrunner:core:0.5.5'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.12'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.15'
implementation 'org.openftc:easyopencv:1.7.0'
implementation 'org.openftc:apriltag:2.0.0'
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'org.ftclib.ftclib:core:2.0.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,391 @@
package org.firstinspires.ftc.teamcode.auton;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;

import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor;
import org.firstinspires.ftc.teamcode.subsystems.HWC;
import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence;

@Autonomous(name = "AutonomousV1")
public class AutonomousV1 extends OpMode {
// ------ State Enum ------ //
private enum State {
DRIVING_TO_DEPOSIT_PURPLE_PIXEL, DEPOSITING_PURPLE_PIXEL, DRIVING_TO_BACKBOARD, DEPOSITING_YELLOW_PIXEL, PARK, STOP
}

// ------ Declare Others ------ //
private HWC robot;
private State state = State.DRIVING_TO_DEPOSIT_PURPLE_PIXEL;
private AllianceColor allianceColor = AllianceColor.RED;
private HWC.Location elementLocation;
private String activeTrajectory = "";
private boolean firstRun = true;

// ------ Trajectories ------ //
private Trajectory toDepositCenter;
private Trajectory toDepositRight;
private Trajectory toDepositLeft;
private Trajectory toBackboardFromCenter;
private Trajectory toBackboardFromRight;
private Trajectory toBackboardFromLeft;
private Trajectory toParkFromBackboardLeft;
private Trajectory toParkFromBackboardRight;
private Trajectory toParkFromBackboardCenter;

// ------ Starting Positions ------ //
private final Pose2d START_POSE_RED = new Pose2d(12, -60, Math.toRadians(90.00));
private final Pose2d START_POSE_BLUE = new Pose2d(12, 60, Math.toRadians(270.00));

@Override
public void init() {
// ------ Telemetry ------ //
telemetry.addData("Status", "Initializing");
telemetry.update();

// ------ Initialize Robot Hardware ------ //
robot = new HWC(hardwareMap, telemetry, true);

// ------ Start FTC Dashboard Telemetry ------ //
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());

// ------ Start Tensorflow ------ //
robot.initTFOD("fpaVision.tflite");

// ------ Reset Servos ------ //
robot.clawL.setPosition(1);
robot.clawR.setPosition(0);

// ------ Close Claw for Yellow Pixel ------ //
robot.toggleClaw('L');

// ------ Telemetry ------ //
telemetry.addData("Status", "Initialized");
telemetry.addData(">", "Yellow Pixel on Left Side");
telemetry.addData(">", "Purple Pixel in intake");
telemetry.update();
}

@Override
public void init_loop() {
// ------ GamePad Updates ------ //
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.previousGamepad2.copy(robot.currentGamepad2);
robot.currentGamepad1.copy(gamepad1);
robot.currentGamepad2.copy(gamepad2);

// ------ Save Element Position ------ //
elementLocation = robot.detectElement(allianceColor);

// ------ Alliance Color Selection ------ //
if (robot.currentGamepad1.a && !robot.previousGamepad1.a) {
allianceColor = allianceColor.equals(AllianceColor.RED) ? AllianceColor.BLUE : AllianceColor.RED;
}


// ------ Set Trajectories based on Alliance Color ------ //
switch(allianceColor) {
case RED:
// ------ Set Robot Start Pose ------ //
robot.drive.setPoseEstimate(START_POSE_RED);

// ------ Declare Trajectories ------ //
// Drive to Center Line
toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_RED)
.lineTo(new Vector2d(12.0, -34))
.build();

// Drive to Right Line
toDepositRight = robot.drive.trajectoryBuilder(START_POSE_RED)
.strafeTo(new Vector2d(22, -43))
.build();

// Drive to Left Line
toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED)
.lineToLinearHeading(new Pose2d(9, -40, Math.toRadians(135)))
.build();

// Drive to Backboard from Center
toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end())
.lineToLinearHeading(new Pose2d(49, -35, Math.toRadians(180)))
.build();

// Drive to Backboard from Right
toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end())
.lineToLinearHeading(new Pose2d(49, -37, Math.toRadians(180)))
.build();

// Drive to Backboard from Left
toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end())
.lineToLinearHeading(new Pose2d(49, -30, Math.toRadians(180)))
.build();

// Park from Backboard Center
toParkFromBackboardCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end())
.strafeTo(new Vector2d(48, -60))
.build();

// Park from Backboard Right
toParkFromBackboardRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end())
.strafeTo(new Vector2d(48, -60))
.build();

// Drive to Park from Backboard 2
toParkFromBackboardLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end())
.strafeTo(new Vector2d(48, -60))
.build();

break;
case BLUE:
// ------ Set Robot Start Pose ------ //
robot.drive.setPoseEstimate(START_POSE_BLUE);

// ------ Declare Trajectories ------ //
// Drive to Center Line
toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_BLUE)
.lineTo(new Vector2d(12.0, 34))
.build();

// Drive to Right Line
toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE)
.strafeTo(new Vector2d(23, 45))
.build();

// Drive to Left Line
toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE)
.splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(225)), Math.toRadians(225))
.build();

// Drive to Backboard from Center
toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end())
.lineToLinearHeading(new Pose2d(49, 35, Math.toRadians(180)))
.build();

// Drive to Backboard from Right
toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end())
.lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180)))
.build();

// Drive to Backboard from Left
toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end())
.lineToLinearHeading(new Pose2d(49, 30, Math.toRadians(180)))
.build();

// Park from Backboard Center
toParkFromBackboardCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end())
.strafeTo(new Vector2d(48, 60))
.build();

// Park from Backboard Right
toParkFromBackboardRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end())
.strafeTo(new Vector2d(48, 60))
.build();

// Drive to Park from Backboard 2
toParkFromBackboardLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end())
.strafeTo(new Vector2d(48, 60))
.build();
break;
}

// ------ Telemetry ------ //
telemetry.addData(">", "Yellow Pixel on Left Side");
telemetry.addData(">", "Purple Pixel in intake");
telemetry.addData("Element Location", elementLocation);
telemetry.addData("Selected Alliance Color", allianceColor);
telemetry.addData("Status", "Init Loop");
telemetry.update();
}

@Override
public void loop() {
// ------ GamePad Updates ------ //
robot.previousGamepad1.copy(robot.currentGamepad1);
robot.previousGamepad2.copy(robot.currentGamepad2);
robot.currentGamepad1.copy(gamepad1);
robot.currentGamepad2.copy(gamepad2);

// ------ State Machine ------ //
switch (state) {
case DRIVING_TO_DEPOSIT_PURPLE_PIXEL:
drivingToDepositPurplePixel();
break;
case DEPOSITING_PURPLE_PIXEL:
depositingPurplePixel();
break;
case DRIVING_TO_BACKBOARD:
drivingToBackboard();
break;
case DEPOSITING_YELLOW_PIXEL:
depositingYellowPixel();
break;
case PARK:
drivingToPark();
case STOP:
stop();
break;
}

// ------ Move Slides Using PID ------ //
robot.pulleyLComponent.moveUsingPID();
robot.pulleyRComponent.moveUsingPID();

// ------ Move Robot ------ //
robot.drive.update();

// ------ Telemetry ------ //
telemetry.addData("Status", state);
telemetry.addData("Element Location", elementLocation);
telemetry.addData("Pose", robot.drive.getPoseEstimate());
telemetry.addData("Active Trajectory", activeTrajectory);
telemetry.addLine();
telemetry.addData("Hardware", robot);
telemetry.update();
}

// ------ State Methods ------ //
private void drivingToDepositPurplePixel() {
// ------ Select Trajectory ------ //
if(firstRun) {
firstRun = false;
if (elementLocation == HWC.Location.CENTER) {
activeTrajectory = "toDepositCenter";
robot.drive.followTrajectoryAsync(toDepositCenter);
} else if (elementLocation == HWC.Location.RIGHT) {
activeTrajectory = "toDepositRight";
robot.drive.followTrajectoryAsync(toDepositRight);
} else {
activeTrajectory = "toDepositLeft";
robot.drive.followTrajectoryAsync(toDepositLeft);
}
}

// ------ Set Next State ------ //
if (!robot.drive.isBusy()) {
state = State.DEPOSITING_PURPLE_PIXEL;
firstRun = true;
}
}

// Deposit Purple Pixel
private void depositingPurplePixel() {
// ------ Run Intake Motor Backwards for 1.5 Seconds ------ //
robot.intakeMotor.setPower(0.8);
robot.elapsedTimeSleep(1500);
robot.intakeMotor.setPower(0);

// ------ Set Next State ------ //
state = State.DRIVING_TO_BACKBOARD;
}

// Drive to Backboard
private void drivingToBackboard() {
// ------ Select Trajectory ------ //
if(firstRun) {
firstRun = false;
if (elementLocation == HWC.Location.CENTER) {
activeTrajectory = "toBackboardFromInitial";
robot.drive.followTrajectoryAsync(toBackboardFromCenter);
} else if (elementLocation == HWC.Location.RIGHT) {
activeTrajectory = "toBackboardFromSecond";
robot.drive.followTrajectoryAsync(toBackboardFromRight);
} else {
activeTrajectory = "toBackBoardFromLeft";
robot.drive.followTrajectoryAsync(toBackboardFromLeft);
}
}

// ------ Set Next State ------ //
if (!robot.drive.isBusy()) {
state = State.DEPOSITING_YELLOW_PIXEL;
firstRun = true;
}
}

// Deposit Yellow Pixel
private void depositingYellowPixel() {
// ------ Move Slides, Passover & Wrist ------ //
deliver();

// ------ Wait for Passover to Move ------ //
robot.elapsedTimeSleep(1000);

// ------ Open Claw ------ //
robot.toggleClaw('L');

// ------ Wait for Claw to Open ------ //
robot.elapsedTimeSleep(1000);

// ------ Move Slides, Passover & Wrist ------ //
intake();

// ------ Set Next State ------ //
state = State.PARK;
}

// Drive to Park
private void drivingToPark() {
// ------ Select Trajectory ------ //
if(firstRun) {
firstRun = false;
activeTrajectory = "toParkFromBackboard2";
if (elementLocation == HWC.Location.CENTER) {
activeTrajectory = "toParkFromBackboardCenter";
robot.drive.followTrajectoryAsync(toParkFromBackboardCenter);
} else if (elementLocation == HWC.Location.RIGHT) {
activeTrajectory = "toParkFromBackboardRight";
robot.drive.followTrajectoryAsync(toParkFromBackboardRight);
} else {
activeTrajectory = "toParkFromBackboardLeft";
robot.drive.followTrajectoryAsync(toParkFromBackboardLeft);
}
}

// ------ Set Next State ------ //
if (!robot.drive.isBusy()) {
state = State.STOP;
firstRun = true;
}
}

// Method to move to Delivery Position
private void deliver() {
robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos);
robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos);
robot.wrist.setPosition(HWC.wristDeliveryPos);
robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0);
robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0);
}

// Method to move to Intake Position
private void intake() {
robot.passoverArmLeft.setPosition(HWC.passoverIntakePos);
robot.passoverArmRight.setPosition(HWC.passoverIntakePos);
robot.wrist.setPosition(HWC.wristIntakePos);
robot.pulleyLComponent.setTarget(HWC.slidePositions[0]);
robot.pulleyRComponent.setTarget(HWC.slidePositions[0]);
}

// Method to Check Claws & Close
private void checkClaws() {
// Close Claws when Pixel Detected
if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) {
robot.clawL.setPosition(0.5);
}

if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) {
robot.clawR.setPosition(0.5);
}

// If both Pixels are Detected, Stop Intake
if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) {
robot.intakeMotor.setPower(0);
}
}
}

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@ public void init() {
telemetry.update();

// ------ Initialize Robot Hardware ------ //
robot = new HWC(hardwareMap, telemetry);
robot = new HWC(hardwareMap, telemetry, false);

// ------ Reset Servos ------ //
robot.clawL.setPosition(1);
@@ -48,14 +48,14 @@ public void init_loop() {

@Override
public void start() {
robot.betterSleep(10000);
robot.elapsedTimeSleep(10000);

if (color.equals("red")) {
robot.leftFront.setPower(-2);
robot.leftRear.setPower(2);
robot.rightFront.setPower(-2);
robot.rightRear.setPower(2);
robot.betterSleep(1500);
robot.elapsedTimeSleep(1500);
robot.leftFront.setPower(0);
robot.leftRear.setPower(0);
robot.rightFront.setPower(0);
@@ -65,7 +65,7 @@ public void start() {
robot.leftRear.setPower(-2);
robot.rightFront.setPower(2);
robot.rightRear.setPower(-2);
robot.betterSleep(1500);
robot.elapsedTimeSleep(1500);
robot.leftFront.setPower(0);
robot.leftRear.setPower(0);
robot.rightFront.setPower(0);

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
package org.firstinspires.ftc.teamcode.auton.enums;

// ------ Alliance Color Enum ------ //
public enum AllianceColor {
RED, BLUE
}
Original file line number Diff line number Diff line change
@@ -18,6 +18,7 @@
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor;
import org.firstinspires.ftc.teamcode.subsystems.pid.RobotComponents;
import org.firstinspires.ftc.teamcode.subsystems.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.vision.VisionPortal;
@@ -34,6 +35,11 @@ public class HWC {
"blue", "red"
};

// ------ Computer Vision Location Enum ------ //
public enum Location {
LEFT, CENTER, RIGHT
}

// ------ Declare Slide Positions ------ //
public static int[] slidePositions = { 0, -964, -2110, -3460};

@@ -80,25 +86,28 @@ public class HWC {

// ------ Computer Vision VisionPortal ------ //
private VisionPortal visionPortal;
private boolean roadrunner;

/**
* Constructor for HWC, declares all hardware components
*
* @param hardwareMap HardwareMap - Used to retrieve hardware devices
* @param telemetry Telemetry - Used to add telemetry to driver hub
*/
public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
// ------ Telemetry ------ //
public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry, boolean roadrunner) {
this.telemetry = telemetry;
this.roadrunner = roadrunner;

// ------ Declare RR Drivetrain ------ //
drive = new SampleMecanumDrive(hardwareMap);

// ------ Retrieve Drive Motors ------ //
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
if (roadrunner) {
// ------ Declare RR Drivetrain ------ //
drive = new SampleMecanumDrive(hardwareMap);
} else {
// ------ Retrieve Drive Motors ------ //
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
}

// ------ Retrieve Other Motors ------ //
rightPulley = hardwareMap.get(DcMotorEx.class, "pulleyR");
@@ -119,24 +128,33 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
colorRight = hardwareMap.get(ColorRangeSensor.class, "colorR");

// ------ Set Motor Directions ------ //
leftFront.setDirection(DcMotorEx.Direction.FORWARD);
rightFront.setDirection(DcMotorEx.Direction.FORWARD);
leftRear.setDirection(DcMotorEx.Direction.FORWARD);
rightRear.setDirection(DcMotorEx.Direction.FORWARD);
if (!roadrunner) {
leftFront.setDirection(DcMotorEx.Direction.FORWARD);
rightFront.setDirection(DcMotorEx.Direction.FORWARD);
leftRear.setDirection(DcMotorEx.Direction.FORWARD);
rightRear.setDirection(DcMotorEx.Direction.FORWARD);
}

leftPulley.setDirection(DcMotorEx.Direction.REVERSE);

// ------ Set Motor Brake Modes ------ //
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
if (!roadrunner) {
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}

intakeMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

// ------ Set Motor Modes ------ //
leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
if (!roadrunner) {
leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
}

intakeMotor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
leftPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
rightPulley.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);
@@ -162,49 +180,24 @@ public void toggleClaw(char servo) {
}
}

public void betterSleep(int sleep) {
// ------ Function that Allows For Sleeping in OpModes ------ //
public void elapsedTimeSleep(int milliseconds) {
sleepTime.reset();
while (sleepTime.milliseconds() < sleep) {
while (sleepTime.milliseconds() < milliseconds) {
pulleyLComponent.moveUsingPID();
pulleyRComponent.moveUsingPID();

telemetry.addData("sleeping", "true");
telemetry.update();
}
telemetry.addData("sleeping", "slept");
telemetry.update();

}

public void sleepDeliver(int time) {
intakeMotor.setPower(-0.3);
betterSleep(time);
intakeMotor.setPower(0);
}

// ------ Function to Power Slides ------ //
public void powerSlides(double target) {
pulleyLComponent.incrementTarget(target);
pulleyRComponent.incrementTarget(target);
}

// ------ Function to Reset Motor Encoder Positions [EMERGENCY ONLY] ------ //
public void resetEncoders() {
leftFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER);

leftFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
rightFront.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
leftRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
rightRear.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
}

// ------ Function to Initialize TensorFlow Object Detection ------ //
public void initTFOD(String TFOD_MODEL_ASSET) {

// Create the TensorFlow processor by using a builder.
tfod = new TfodProcessor.Builder()
.setModelAssetName(TFOD_MODEL_ASSET)
.setModelLabels(LABELS)
.build();
tfod = new TfodProcessor.Builder().setModelAssetName(TFOD_MODEL_ASSET).setModelLabels(LABELS).build();

// Create the vision portal by using a builder.
VisionPortal.Builder builder = new VisionPortal.Builder();
@@ -220,36 +213,56 @@ public void initTFOD(String TFOD_MODEL_ASSET) {
tfod.setMinResultConfidence(0.75f);
}

private double telemetryTFOD() {
// ------ Function to add Telemetry for TensorFlow Object Detection ------ //
public Location detectElement(AllianceColor allianceColor) {
List<Recognition> currentRecognitions = tfod.getRecognitions();
telemetry.addData("# Objects Detected", currentRecognitions.size());
double x = 800;
// Step through the list of recognitions and display info for each one.
double x = 1000, y;

for (Recognition recognition : currentRecognitions) {
x = (recognition.getLeft() + recognition.getRight()) / 2;
double y = (recognition.getTop() + recognition.getBottom()) / 2;

y = (recognition.getTop() + recognition.getBottom()) / 2;

telemetry.addData("", " ");
telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
telemetry.addData("- Position", "%.0f / %.0f", x, y);
telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
}
return x;

switch (allianceColor) {
default:
case RED:
if (x < 400) {
return Location.CENTER;
} else if (x > 400 && x < 800) {
return Location.RIGHT;
} else {
return Location.LEFT;
}
case BLUE:
if (x < 400) {
return Location.LEFT;
} else if (x > 400 && x < 800) {
return Location.CENTER;
} else {
return Location.RIGHT;
}
}
}

public int cv() {
double pos = telemetryTFOD(); //set it to a value depending on locaiton of obkect
if (pos > 800) {
return 0;
} else if (pos < 100 && pos > 0) {
return 1;
} else if (pos < 400) {
return 2;
} else if (pos < 800) {
return 3;
@NonNull
@Override
public String toString() {
StringBuilder builder = new StringBuilder("HWC {");

if (roadrunner) {
builder.append("\n\tbusy = ").append(drive.isBusy());
} else {
return 0;
builder.append("\n\tleftFront = ").append(leftFront.getPower()).append("\n\trightFront = ").append(rightFront.getPower()).append("\n\tleftRear = ").append(leftRear.getPower()).append("\n\trightRear = ").append(rightRear.getPower());
}

builder.append("\n\trightPulley = ").append(rightPulley.getPower()).append("\n\tleftPulley = ").append(leftPulley.getPower()).append("\n\tintakeMotor = ").append(intakeMotor.getPower()).append("\n\twrist = ").append(wrist.getPosition()).append("\n\tclawR = ").append(clawR.getPosition()).append("\n\tclawL = ").append(clawL.getPosition()).append("\n\tpassoverArmLeft = ").append(passoverArmLeft.getPosition()).append("\n\tpassoverArmRight = ").append(passoverArmRight.getPosition()).append("\n}");

return builder.toString();
}
}
Original file line number Diff line number Diff line change
@@ -32,7 +32,7 @@ public class MotorPIDTuning extends OpMode {
@Override
public void init() {
// ------ Initialize Hardware ------ //
kit = new HWC(hardwareMap, telemetry);
kit = new HWC(hardwareMap, telemetry, false);

slideLeft = kit.leftPulley;
slideRight = kit.rightPulley;
Original file line number Diff line number Diff line change
@@ -21,8 +21,8 @@ public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 3895.9;
public static final double MAX_RPM = 312.0;
public static final double TICKS_PER_REV = 384.5;
public static final double MAX_RPM = 435;

/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
@@ -32,8 +32,8 @@ public class DriveConstants {
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = true;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0.061, 0.00473, 0.0,
public static final boolean RUN_USING_ENCODER = false;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));

/*
@@ -44,19 +44,19 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 3.77953; // in
public static double GEAR_RATIO = 1.0 / 10.71; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 10.6299; // in
public static double WHEEL_RADIUS = 2; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 13.39; // in

/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
* motor encoders or have elected not to use them for velocity control, these values should be
* empirically tuned.
*/
public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kA = 1.95;
public static double kStatic = 0.02;
public static double kV = 0.0155; // 1.0 / rpmToVelocity(MAX_RPM);
public static double kA = 0.003;
public static double kStatic = 0;

/*
* These values are used to generate the trajectories for you robot. To ensure proper operation,
@@ -65,10 +65,10 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 25;
public static double MAX_ACCEL = 25;
public static double MAX_ANG_VEL = Math.toRadians(50);
public static double MAX_ANG_ACCEL = Math.toRadians(50);
public static double MAX_VEL = 30;
public static double MAX_ACCEL = 30;
public static double MAX_ANG_VEL = 6.217;
public static double MAX_ANG_ACCEL = Math.toRadians(180);

/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
@@ -91,4 +91,4 @@ public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}
}
}
Original file line number Diff line number Diff line change
@@ -57,8 +57,8 @@
public class SampleMecanumDrive extends MecanumDrive {
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0);
public static double LATERAL_MULTIPLIER = 1;
public static double VX_WEIGHT = 1;
public static double VY_WEIGHT = 1;
@@ -126,12 +126,16 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
}

// TODO: reverse any motors using DcMotor.setDirection()
leftFront.setDirection(DcMotorEx.Direction.REVERSE);
rightFront.setDirection(DcMotorEx.Direction.FORWARD);
leftRear.setDirection(DcMotorEx.Direction.REVERSE);
rightRear.setDirection(DcMotorEx.Direction.FORWARD);

List<Integer> lastTrackingEncPositions = new ArrayList<>();
List<Integer> lastTrackingEncVels = new ArrayList<>();

// TODO: if desired, use setLocalizer() to change the localization method
// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));
setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));

trajectorySequenceRunner = new TrajectorySequenceRunner(
follower, HEADING_PID, batteryVoltageSensor,
Original file line number Diff line number Diff line change
@@ -28,13 +28,14 @@
*/
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 0;
public static double WHEEL_RADIUS = 2; // in
public static double TICKS_PER_REV = 2000;
public static double WHEEL_RADIUS = 0.3; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed

public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel

public static double LATERAL_DISTANCE = 12.57; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = -6.5; // in; offset of the lateral wheel
public static double X_MULTIPLIER = 3.12;
public static double Y_MULTIPLIER = 2.89;
private final Encoder leftEncoder;
private final Encoder rightEncoder;
private final Encoder frontEncoder;
@@ -52,11 +53,13 @@ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List<Integer> las
lastEncPositions = lastTrackingEncPositions;
lastEncVels = lastTrackingEncVels;

leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront"));

// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
rightEncoder.setDirection(Encoder.Direction.REVERSE);
frontEncoder.setDirection(Encoder.Direction.REVERSE);
}

public static double encoderTicksToInches(double ticks) {
@@ -76,9 +79,9 @@ public List<Double> getWheelPositions() {
lastEncPositions.add(frontPos);

return Arrays.asList(
encoderTicksToInches(leftPos),
encoderTicksToInches(rightPos),
encoderTicksToInches(frontPos)
encoderTicksToInches(leftPos) * X_MULTIPLIER,
encoderTicksToInches(rightPos) * X_MULTIPLIER,
encoderTicksToInches(frontPos) * Y_MULTIPLIER
);
}

@@ -95,9 +98,9 @@ public List<Double> getWheelVelocities() {
lastEncVels.add(frontVel);

return Arrays.asList(
encoderTicksToInches(leftVel),
encoderTicksToInches(rightVel),
encoderTicksToInches(frontVel)
encoderTicksToInches(leftVel) * X_MULTIPLIER,
encoderTicksToInches(rightVel) * X_MULTIPLIER,
encoderTicksToInches(frontVel) * Y_MULTIPLIER
);
}
}
}
Original file line number Diff line number Diff line change
@@ -19,7 +19,7 @@ public void init() {
telemetry.update();

// ------ Initialize Robot Hardware ------ //
robot = new HWC(hardwareMap, telemetry);
robot = new HWC(hardwareMap, telemetry, false);
}

@Override
Original file line number Diff line number Diff line change
@@ -42,7 +42,7 @@ public void init() {
telemetry.update();

// ------ Initialize Robot Hardware ------ //
robot = new HWC(hardwareMap, telemetry);
robot = new HWC(hardwareMap, telemetry, false);

// ------ Reset Pulley Encoders ------ //
robot.leftPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

0 comments on commit 1c13880

Please sign in to comment.