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

Commit

Permalink
Merge pull request #68 from GoldenDragons772/dev
Browse files Browse the repository at this point in the history 
bucket sense subsystem and maryland tech invitational tuning
  • Loading branch information
@gabehowe
gabehowe authored Jul 1, 2024
2 parents cea0571 + b7a2de8 commit 70bfb85
Show file tree
Hide file tree
Showing 16 changed files with 153 additions and 110 deletions.
3 changes: 3 additions & 0 deletions GoldenDragonTesting/src/main/java/com/example/goldendragontesting/GDtest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,9 @@ public static void main(String[] args) {
System.out.println("Run ArmSequence");
})
.splineToConstantHeading(new Vector2d(47, -35), Math.toRadians(270))
.lineToConstantHeading(new Vector2d(52, -15))
.lineToConstantHeading(new Vector2d(10, -10))
.lineToLinearHeading(new Pose2d(-60, -11, Math.toRadians(185)))
//
// .lineToConstantHeading(new Vector2d(10, -10))
// .splineToConstantHeading(new Vector2d(47, -35), Math.toRadians(270))
Expand Down
39 changes: 15 additions & 24 deletions TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,26 +15,28 @@
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
apply plugin: 'org.jetbrains.kotlin.android'
//apply plugin: 'org.team11260.fast-load-plugin'
apply plugin: 'org.team11260.fast-load-plugin'

buildscript {
repositories {
mavenCentral()
maven { url = 'https://jitpack.io' }


// maven {
// url = 'https://www.matthewo.tech/maven/'
// }
}}
// dependencies {
// classpath 'org.team11260:fast-load-plugin:0.1.1'
// }}
maven {
url = 'https://www.matthewo.tech/maven/'
}
}
dependencies {
classpath 'org.team11260:fast-load-plugin:0.1.2'
}
}


repositories {
// maven {
// url = 'https://www.matthewo.tech/maven/'
// }
maven {
url = 'https://www.matthewo.tech/maven/'
}
maven { url = 'https://jitpack.io' }
}

