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

before LM3 all subsystems working, auto nothing

This commit is contained in:
sihan 2024-01-09 19:17:19 -06:00
parent 1814413011
commit 24540bf5ca
8 changed files with 157 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,26 +1,31 @@
package org.firstinspires.ftc.teamcode.hardware; package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCOREAUTO;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE;
import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN; import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE; import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.HANG;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED; import static org.firstinspires.ftc.teamcode.util.Configurables.HANGPLANE;
import static org.firstinspires.ftc.teamcode.util.Configurables.HANGRELEASE;
import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN; import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP; import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH; import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH;
import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP; import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE; import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE;
import static org.firstinspires.ftc.teamcode.util.Constants.CLAW; import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM; import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFTHANG;
import static org.firstinspires.ftc.teamcode.util.Constants.PLANE; import static org.firstinspires.ftc.teamcode.util.Constants.PLANE;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM; import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTHANG; import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
@ -36,8 +41,8 @@ import lombok.Getter;
public class Robot { public class Robot {
@Getter @Getter
private MecanumDrive drive; private MecanumDrive drive;
@Getter // @Getter
private Intake intake; // private Intake intake;
@Getter @Getter
private Arm arm; private Arm arm;
@Getter @Getter
@ -50,64 +55,111 @@ public class Robot {
private Camera camera; private Camera camera;
@Getter @Getter
private Plane plane; private Plane plane;
@Getter
private Slides slides;
public Robot init(HardwareMap hardwareMap) { public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap); this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap); this.hang = new Hang().init(hardwareMap);
this.intake = new Intake().init(hardwareMap);
this.arm = new Arm().init(hardwareMap); this.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap); this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().init(hardwareMap); this.claw = new Claw().init(hardwareMap);
this.camera = new Camera(hardwareMap); this.camera = new Camera(hardwareMap);
this.plane = new Plane().init(hardwareMap); this.plane = new Plane().init(hardwareMap);
this.slides= new Slides().init(hardwareMap);
return this; return this;
} }
public static class Intake { public static class Slides {
private DcMotor intake = null; private DcMotor slidesRight = null;
private DcMotor slidesLeft = null;
public Intake init(HardwareMap hardwareMap) { public Slides init(HardwareMap hardwareMap) {
this.intake = hardwareMap.dcMotor.get("intake"); this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT);
this.intake.setDirection(DcMotorSimple.Direction.REVERSE); this.slidesRight = hardwareMap.get(DcMotor.class, SLIDERIGHT);
this.intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); this.slidesLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.slidesRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.slidesRight.setTargetPosition(0);
this.slidesLeft.setTargetPosition(0);
this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesLeft.setDirection(DcMotorSimple.Direction.REVERSE);
this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
return this; return this;
} }
public void spinIn() { public void slideTo(int position, double power) {
this.intake.setPower(0.7); this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesLeft.setTargetPosition(position);
this.slidesLeft.setPower(power);
this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.slidesRight.setTargetPosition(position);
this.slidesRight.setPower(power);
} }
public void spinOut() { public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);}
this.intake.setPower(-0.7);
}
public void stop() { public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);}
this.intake.setPower(0);
} public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);}
} }
public static class Hang { public static class Hang {
private Servo hangLeft; public DcMotor hangLeft = null;
private Servo hangRight; public DcMotor hangRight = null;
public Hang init(HardwareMap hardwareMap) { public Hang init(HardwareMap hardwareMap) {
this.hangLeft = hardwareMap.get(Servo.class, LEFTHANG); this.hangLeft = hardwareMap.get(DcMotor.class, HANGLEFT);
this.hangRight = hardwareMap.get(Servo.class, RIGHTHANG); this.hangRight = hardwareMap.get(DcMotor.class, HANGRIGHT);
this.hangLeft.setPosition(LOCKSPEED); this.hangLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.hangRight.setPosition(LOCK); this.hangRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.hangRight.setTargetPosition(0);
this.hangLeft.setTargetPosition(0);
this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangLeft.setDirection(DcMotorSimple.Direction.REVERSE);
this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
return this; return this;
} }
public void lock() { public void hangTo(int hangPos, double daPower) {
this.hangLeft.setPosition(LOCKSPEED); this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangRight.setPosition(LOCK); this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.hangLeft.setTargetPosition(hangPos);
this.hangLeft.setPower(daPower);
this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.hangRight.setTargetPosition(hangPos);
this.hangRight.setPower(daPower);
} }
public void release() { public void hangRelease(){
this.hangLeft.setPosition(UNLOCKSPEED); this.hangTo(HANGRELEASE,1);
this.hangRight.setPosition(UNLOCK);
} }
public void hang(){
this.hangTo(HANG,1);
}
public void hangPlane(){
this.hangTo(HANGPLANE,1);
}
public void hangIdle() {
this.hangLeft.setPower(0);
this.hangRight.setPower(0);
}
} }
public static class Arm { public static class Arm {
@ -139,6 +191,12 @@ public class Robot {
this.rightArm.setPosition(ARMACCSCORE); this.rightArm.setPosition(ARMACCSCORE);
} }
public void armAccurateScoreAuto() {
this.leftArm.setPosition(ARMACCSCOREAUTO);
this.rightArm.setPosition(ARMACCSCOREAUTO);
}
public void armRest() { public void armRest() {
this.leftArm.setPosition(ARMREST); this.leftArm.setPosition(ARMREST);
this.rightArm.setPosition(ARMREST); this.rightArm.setPosition(ARMREST);

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java
View File

@ -57,7 +57,7 @@ public class AutoBlue extends LinearOpMode {
.build(); .build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(-72, -26, Math.toRadians(180))) .lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore) .addTemporalMarker(.2, robot.getWrist()::wristScore)
.build(); .build();
@ -99,12 +99,12 @@ public class AutoBlue extends LinearOpMode {
//Park //Park
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
.lineToLinearHeading(new Pose2d(-65, -10, Math.toRadians(180))) .lineToLinearHeading(new Pose2d(-65, -55, Math.toRadians(180)))
.addTemporalMarker(.3, robot.getArm()::armRest) .addTemporalMarker(.3, robot.getArm()::armRest)
.addTemporalMarker(.3, robot.getWrist()::wristPickup) .addTemporalMarker(.3, robot.getWrist()::wristPickup)
.build(); .build();
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
.lineToLinearHeading(new Pose2d(-80, -10, Math.toRadians(180))) .lineToLinearHeading(new Pose2d(-80, -60, Math.toRadians(180)))
.build(); .build();
// Do super fancy chinese shit // Do super fancy chinese shit

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java
View File

@ -71,7 +71,7 @@ public class AutoBlueFar extends LinearOpMode {
.build(); .build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end()) this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(-49.25, -28, Math.toRadians(180))) .lineToLinearHeading(new Pose2d(-50, -28, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore) .addTemporalMarker(.2, robot.getWrist()::wristScore)
.build(); .build();

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -76,12 +76,12 @@ public class AutoRed extends LinearOpMode {
//Randomization Two //Randomization Two
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(35, -28, Math.toRadians(270))) .lineToLinearHeading(new Pose2d(31, -28, Math.toRadians(270)))
.build(); .build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(72, -33.3, Math.toRadians(360))) .lineToLinearHeading(new Pose2d(70.75, -36.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getArm()::armAccurateScoreAuto)
.addTemporalMarker(.2, robot.getWrist()::wristScore) .addTemporalMarker(.2, robot.getWrist()::wristScore)
.build(); .build();

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java
View File

@ -127,24 +127,24 @@ public class AutoRedFar extends LinearOpMode {
.build(); .build();
this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end()) this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
.lineToLinearHeading(new Pose2d(50, -37, Math.toRadians(360))) .lineToLinearHeading(new Pose2d(50, -39, Math.toRadians(360)))
.addTemporalMarker(.02, robot.getArm()::armAccurateScore) .addTemporalMarker(.02, robot.getArm()::armAccurateScore)
.addTemporalMarker(.02, robot.getWrist()::wristScore) .addTemporalMarker(.02, robot.getWrist()::wristScore)
.build(); .build();
this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end()) this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
.lineToLinearHeading(new Pose2d(40, -47, Math.toRadians(360))) .lineToLinearHeading(new Pose2d(40, -42, Math.toRadians(360)))
.build(); .build();
//Park //Park
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end()) this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
.lineToLinearHeading(new Pose2d(40, -10, Math.toRadians(360))) .lineToLinearHeading(new Pose2d(40, -12, Math.toRadians(360)))
.addTemporalMarker(.3, robot.getArm()::armRest) .addTemporalMarker(.3, robot.getArm()::armRest)
.addTemporalMarker(.3, robot.getWrist()::wristPickup) .addTemporalMarker(.3, robot.getWrist()::wristPickup)
.build(); .build();
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
.lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(360))) .lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(10)))
.build(); .build();
// Do super fancy chinese shit // Do super fancy chinese shit

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
View File

