eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

get the 2+2 done first

This commit is contained in:
ImposterVarunoverlord 2024-02-13 13:17:30 -06:00
parent 5d9f46dd2a
commit cd33603cfe
12 changed files with 577 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
MeepMeepTesting/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/build

17
MeepMeepTesting/build.gradle Normal file
View File

@ -0,0 +1,17 @@
plugins {
id 'java-library'
}
java {
sourceCompatibility = JavaVersion.VERSION_1_8
targetCompatibility = JavaVersion.VERSION_1_8
}
repositories {
maven { url = 'https://jitpack.io' }
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
implementation 'com.github.NoahBres:MeepMeep:2.0.3'
}

32
MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java Normal file
View File

@ -0,0 +1,32 @@
package com.example.meepmeeptesting;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.noahbres.meepmeep.MeepMeep;
import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder;
import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
public class MeepMeepTesting {
public static void main(String[] args) {
MeepMeep meepMeep = new MeepMeep(800);
RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
// Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
.setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-51.5, 33.6,Math.toRadians(180)))
// .lineToConstantHeading(new Vector2d(-45,59.5))
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-48,59.5).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33,59.5).vec())
.splineToConstantHeading(new Pose2d(52.5, 36).vec(),Math.toRadians(0))
.build()
);
meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
.setDarkMode(true)
.setBackgroundAlpha(0.95f)
.addEntity(myBot)
.start();
}
}

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

@ -34,7 +34,7 @@ public class Slides {
public static int manualSpeed = 50;
public enum Position { DOWN, TIER1, TIER2, TIER3 }
public enum Position { DOWN, mini_tier1, TIER1, TIER2, TIER3 }
public Slides(HardwareMap hardwareMap) {
slide = hardwareMap.get(DcMotor.class, "Rightslide");
@ -57,6 +57,8 @@ public class Slides {
public void setTarget(Position pos) {
if (pos == Position.DOWN) {
target = Math.min(Math.max(down, targetMin), targetMax);
} else if (pos == Position.mini_tier1) {
target = Math.min(Math.max(mini_tier1, targetMin), targetMax);
} else if (pos == Position.TIER1) {
target = Math.min(Math.max(tier1, targetMin), targetMax);
} else if (pos == Position.TIER2) {

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java
View File

@ -1,6 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@ -9,17 +11,34 @@ public class endGame_Mechs {
private Servo servo;
private Servo hang1;
private Servo hang2;
public static double initPos = 0.8;
public static double launchPos = 0.4;
private DcMotor hang;
public static double initPos = 0.42;
public static double launchPos = 0.8;
public static int initHang = -1000;
public static double hold = 0.8;
public static double release = 0.5;
public static double hold2 = 0.8;
public static double release2 = 0.8;
public static int HangUp = -860;
public static int Hanged = -30;
private int target = 0;
public static int down = 0;
public static int manualSpeed = 50;
public endGame_Mechs(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, "Drone");
this.servo.setPosition(initPos);
// this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
this.hang = hardwareMap.get(DcMotor.class, "Hang");
this.hang.setTargetPosition(0);
this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hang.setDirection(DcMotorSimple.Direction.REVERSE);
this.hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
// this.hang1.setPosition(hold);
// this.hang2 = hardwareMap.get(Servo.class, "Hanger 2");
// this.hang2.setPosition(hold);
@ -35,16 +54,19 @@ public class endGame_Mechs {
}
public void release() {
this.servo.setPosition(release);
this.servo.setPosition(release2);
public void hang_init_pos(){
this.hang.setTargetPosition(0);
this.hang.setPower(1);
}
public void hold() {
this.servo.setPosition(hold);
this.servo.setPosition(hold2);
public void hang_final_pos(){
this.hang.setTargetPosition(2250);
this.hang.setPower(1);
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -25,7 +25,7 @@ public class BlueBackStageAuto extends AutoBase {
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90));
public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
@ -222,8 +222,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5
)));
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 )));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());

313
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java Normal file
View File

@ -0,0 +1,313 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue Backstage Auto(2 and Park)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueBackStagePark extends AutoBase {
public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
public static final Pose2d PARK_1 = new Pose2d(-53,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(-58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
builder.lineToLinearHeading(DROP_3M);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
robot.intake.setDcMotor(-0.24);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 1 -------------------------
case 4:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
builder.lineToLinearHeading(PARK_2);
// switch (teamPropLocation) {
// case 1:
// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
// break;
// case 2:
// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0)));
// break;
// case 3:
// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5)));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
}
break;
// //waits for the robot to fin the trajectory
// case 5:
// robot.resetMacro(0, getRuntime());
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK5);
// if (!robot.drive.isBusy()) {
// macroState++;
// }
// break;
// //First 2 pixels off the stack are intaken by this
// case 6:
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK5);
// macroTime = getRuntime();
// macroState++;
// break;
// //goes back to the easel
// case 7:
// if (getRuntime() > macroTime + 0.03) {
// //robot.intake.setDcMotor(-0.0);
// robot.intake.setDcMotor(-0.35);
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
// switch (teamPropLocation) {
// case 1:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
// break;
// case 3:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// case 8:
// // if drive is done move on
// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
// macroTime = getRuntime();
// robot.macroState = 0;
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// macroState++;
// }
// break;
// //extending the macro and about to score
// case 9:
// if (robot.macroState != 0) {
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// }
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// robot.resetMacro(0, getRuntime());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// // STACK RUN 2
// case 10:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
// switch (teamPropLocation) {
// case 1:
// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
// break;
// case 2:
// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2)));
// break;
// case 3:
// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5
// )));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
// //waits for the robot to fin the trajectory
// case 11:
// robot.resetMacro(0, getRuntime());
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK3);
// if (!robot.drive.isBusy()) {
// macroState++;
// }
// break;
// //Third and 4th pixels off the stack are intaken by this
// case 12:
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK2);
// macroTime = getRuntime();
// macroState++;
// break;
// //goes back to the easel
// case 13:
// if (getRuntime() > macroTime + 0.03) {
// robot.intake.setDcMotor(-0.35);
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
// switch (teamPropLocation) {
// case 1:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
// break;
// case 3:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// case 14:
// // if drive is done move on
// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
// macroTime = getRuntime();
// robot.macroState = 0;
// robot.extendMacro(Slides.mini_tier1 + 140, getRuntime());
// macroState++;
// }
// break;
// //extending the macro and about to score
// case 15:
// if (robot.macroState != 0) {
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// }
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// robot.resetMacro(0, getRuntime());
// macroState++;
// }
// break;
//
// // PARK ROBOT
case 5:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
// builder.lineToLinearHeading(PARK_1);
// builder.lineToLinearHeading(PARK_2);
// builder = this.robot.getTrajectorySequenceBuilder();
}
//
// // PARK ROBOT
////
}
}
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java
View File

