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

2+1(blue works not red)

This commit is contained in:
ImposterVarunoverlord 2024-03-19 19:58:03 -05:00
parent f07e298100
commit 28e6aa7836
13 changed files with 824 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,8 +95,8 @@ public class MeepMeepTesting {
.setBackgroundAlpha(0.95f)
//.addEntity(myBot)
//.addEntity(BlueFrontStage)
.addEntity(BlueFrontStage3)
//.addEntity(BlueFrontStage1)
//.addEntity(BlueFrontStage3)
.addEntity(BlueFrontStage1)
.start();
}
}

24
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;
@ -34,6 +42,10 @@ public class Arm {
public static double wristStart = 0.29;
public static double wristScore = 0.5;
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 enum Position {
@ -49,6 +61,7 @@ public class Arm {
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);
@ -91,6 +104,17 @@ public class Arm {
}
}
public void setElbowPos (Position pos){
if (pos == Position.INTAKE) {
wristPos = elbow_mid;
} else if (pos == Position.SCORE) {
wristPos = elbow_right;
} else if (pos == Position.SCOREHI) {
wristPos = elbow_left;
}
}
public void setDoor(DoorPosition pos) {
if (pos == DoorPosition.OPEN) {
doorPos = doorOpenPos;

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(9.75, 0, 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());

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);
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());
@ -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;
}
}
}
}
}

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

@ -21,33 +21,37 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main 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(-35.7,-41, 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(50, -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(-55, -40, 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(12, -58.5, Math.toRadians(0));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
@ -95,8 +99,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 +116,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);
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());
@ -132,11 +148,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) {
@ -169,7 +184,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;

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

@ -0,0 +1,239 @@
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, "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 = 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();
}
// 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 (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;

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

@ -0,0 +1,240 @@
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);
}
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();
}
}