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

Merge remote-tracking branch 'origin/optimus_dev' into optimus_dev

This commit is contained in:
Scott Barnes 2024-03-21 17:34:22 -05:00
parent 4911dfa6b7 70e37ea532
commit 8f4a0bdebd
15 changed files with 1202 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
View File

@ -90,13 +90,42 @@ public class MeepMeepTesting {
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
//Go back to white stack
.splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
.lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
.splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
//Go back to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
.build()
);
meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
.setDarkMode(true)
.setBackgroundAlpha(0.95f)
//.addEntity(myBot)
//.addEntity(BlueFrontStage)
.addEntity(BlueFrontStage3)
//.addEntity(BlueFrontStage3)
//.addEntity(BlueFrontStage1)
.addEntity(RedFrontStage)
.start();
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@ -12,6 +13,11 @@ public class Arm {
private Servo lAServo;
private Servo doorServo;
private Servo wristServo;
private Servo elbow;
private Slides.Position pos = Slides.Position.DOWN;
private PController armController;
private double armControllerTarget;
@ -22,6 +28,8 @@ public class Arm {
private double armTempPos;
private double doorPos;
private double wristPos;
private double elbowPos;
public static double ARM_KP = 0.18;
public static double doorOpenPos = 0.36;
@ -31,24 +39,35 @@ public class Arm {
public static double armScore = 0.95;
public static double armScore_highMacro = 0.95;
public static double wristStart = 0.29;
public static double wristScore = 0.5;
public static double wristStart = 0.8;
public static double wristScore = 0.31;
public static double elbow_mid = 0.5;
public static double elbow_right = 0.29;
public static double elbow_left = 0.95;
public static double wristScore_highMacro = 0.3;
public double counter;
public enum Position {
INTAKE, SCORE, SCOREHI
INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
}
public enum DoorPosition {
OPEN, CLOSE
}
public enum Elbowpos{
}
public Arm(HardwareMap hardwareMap) {
wristServo = hardwareMap.get(Servo.class, "Wrist");
doorServo = hardwareMap.get(Servo.class, "Door");
lAServo = hardwareMap.get(Servo.class, "LeftArm");
rAServo = hardwareMap.get(Servo.class, "RightArm");
elbow = hardwareMap.get(Servo.class, "Elbow");
// lAServo.setDirection(Servo.Direction.REVERSE);
rAServo.setDirection(Servo.Direction.REVERSE);
doorServo.setDirection(Servo.Direction.REVERSE);
@ -63,6 +82,7 @@ public class Arm {
setWristPos(Position.INTAKE);
setDoor(DoorPosition.CLOSE);
setElbowPos(1);
}
public void setArmPos(Position pos) {
@ -91,6 +111,18 @@ public class Arm {
}
}
public void setElbowPos (int counter){
if (counter == 1) {
elbowPos = elbow_mid;
} else if (counter == 2) {
elbowPos = elbow_mid;
} else if (counter == 3) {
elbowPos = elbow_left;
} else if (counter == 4) {
elbowPos = elbow_right;
}
}
public void setDoor(DoorPosition pos) {
if (pos == DoorPosition.OPEN) {
doorPos = doorOpenPos;
@ -134,5 +166,6 @@ public class Arm {
doorServo.setPosition(doorPos);
wristServo.setPosition(wristPos);
elbow.setPosition(elbowPos);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -19,7 +19,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
public class Camera {
private static final String REAR_WEBCAM_NAME = "rearWebcam";
public static final String FRONT_WEBCAM_NAME = "frontWebcam";
private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(8.9, 1.5, Math.PI);
private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(6.5, 0, Math.PI);
public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
public static float PROP_REJECTION_VERTICAL_LOWER = 440;
private PropDetectionPipeline prop;

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java Normal file
View File

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.HardwareMap;
import java.util.Locale;
@Config
public class LEDs {
public static int NUMBER = 1;//80 solid
public static int RED = 80;
public static int REDFULL = 3;
public static int REDSCORING = 5;//5
public static int REDINIT = 79;
public static int BLUE = 93;
public static int BLUEFULL = 2;
public static int BLUESCORING = 5;//5
public static int BLUEINIT = 99;
public static int yellow = 81;
public static int lime = 87;
public static int white = 97;
public static int purple = 78;
//yellow:81, green: 87, white:97 , purple:78 ,
private RevBlinkinLedDriver blinkinLedDriver;
private RevBlinkinLedDriver.BlinkinPattern pattern;
public LEDs(HardwareMap hardwareMap) {
blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "LEDs");
setPattern();
}
public void setPattern() {
pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(NUMBER);
blinkinLedDriver.setPattern(pattern);
}
public void setPattern(int patternNumber) {
pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(patternNumber);
blinkinLedDriver.setPattern(pattern);
}
public void nextPattern() {
pattern = pattern.next();
blinkinLedDriver.setPattern(pattern);
}
public void previousPattern() {
pattern = pattern.previous();
blinkinLedDriver.setPattern(pattern);
}
public String getTelemetry() {
return String.format(Locale.US, "Pattern: %s", pattern.toString());
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@ -20,6 +21,7 @@ public class Robot {
public Intake intake;
public Slides slides;
public Arm arm;
public LEDs leds;
public endGame_Mechs endGameMechs;
public double macroStartTime = 0;
@ -32,6 +34,8 @@ public class Robot {
public static double scoreWait = 0.65;
public static double armWait = 2.0;
public int alliance = 0;
//Name the objects
public Robot(HardwareMap hardwareMap) {
drive = new SampleMecanumDrive(hardwareMap);
@ -41,9 +45,16 @@ public class Robot {
arm = new Arm(hardwareMap);
endGameMechs = new endGame_Mechs(hardwareMap);
camEnabled = true;
leds = new LEDs(hardwareMap);
}
public void extendMacro(int slidePos, double runTime) {
if (alliance == 1){
leds.setPattern(2);
}else{
leds.setPattern(3);
}
switch(macroState) {
case(0):
macroStartTime = runTime;
@ -66,12 +77,23 @@ public class Robot {
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
if (alliance == 1){
leds.setPattern(93);
}else{
leds.setPattern(80);
}
}
break;
}
}
public void resetMacro(int slidePos, double runTime) {
if (alliance == 1){
leds.setPattern(2);
}else{
leds.setPattern(3);
}
switch(macroState) {
case(0):
macroStartTime = runTime;
@ -103,6 +125,11 @@ public class Robot {
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
if (alliance == 1){
leds.setPattern(93);
}else{
leds.setPattern(80);
}
}
break;
}
@ -180,15 +207,35 @@ public class Robot {
}
public void update(double runTime) {
Pose2d estimatedPose = null;
if (camera != null) {
estimatedPose = this.camera.estimatePoseFromAprilTag();
}
drive.update(estimatedPose);
// drive.getPoseEstimate().getX();
// = estimatedPose.getX();
Pose2d estimatedPose = null;
// if (camera != null) {
// estimatedPose = this.camera.estimatePoseFromAprilTag();
// }
//drive.update(estimatedPose);
// Pose2d p1 = drive.getPoseEstimate();
//
// Pose2d p2 = this.camera.estimatePoseFromAprilTag();
//
// double D = Math.sqrt(Math.pow((p2.getX()-p1.getX()),2) + Math.pow((p2.getY()-p1.getY()),2));
//
// if (D >= 6 || D <= 1){
// estimatedPose = null;
// }else{
// estimatedPose = this.camera.estimatePoseFromAprilTag();
//
// }
drive.update(/*estimatedPose*/);
slides.update(runTime);
arm.update();
//leds.setPattern();
}
public String getTelemetry() {
return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s\nIMU:\n------------\n%s" , arm.getTelemetry(), slides.getTelemetry(), drive.getExternalHeadingVelocity());
}

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

@ -9,8 +9,8 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@Config
public class Slides {
private DcMotor slide;
private DcMotor slide2;
public DcMotor slide;
public DcMotor slide2;
// public static double p = 0.0014;
// public static double i = 0.02;
@ -100,28 +100,26 @@ public class Slides {
// pickupPos = 20 + heightOffset;
// downPos = heightOffset;// TODO add these back in
// if (target == 0) {
// if (slide.getCurrentPosition() <= pTolerance && target <= pTolerance) {
// slide.setPower(0);
// slide2.setPower(0);
// } else {
// if (target < 5) {
// slide.setPower(0);
// slide2.setPower(0);
// } else {
double pid, ff;
controller.setPID(coefficients.p, coefficients.i, coefficients.d);
controller.setTolerance(pTolerance);
pid = controller.calculate(slide.getCurrentPosition(), target);
ff = coefficients.f;
slide.setPower(pid + ff);
double pid, ff;
controller.setPID(coefficients.p, coefficients.i, coefficients.d);
controller.setTolerance(pTolerance);
pid = controller.calculate(slide2.getCurrentPosition(), target);
ff = coefficients.f;
slide2.setPower(pid + ff);
pid = controller.calculate(slide.getCurrentPosition(), target);
ff = coefficients.f;
slide.setPower(pid + ff);
pid = controller.calculate(slide2.getCurrentPosition(), target);
ff = coefficients.f;
slide2.setPower(pid + ff);
// }
// }
}
}
public String getTelemetry() {
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java
View File

@ -43,7 +43,7 @@ public abstract class AutoBase extends LinearOpMode {
telemetry.addLine("Initialized");
// Detection vndafds = robot.camera.getProp() <- returns a detection
// int fdsf = detection.getCenter().x <- x value on the screen between -50,50
double PropDetection = robot.camera.getProp().getCenter().x;
double PropDetection = robot.camera.getProp().getCenter().x;
if (PropDetection <= -DetectionTest ) {
teamPropLocation = 1;

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

@ -29,24 +29,24 @@ public class BlueBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.5, 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_3 = new Pose2d(53.2, 35.6, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.7, 32.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51, 32.6, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-180));
//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, 12.1 , Math.toRadians(-187));//-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, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-55, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-58, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
@Override
@ -89,30 +89,30 @@ public class BlueBackStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.25) {
robot.intake.setDcMotor(-0.24);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1);
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++;
// if (getRuntime() < macroTime + 0.05) {
//
// }
// // if intake is done move on
// else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1);
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:
@ -139,7 +139,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.65)));
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
@ -154,7 +154,7 @@ public class BlueBackStageAuto extends AutoBase {
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK6);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
//robot.intake.setDcMotor(0.5);
macroState++;
}
@ -168,7 +168,7 @@ public class BlueBackStageAuto extends AutoBase {
break;
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.6) {
if (getRuntime() > macroTime + 0.7) {
//robot.intake.setDcMotor(-0.0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
@ -224,7 +224,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.75)));
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
@ -239,7 +239,7 @@ public class BlueBackStageAuto extends AutoBase {
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
//robot.intake.setDcMotor(0.68);
macroState++;
}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java
View File

@ -21,33 +21,37 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-51, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
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_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
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, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
@ -113,10 +117,23 @@ public class BlueFrontPreload extends AutoBase {
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
break;
}
robot.intake.setDcMotor(0.74);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -164,12 +181,12 @@ public class BlueFrontPreload extends AutoBase {
macroTime = getRuntime();
}
break;
case 5:
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;

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

@ -19,35 +19,39 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontStage Auto (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Blue FrontPreload (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-49, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
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_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
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, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
@ -113,10 +117,23 @@ public class BlueFrontStageAuto extends AutoBase {
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
}
robot.intake.setDcMotor(0.74);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -166,10 +183,10 @@ public class BlueFrontStageAuto extends AutoBase {
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
@ -184,183 +201,92 @@ public class BlueFrontStageAuto extends AutoBase {
}
break;
//Stack run 2
case 7:
robot.resetMacro(0, getRuntime());
robot.resetMacro(0,getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
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.9, -3.7)).vec(), Math.toRadians(180));
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 8:
robot.resetMacro(0, getRuntime());
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.54);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK4);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are in-taken by this
case 9:
robot.intake.setDcMotor(0.54);
robot.arm.setDoor(OPEN);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 9:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
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_2.vec(),Math.toRadians(0));
break;
case 2:
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 3:
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;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
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.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
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.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.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());
macroState++;
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
case 11:
// if drive is done move on
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
//Park
case 12:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//stack run 3
case 13:
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(180));
builder.lineToLinearHeading(POST_DROP_POS);
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180));
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 14:
robot.resetMacro(0, getRuntime());
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK2);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 15:
robot.intake.setDcMotor(0.54);
robot.arm.setDoor(OPEN);
robot.intake.setpos(STACK1);
macroTime = getRuntime();
macroState++;
break;
case 16:
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.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
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.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.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());
macroState++;
macroTime = getRuntime();
}
break;
case 17:
// if drive is done move on
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 18:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java
View File