@ -21,15 +21,20 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-50, 45.5, Math.toRadians(-90));
public static final Pose2d DROP_1 = new Pose2d(-35, 32.5, Math.toRadians(0)); //THIS ANGLE NEEDS TO BE CHANGED
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-33.5, 40.5, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(-49, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, 35.3, Math.toRadians(8));//187
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.5, 36, Math.toRadians(8));//187
public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(8));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180));
@ -37,7 +42,7 @@ public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 60.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180));
@ -81,20 +86,19 @@ public class BlueFrontStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.22) {
if (getRuntime() < macroTime + 0.32) {
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.8,1.5))).waitSeconds(.01);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
builder.lineToLinearHeading(POST_DROP_POS);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -112,7 +116,7 @@ public class BlueFrontStageAuto extends AutoBase {
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
robot.intake.setDcMotor(-0.8);
builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_DROP_POS.vec());
@ -120,17 +124,41 @@ public class BlueFrontStageAuto extends AutoBase {
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
// case 1:
// builder.setTangent(Math.toRadians(90))
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
// break;
// case 3:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
// break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
@ -142,7 +170,7 @@ public class BlueFrontStageAuto extends AutoBase {
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
macroState++;
}
break;
@ -164,7 +192,7 @@ public class BlueFrontStageAuto extends AutoBase {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,-2)).vec(), Math.toRadians(180));
builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,-2)).vec());
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.7, -3.5)).vec(), Math.toRadians(180));
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -194,22 +222,28 @@ public class BlueFrontStageAuto extends AutoBase {
robot.intake.setDcMotor(-0.45);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.lineToLinearHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,-2)));
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -222,7 +256,7 @@ public class BlueFrontStageAuto extends AutoBase {
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
macroState++;
}
break;

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -29,7 +29,7 @@ public class RedBackStageAuto extends AutoBase {
public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(25, -45.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -28.3, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -34.5, Math.toRadians(190));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(190));
@ -41,13 +41,13 @@ public class RedBackStageAuto extends AutoBase {
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -10.2, Math.toRadians(190));//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11.2, Math.toRadians(185));//-36
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11.2, Math.toRadians(185));
@Override
public void createTrajectories() {
@ -101,7 +101,7 @@ public class RedBackStageAuto extends AutoBase {
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1 );
break;
case 2:
robot.extendMacro(Slides.mini_tier1 -20, getRuntime());
@ -140,7 +140,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-5)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3)));