Expand Down Expand Up @@ -76,22 +78,11 @@ dependencies {
//noinspection GradleDependency
implementation 'androidx.core:core-ktx:1.6.0'
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 'org.openftc:easyopencv:1.7.0'
implementation 'org.openftc:easyopencv:1.7.2'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'

implementation 'com.acmerobotics.dashboard:dashboard:0.4.14'

// implementation 'org.team11260:fast-load:0.1.1'

implementation 'org.team11260:fast-load:0.1.2'
implementation 'org.ftclib.ftclib:core:2.1.1'

// implementation project(path: ':opencv')

// Vision
//implementation 'org.bytedeco:depthai-platform:2.21.2-1.5.9'
//implementation 'org.bytedeco:opencv-platform:4.7.0-1.5.9'
}
44 changes: 31 additions & 13 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/helper/DriveManager.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,11 @@ import com.arcrobotics.ftclib.command.CommandScheduler
import com.arcrobotics.ftclib.command.InstantCommand
import com.arcrobotics.ftclib.gamepad.GamepadEx
import com.arcrobotics.ftclib.gamepad.GamepadKeys
import com.qualcomm.hardware.rev.RevBlinkinLedDriver
import com.qualcomm.robotcore.hardware.DcMotor
import com.qualcomm.robotcore.hardware.Gamepad
import com.qualcomm.robotcore.hardware.HardwareMap
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
import org.firstinspires.ftc.teamcode.rr.drive.MainMecanumDrive
import org.firstinspires.ftc.teamcode.subsystem.*
import org.firstinspires.ftc.teamcode.subsystem.subcommand.CarriageCommand
Expand All @@ -22,6 +24,8 @@ class DriveManager(hardwareMap: HardwareMap, val keymap: Keymap, gamepad1: Gamep
val drive: MecanumDriveSubsystem = MecanumDriveSubsystem(MainMecanumDrive(hardwareMap), false)
val intake: IntakeSubsystem = IntakeSubsystem(hardwareMap)
val linkTake: LinkTakeSubsystem = LinkTakeSubsystem(hardwareMap)
val bucketSense: BucketSenseSubsystem = BucketSenseSubsystem(hardwareMap)
val blinker: BlinkinSubsystem = BlinkinSubsystem(hardwareMap)
private val bucketPivot: BucketPivotSubsystem =
BucketPivotSubsystem(hardwareMap)
private val dipper: DipperSubsystem =
Expand All @@ -39,6 +43,18 @@ class DriveManager(hardwareMap: HardwareMap, val keymap: Keymap, gamepad1: Gamep
}

fun run() {
val gamepad: Gamepad = getGamepad(keymap.linkTakeMap.second).gamepad
if (!gamepad.left_bumper && !gamepad.right_bumper) {
if (gamepad.right_trigger > 0.1) {
val intakePosition: Double = max(0.4, min(0.7, gamepad.right_trigger.pow(2).toDouble()))
linkTake.setLinkTakePosRaw(intakePosition)
intake.runIntake()
} else {
linkTake.setLinkTakePos(LinkTakeSubsystem.linkPos)
intake.stopIntake()
}
}

// Run Scheduler.
CommandScheduler.getInstance().run()
// Drive System
Expand All @@ -48,27 +64,29 @@ class DriveManager(hardwareMap: HardwareMap, val keymap: Keymap, gamepad1: Gamep

// Precision Drive
val triggerVal: Double = getGamepad(keymap.precisionDriveMap.second).getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)
val minSlowdown: Double = if(triggerVal > 0.1) .2 else 1.0 // minimum speed is 20%
val speedMultiplier: Double = 1.0 - (triggerVal * (1.0 - minSlowdown))

val minSlowdown: Double = if(triggerVal > 0.1) .4 else 1.0 // minimum speed is 20%
val speedMultiplier: Double = 1.0 - ((1.0 - minSlowdown))

drive.drive(
forward * speedMultiplier,
strafe * speedMultiplier,
spin * speedMultiplier
)

// Link Take System
val gamepad: Gamepad = getGamepad(keymap.linkTakeMap.second).gamepad
if (!gamepad.left_bumper && !gamepad.right_bumper) {
if (gamepad.right_trigger > 0.1) {
val intakePosition: Double = max(0.4, min(0.7, gamepad.right_trigger.pow(2).toDouble()))
linkTake.setLinkTakePosRaw(intakePosition)
intake.runIntake()
} else {
linkTake.setLinkTakePos(LinkTakeSubsystem.linkPos)
intake.stopIntake()
// BucketSenseSystem
val dist = bucketSense.colorSensorV3.getDistance(DistanceUnit.CM)
if(dist < 1) {
if (bucketSense.isYellow()) {
blinker.setColor(BlinkinSubsystem.PixelColor.PIXEL_YELLOW)
} else if(bucketSense.isWhite()) {
blinker.setColor(BlinkinSubsystem.PixelColor.PIXEL_WHITE)
} else if(bucketSense.isPurple()) {
blinker.setColor(BlinkinSubsystem.PixelColor.PIXEL_PURPLE);
} else if(bucketSense.isGreen()) {
blinker.setColor(BlinkinSubsystem.PixelColor.PIXEL_GREEN)
}
} else {
blinker.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLACK)
}

// Update Current Pos
Expand Down
6 changes: 2 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/helper/TrajectoryManager.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class TrajectoryManager {
name(Alliance.BLUE, Distance.LONG, PropThresholdPipeline.propPos.RIGHT) to Pose2d(-44.0, 18.0, Math.toRadians(90.0)),
name(Alliance.RED, Distance.SHORT, PropThresholdPipeline.propPos.LEFT) to Pose2d(10.0, -37.0, Math.toRadians(155.0)),
name(Alliance.RED, Distance.SHORT, PropThresholdPipeline.propPos.CENTER) to Pose2d(21.0, -24.0, Math.toRadians(180.0)),
name(Alliance.RED, Distance.SHORT, PropThresholdPipeline.propPos.RIGHT) to Pose2d(23.0, -39.0, Math.toRadians(90.0)),
name(Alliance.RED, Distance.SHORT, PropThresholdPipeline.propPos.RIGHT) to Pose2d(23.0, -39.0, Math.toRadians(85.0)),
name(Alliance.RED, Distance.LONG, PropThresholdPipeline.propPos.LEFT) to Pose2d(-44.0, -18.0, Math.toRadians(270.0)),
name(Alliance.RED, Distance.LONG, PropThresholdPipeline.propPos.CENTER) to Pose2d(-42.0, -25.0, Math.toRadians(0.0)),
name(Alliance.RED, Distance.LONG, PropThresholdPipeline.propPos.RIGHT) to Pose2d(-32.0, -37.0, Math.toRadians(30.0))
Expand All @@ -58,8 +58,6 @@ class TrajectoryManager {
}

enum class Type {
SPIKE, FOLLOW, BACKBOARD, CARRIER, PARK
SPIKE, STACK, FOLLOW, BACKBOARD, CARRIER, PARK
}


}
95 changes: 59 additions & 36 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import com.arcrobotics.ftclib.command.InstantCommand;
import com.arcrobotics.ftclib.command.SequentialCommandGroup;
import com.arcrobotics.ftclib.command.WaitCommand;
import com.arcrobotics.ftclib.command.WaitUntilCommand;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

Expand All @@ -33,7 +34,7 @@
import static org.firstinspires.ftc.teamcode.helper.TrajectoryManager.*;


@Autonomous(name = "GDAuto", group = "Auto")
@Autonomous(name = "Auto", group = "Auto")
public class Auto extends LinearOpMode {

private TrajectoryFollowerCommand<TrajectorySequence> follower, driveToBackdrop, driveToSpike, carrier, park;
Expand All @@ -54,6 +55,8 @@ public class Auto extends LinearOpMode {
private ArmMotorSubsystem armMotor;
private IntakeSubsystem intake;

private LinkTakeSubsystem linkTake;

// Camera Instance
private OpenCvCamera camera;

Expand Down Expand Up @@ -81,6 +84,7 @@ public void runOpMode() throws InterruptedException {
dipper = new DipperSubsystem(hardwareMap);
armMotor = new ArmMotorSubsystem(hardwareMap);
intake = new IntakeSubsystem(hardwareMap);
linkTake = new LinkTakeSubsystem(hardwareMap);

detectProp = new PropThresholdPipeline(telemetry);

Expand Down Expand Up @@ -126,27 +130,28 @@ public void onError(int errorCode) {
}

// Auto Selector
if (gamepad1.dpad_right) { // Long Distance Red Auto
distance = Distance.LONG;
alliance = Alliance.RED;
curAuto = "LD_RED";
updateAutoChnage = true;
} else if (gamepad1.dpad_left) { // Short Distance Red Auto
if (gamepad1.dpad_left) { // Short Distance Red Auto
alliance = Alliance.RED;
distance = Distance.SHORT;
curAuto = "SD_RED";
updateAutoChnage = true;
} else if (gamepad1.dpad_up) { // Long Distance Blue Auto
alliance = Alliance.BLUE;
distance = Distance.LONG;
curAuto = "LD_BLUE";
updateAutoChnage = true;
} else if (gamepad1.dpad_down) { // Short Distance Blue Auto
alliance = Alliance.BLUE;
distance = Distance.SHORT;
curAuto = "SD_BLUE";
updateAutoChnage = true;
}
else if (gamepad1.dpad_up) { // Long Distance Blue Auto
alliance = Alliance.BLUE;
distance = Distance.LONG;
curAuto = "LD_BLUE";
updateAutoChnage = true;
} else if (gamepad1.dpad_right) { // Long Distance Red Auto
distance = Distance.LONG;
alliance = Alliance.RED;
curAuto = "LD_RED";
updateAutoChnage = true;
}

if(updateAutoChnage) {
// check if Distance is Long
Expand Down Expand Up @@ -200,29 +205,32 @@ private SequentialCommandGroup createCommandGroup() {
}));
if (distance == Distance.LONG) { // TODO: create a transition from pppp (PurPle Pixel Placing) to placing on the backdrop.
commandGroup.addCommands(carrier);
commandGroup.addCommands(new WaitCommand(7000));
}
commandGroup.addCommands(new InstantCommand(() -> {
armMotor.setArmToPos(ArmMotorSubsystem.ArmPos.LOW);
//
armMotor.setArmToPos(ArmMotorSubsystem.ArmPos.BONUS);
dipper.setDipperPosition(BucketPivotSubsystem.BucketPivotPos.DROPPING_POS);
bucketPivot.runBucketPos(BucketPivotSubsystem.BucketPivotPos.DROPPING_POS);
}));
commandGroup.addCommands(new WaitCommand(750));
commandGroup.addCommands(driveToBackdrop);
commandGroup.addCommands(new InstantCommand(() -> intake.specialDispenseJustForAutoPixelDispenseThing()));
commandGroup.addCommands(new WaitCommand(2000));
commandGroup.addCommands(new WaitCommand(1500));
commandGroup.addCommands(new InstantCommand(() -> { // TODO: Figure out how to make this into a function.
intake.stopIntake();
dipper.setDipperPosition(BucketPivotSubsystem.BucketPivotPos.LOADING_POS);
armMotor.setArmToPos(ArmMotorSubsystem.ArmPos.HOME);
linkTake.setLinkTakePos(LinkTakeSubsystem.LinkPosition.HOME);
int timeout = 1200;
int epsilon = 550; // Machine epsilon
while (!(-epsilon < armMotor.getAvgArmPosition() && armMotor.getAvgArmPosition() < epsilon)) {
try {
Thread.sleep(20);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
}
// while (!(-epsilon < armMotor.getAvgArmPosition() && armMotor.getAvgArmPosition() < epsilon)) {
// try {
// Thread.sleep(20);
// } catch (InterruptedException e) {
// throw new RuntimeException(e);
// }
// }
bucketPivot.runBucketPos(BucketPivotSubsystem.BucketPivotPos.LOADING_POS);
}));
commandGroup.addCommands(park);
Expand All @@ -231,17 +239,29 @@ private SequentialCommandGroup createCommandGroup() {


private Pose2d getBackdropPos() {
switch (currentSpikeLocation) {
case LEFT: {
return new Pose2d(56, (alliance == Alliance.BLUE) ? 41 : -27, Math.toRadians(180));
}
case RIGHT: {
return new Pose2d(56, (alliance == Alliance.BLUE) ? 30 : -41, Math.toRadians(180));
}
case CENTER: {
return new Pose2d(56, (alliance == Alliance.BLUE) ? 34 : -34, Math.toRadians(180));
// Short Distance
switch (currentSpikeLocation) {
case LEFT: {
if(distance == Distance.SHORT) {
return new Pose2d(54, (alliance == Alliance.BLUE) ? 41 : -27, Math.toRadians(180));
}
return new Pose2d(54, (alliance == Alliance.BLUE) ? 43 : -27, Math.toRadians(180));
}
case RIGHT: {
if (distance == Distance.SHORT) {
return new Pose2d(54, (alliance == Alliance.BLUE) ? 27 : -40, Math.toRadians(180));
} else {
return new Pose2d(54, (alliance == Alliance.BLUE) ? 28 : -39, Math.toRadians(180));
}
}
case CENTER: {
if (distance == Distance.SHORT) {
return new Pose2d(54, (alliance == Alliance.BLUE) ? 35 : -34, Math.toRadians(175));
} else {
return new Pose2d(54, (alliance == Alliance.BLUE) ? 37 : -37, Math.toRadians(185));
}
}
}
}
return null;
}

Expand Down Expand Up @@ -269,7 +289,8 @@ private TrajectorySequence getTrajectory(Alliance alliance, Distance distance, T

TrajectorySequence SD_PARK = drive.trajectorySequenceBuilder(SD_BACKBOARD.end())
.forward(2)
.lineToConstantHeading(new Vector2d(50, -60 * reflection))
//.lineToConstantHeading(new Vector2d(52, -10 * reflection)) // Mid
.lineToConstantHeading(new Vector2d(50, -60 * reflection)) // Corner
.build();

// Long Distance Auto
Expand All @@ -284,19 +305,21 @@ private TrajectorySequence getTrajectory(Alliance alliance, Distance distance, T

TrajectorySequence LD_CARRIER = drive.trajectorySequenceBuilder(LD_SPIKE.end())
.back(10)
.lineToLinearHeading(new Pose2d(-35, -10 * reflection, Math.toRadians(180)))
.lineToConstantHeading(new Vector2d(10, -10 * reflection))
.lineToLinearHeading(new Pose2d(-35, -12 * reflection, Math.toRadians(180)))
.lineToLinearHeading(new Pose2d(23, -12 * reflection, Math.toRadians(180)))
.build();

TrajectorySequence LD_BACKBOARD = drive.trajectorySequenceBuilder(LD_CARRIER.end())
.setConstraints(new AngularVelocityConstraint(30), new ProfileAccelerationConstraint(30))
.splineToConstantHeading(new Vector2d(47, -35 * reflection), Math.toRadians(270 * reflection))
//.splineToConstantHeading(new Vector2d(47, -35 * reflection), Math.toRadians(270 * reflection))
.lineToLinearHeading(new Pose2d(37, -12 * reflection, Math.toRadians(180)))
.lineToLinearHeading(new Pose2d(37, -39 * reflection, Math.toRadians(180)))
.lineToLinearHeading(backDropPos)
.build();

TrajectorySequence LD_PARK = drive.trajectorySequenceBuilder(LD_BACKBOARD.end())
.forward(2)
.lineToConstantHeading(new Vector2d(52, -15 * reflection))
.lineToConstantHeading(new Vector2d(52, -10 * reflection)) // Left
.build();


Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Duo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ public void initialize() {
new Pair<>(GamepadKeys.Button.X, 2), // Decrement LinkTakePos,
new Pair<>(GamepadKeys.Button.Y, 1), // Shoot Drone,
new Pair<>(GamepadKeys.Button.A, 1), // Shoot Drone,
new Pair<>(null, 1) // Run LinkTake
new Pair<>(null, 2) // Run LinkTake
);
driveManager = new DriveManager(hardwareMap, keymap, gamepad1, gamepad2);
}
Expand Down
4 changes: 3 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmMotorSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ public enum ArmPos {
HANG(100),
SET1(550),
LOW(700),
BONUS(850),
MIDDLE(1000),
HIGH(1300),
HIGHER(1600),
Expand Down Expand Up @@ -165,9 +166,10 @@ public void decrementArmPos() {
}
}

public int getAvgArmPosition() {
public int getAvgArmPosition() {
return (leftArmMotor.getCurrentPosition() + rightArmMotor.getCurrentPosition()) / 2;
}

public ArmPos getArmPos(){
return armPos;
}
Expand Down
Loading

0 comments on commit 70bfb85

Please sign in to comment.