@ -11,6 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
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;
@ -19,35 +20,39 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-32,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,-36.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, -35.9, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -23.5, Math.toRadians(186));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-52, -29.6, Math.toRadians(0));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
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, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
@ -95,8 +100,7 @@ public class RedFrontPreload extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.18) {
robot.intake.setDcMotor(-0.23);
if (getRuntime() < macroTime + 0.1) {
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
@ -113,10 +117,23 @@ public class RedFrontPreload extends AutoBase {
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-2.5))).waitSeconds(.01);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01);
break;
}
robot.intake.setDcMotor(0.74);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -132,11 +149,10 @@ public class RedFrontPreload extends AutoBase {
}
break;
case 4:
if (getRuntime() > macroTime + 0.06) {
if (getRuntime() > macroTime + 0.1) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
robot.intake.setDcMotor(-0.1);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
@ -150,7 +166,7 @@ public class RedFrontPreload extends AutoBase {
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));
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
@ -169,7 +185,7 @@ public class RedFrontPreload extends AutoBase {
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;

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

@ -0,0 +1,304 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
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 static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
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.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontStageme extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
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, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,-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(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
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) {
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
builder.lineToLinearHeading(DROP_2_PART_3);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
//robot.arm.setDoor(OPEN);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.1) {
//robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.1);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
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.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.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());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 7:
robot.resetMacro(0,getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
case 9:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
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_2.vec(),Math.toRadians(0));
break;
case 2:
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 3:
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;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 10:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
case 11:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 12:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//Park
// case 7:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToLinearHeading(PARK_1);
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
//
// case 8:
// // if drive is done move on
// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
// macroState++;
// }
// break;
//
// case 9:
// robot.resetMacro(0, getRuntime());
// if (robot.macroState == 0) {
// macroState = -1;
// }
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

259
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java Normal file
View File

@ -0,0 +1,259 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@Config
@TeleOp(name = "Blue TeleOp", group = "OpModes")
public class BlueTeleop extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Elbow");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
this.robot.alliance = 1;
this.robot.leds.setPattern(92);
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
this.robot.leds.setPattern(93);
// Driver 1
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
if (controller1.getRightTrigger().getValue() > 0.1) {
x *= turbo;
y *= turbo;
z *= turbo;
}
else if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
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();
}
// if (controller1.getRightBumper().isPressed()) {
// this.robot.endGameMechs.Finger_in();
// }else {
// this.robot.endGameMechs.Finger_out();
// }
//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();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
//ElbowPos
if (controller2.getDRight().isPressed()){
robot.arm.setElbowPos(4);
} else if (controller2.getDLeft().isPressed()) {
robot.arm.setElbowPos(3);
}else {
robot.arm.setElbowPos(1);
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
// if (robot.intake.getPower() >= 0.01) {
// robot.arm.setDoor(OPEN);
// } else if (robot.intake.getPower() <= -0.01) {
// robot.arm.setDoor(OPEN);
// } else if (controller2.getLeftBumper().isPressed()) {
// robot.arm.setDoor(Arm.DoorPosition.OPEN);
// } else {
// robot.arm.setDoor(CLOSE);
// }
if (robot.intake.getPower() >= 0.01) {
robot.intake.wheel_swallow();
} else if (robot.intake.getPower() <= -0.01) {
robot.intake.wheel_swallow();
} else if (controller2.getLeftBumper().isPressed()) {
robot.intake.wheel_swallow();
} else {
robot.intake.wheel_stop();
}
//Start A/B stuff
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
} else if (controller2.getDDown().isJustPressed() ) {
robot.runningMacro = 5;
}
break;
case (1):
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(0, getRuntime());
break;
case(5):
robot.extendMacro(Slides.mini_tier1, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

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

@ -24,8 +24,8 @@ public class MainTeleOp extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.65;
public static double intakeMax2 = -0.65;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;

250
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java Normal file
View File

@ -0,0 +1,250 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@Config
@TeleOp(name = "Red TeleOp", group = "OpModes")
public class RedTeleop extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
this.robot.alliance = 2;
this.robot.leds.setPattern(79);
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
this.robot.leds.setPattern(80);
// Driver 1
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
if (controller1.getRightTrigger().getValue() > 0.1) {
x *= turbo;
y *= turbo;
z *= turbo;
}
else if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
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();
}
// if (controller1.getRightBumper().isPressed()) {
// this.robot.endGameMechs.Finger_in();
// }else {
// this.robot.endGameMechs.Finger_out();
// }
//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();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
// if (robot.intake.getPower() >= 0.01) {
// robot.arm.setDoor(OPEN);
// } else if (robot.intake.getPower() <= -0.01) {
// robot.arm.setDoor(OPEN);
// } else if (controller2.getLeftBumper().isPressed()) {
// robot.arm.setDoor(Arm.DoorPosition.OPEN);
// } else {
// robot.arm.setDoor(CLOSE);
// }
//Elbowpos(
if (controller2.getDRight().isJustPressed()){
robot.arm.setElbowPos(4);
} else if (controller2.getDLeft().isJustPressed()) {
robot.arm.setElbowPos(3);
}else if(controller2.getDUp().isJustPressed()) {
robot.arm.setElbowPos(1);
}
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
} else if (controller2.getDDown().isJustPressed() ) {
robot.runningMacro = 5;
}
break;
case (1):
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(0, getRuntime());
break;
case(5):
robot.extendMacro(Slides.mini_tier1, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}