@ -17,20 +17,27 @@ public class MainTeleOp extends OpMode {
@Override @Override
public void loop() { public void loop() {
boolean hang = gamepad2.dpad_up; boolean slideUp = gamepad2.dpad_up;
boolean restArm = gamepad2.dpad_right || gamepad2.x; boolean restArm = gamepad2.x;
boolean pickupArm = gamepad2.dpad_down; boolean pickupArm = gamepad2.dpad_down;
boolean scoreArm = gamepad2.dpad_left || gamepad2.a; boolean scoreArm = gamepad2.a;
boolean accurateScoreArm = gamepad2.right_bumper; boolean plane = gamepad2.right_bumper;
boolean claw = gamepad2.b; boolean claw = gamepad2.b;
boolean pickupWrist = gamepad2.x; boolean pickupWrist = gamepad2.x;
boolean scoreWrist = gamepad2.a || gamepad2.left_bumper; boolean scoreWrist = gamepad2.a;
boolean launch = gamepad2.y; boolean slideDown = gamepad2.dpad_left;
boolean hang = gamepad2.left_bumper;
boolean hangRelease = gamepad2.y;
boolean hangPlane = gamepad2.dpad_right;
//Drive //Drive
robot.getDrive().setInput(gamepad1, gamepad2); robot.getDrive().setInput(gamepad1, gamepad2);
//Hang //slides
if (hang) { if (slideUp) {
this.robot.getHang().release(); this.robot.getSlides().slideUp();
} else if (slideDown) {
this.robot.getSlides().slideDown();
} else {
this.robot.getSlides().slideStop();
} }
//Arm //Arm
if (pickupArm) { if (pickupArm) {
@ -39,8 +46,6 @@ public class MainTeleOp extends OpMode {
this.robot.getArm().armRest(); this.robot.getArm().armRest();
} else if (scoreArm) { } else if (scoreArm) {
this.robot.getArm().armScore(); this.robot.getArm().armScore();
} else if (accurateScoreArm) {
this.robot.getArm().armAccurateScore();
} }
//Claw //Claw
if (claw) { if (claw) {
@ -54,10 +59,23 @@ public class MainTeleOp extends OpMode {
} else if (scoreWrist) { } else if (scoreWrist) {
this.robot.getWrist().wristScore(); this.robot.getWrist().wristScore();
} }
//Plane //Hang
if (launch) { if (hang) {
this.robot.getPlane().planeLaunch(); this.robot.getHang().hang();
} else if (hangRelease){
this.robot.getHang().hangRelease();
} else if (hangPlane) {
this.robot.getHang().hangPlane();
} else { } else {
this.robot.getHang().hangIdle();
}
int Position = this.robot.getHang().hangRight.getCurrentPosition();
telemetry.addData("position",(Position));
telemetry.update();
//Plane
if (plane) {
this.robot.getPlane().planeLaunch();
}else {
this.robot.getPlane().planeLock(); this.robot.getPlane().planeLock();
} }
} }

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -14,19 +14,16 @@ public class Configurables {
public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
//Servo Positions //Servo Positions
public static double UNLOCKSPEED = .1; public static double ARMREST = 0.8;
public static double LOCKSPEED = .25; public static double ARMSCORE = 0.38;
public static double UNLOCK = .6; public static double ARMACCSCORE = 0.6;
public static double LOCK = 0.4; public static double ARMACCSCOREAUTO = 0.6;
public static double ARMREST = .91; public static double PICKUP = 0.84;
public static double ARMSCORE = 0.14 ;
public static double ARMACCSCORE = 0.04;
public static double PICKUP = 0.97;
public static double WRISTPICKUP = 0.28; public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96; public static double WRISTSCORE = .96;
public static double OPEN = 0.53; public static double OPEN = 0.85;
public static double BIGOPEN = 0.45; public static double BIGOPEN = 0.45;
public static double CLOSE = 0.6; public static double CLOSE = 0.95;
public static double PLANELOCK = 0.1; public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9; public static double PLANELAUNCH = 0.9;
@ -36,4 +33,13 @@ public class Configurables {
public static double TURN = 1; public static double TURN = 1;
public static double SLOWMODE_TURN = 0.3; public static double SLOWMODE_TURN = 0.3;
//Motor Positions
public static double SLIDE_POWER_UP = 1;
public static double SLIDE_POWER_DOWN = .7;
public static int SLIDEUP = 1150;
public static int HANGRELEASE = 2500;
public static int HANG = 1000;
public static int HANGPLANE = 1900;
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -57,4 +57,8 @@ public class Constants {
public static final String RIGHTARM = "rightArm"; public static final String RIGHTARM = "rightArm";
public static final String WRIST = "wrist"; public static final String WRIST = "wrist";
public static final String PLANE = "plane"; public static final String PLANE = "plane";
public static final String SLIDERIGHT = "slideright";
public static final String SLIDELEFT = "slideleft";
public static final String HANGRIGHT = "hangright";
public static final String HANGLEFT = "hangleft";
} }