116
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java
View File

@ -19,27 +19,32 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "JANK Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-50, -45.5, Math.toRadians(90));
public static final Pose2d DROP_1 = new Pose2d(-35, -32.5, Math.toRadians(180)); //THIS ANGLE NEEDS TO BE CHANGED
public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-33.5, -40.5, Math.toRadians(0));
public static final Pose2d DROP_3 = new Pose2d(-49, -33.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -33, Math.toRadians(188));//187
public static final Pose2d DROP_3M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187
public static final Pose2d STACK_LOCATION = new Pose2d(-54.5, -33.6, Math.toRadians(0));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -32, Math.toRadians(188));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, -33.6, Math.toRadians(0));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -57.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0));
@Override
public void createTrajectories() {
@ -81,14 +86,16 @@ public class RedFrontStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.22) {
if (getRuntime() < macroTime + 0.32) {
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-2,-1.5)));
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,1.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
@ -109,25 +116,49 @@ public class RedFrontStageAuto extends AutoBase {
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.44);
robot.intake.setDcMotor(-0.8);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToLinearHeading(POST_DROP_POS);
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0));
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_DROP_POS.vec());
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0));
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
break;
case 2:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
break;
case 3:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
break;
// case 1:
// builder.setTangent(Math.toRadians(90))
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
// break;
// case 3:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
// break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
@ -139,7 +170,7 @@ public class RedFrontStageAuto extends AutoBase {
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
macroState++;
}
break;
@ -157,11 +188,11 @@ public class RedFrontStageAuto extends AutoBase {
//Stack run 2
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) {
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(0));
builder.lineToLinearHeading(POST_DROP_POS);
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-0.5)).vec(), Math.toRadians(0));
builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,2)).vec(), Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,2)).vec());
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, 3.7)).vec(), Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -187,25 +218,32 @@ public class RedFrontStageAuto extends AutoBase {
case 10:
if (getRuntime() > macroTime + 0.6) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.45);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.lineToLinearHeading(PRE_DEPOSIT_POS);
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -218,7 +256,7 @@ public class RedFrontStageAuto extends AutoBase {
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
macroState++;
}
break;
@ -232,8 +270,13 @@ public class RedFrontStageAuto extends AutoBase {
macroState++;
}
break;
case 13:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
//stack run 3
//stack run 3
// case 13:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) {
@ -319,3 +362,4 @@ public class RedFrontStageAuto extends AutoBase {
}
}

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -27,6 +27,8 @@ public class MainTeleOp extends OpMode {
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
@Override
public void init() {
controller1 = new Controller(gamepad1);
@ -34,7 +36,7 @@ public class MainTeleOp extends OpMode {
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
this.robot.endGameMechs.hold();
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
@ -68,6 +70,31 @@ public class MainTeleOp extends OpMode {
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.launch();
} else {
this.robot.endGameMechs.reset();
}
//Hang Motor
// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
// this.robot.endGameMechs.hang_init_pos();
// hang_counter = true;
// }
// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
// this.robot.endGameMechs.hang_final_pos();
// hang_counter = false;
// }
if (controller1.getB().isPressed()) {
this.robot.endGameMechs.hang_final_pos();
} else {
this.robot.endGameMechs.hang_init_pos();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
@ -87,16 +114,8 @@ public class MainTeleOp extends OpMode {
robot.intake.decrementPos();
}
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.launch();
} else {
this.robot.endGameMechs.reset();
}
//Hang Servos
if (controller1.getX().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.release();
}
// macros

1
settings.gradle
View File

@ -1,2 +1,3 @@
include ':FtcRobotController'
include ':TeamCode'
include ':MeepMeepTesting'