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;
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.ARMSCORE;
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.LOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.HANG;
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.PICKUP;
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.UNLOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP;
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.WRISTSCORE;
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.LEFTHANG;
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.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 com.qualcomm.robotcore.hardware.DcMotor;
@ -36,8 +41,8 @@ import lombok.Getter;
public class Robot {
@Getter
private MecanumDrive drive;
@Getter
private Intake intake;
// @Getter
// private Intake intake;
@Getter
private Arm arm;
@Getter
@ -50,64 +55,111 @@ public class Robot {
private Camera camera;
@Getter
private Plane plane;
@Getter
private Slides slides;
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap);
this.intake = new Intake().init(hardwareMap);
this.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().init(hardwareMap);
this.camera = new Camera(hardwareMap);
this.plane = new Plane().init(hardwareMap);
this.slides= new Slides().init(hardwareMap);
return this;
}
public static class Intake {
private DcMotor intake = null;
public static class Slides {
private DcMotor slidesRight = null;
private DcMotor slidesLeft = null;
public Intake init(HardwareMap hardwareMap) {
this.intake = hardwareMap.dcMotor.get("intake");
this.intake.setDirection(DcMotorSimple.Direction.REVERSE);
this.intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
public Slides init(HardwareMap hardwareMap) {
this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT);
this.slidesRight = hardwareMap.get(DcMotor.class, SLIDERIGHT);
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;
}
public void spinIn() {
this.intake.setPower(0.7);
public void slideTo(int position, double power) {
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() {
this.intake.setPower(-0.7);
}
public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);}
public void stop() {
this.intake.setPower(0);
}
public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);}
public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);}
}
public static class Hang {
private Servo hangLeft;
private Servo hangRight;
public DcMotor hangLeft = null;
public DcMotor hangRight = null;
public Hang init(HardwareMap hardwareMap) {
this.hangLeft = hardwareMap.get(Servo.class, LEFTHANG);
this.hangRight = hardwareMap.get(Servo.class, RIGHTHANG);
this.hangLeft.setPosition(LOCKSPEED);
this.hangRight.setPosition(LOCK);
this.hangLeft = hardwareMap.get(DcMotor.class, HANGLEFT);
this.hangRight = hardwareMap.get(DcMotor.class, HANGRIGHT);
this.hangLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
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;
}
public void lock() {
this.hangLeft.setPosition(LOCKSPEED);
this.hangRight.setPosition(LOCK);
public void hangTo(int hangPos, double daPower) {
this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
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() {
this.hangLeft.setPosition(UNLOCKSPEED);
this.hangRight.setPosition(UNLOCK);
public void hangRelease(){
this.hangTo(HANGRELEASE,1);
}
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 {
@ -139,6 +191,12 @@ public class Robot {
this.rightArm.setPosition(ARMACCSCORE);
}
public void armAccurateScoreAuto() {
this.leftArm.setPosition(ARMACCSCOREAUTO);
this.rightArm.setPosition(ARMACCSCOREAUTO);
}
public void armRest() {
this.leftArm.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();
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.getWrist()::wristScore)
.build();
@ -99,12 +99,12 @@ public class AutoBlue extends LinearOpMode {
//Park
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.getWrist()::wristPickup)
.build();
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();
// 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();
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.getWrist()::wristScore)
.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
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(35, -28, Math.toRadians(270)))
.lineToLinearHeading(new Pose2d(31, -28, Math.toRadians(270)))
.build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(72, -33.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.lineToLinearHeading(new Pose2d(70.75, -36.3, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScoreAuto)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.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();
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.getWrist()::wristScore)
.build();
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();
//Park
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.getWrist()::wristPickup)
.build();
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();
// 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
public void loop() {
boolean hang = gamepad2.dpad_up;
boolean restArm = gamepad2.dpad_right || gamepad2.x;
boolean slideUp = gamepad2.dpad_up;
boolean restArm = gamepad2.x;
boolean pickupArm = gamepad2.dpad_down;
boolean scoreArm = gamepad2.dpad_left || gamepad2.a;
boolean accurateScoreArm = gamepad2.right_bumper;
boolean scoreArm = gamepad2.a;
boolean plane = gamepad2.right_bumper;
boolean claw = gamepad2.b;
boolean pickupWrist = gamepad2.x;
boolean scoreWrist = gamepad2.a || gamepad2.left_bumper;
boolean launch = gamepad2.y;
boolean scoreWrist = gamepad2.a;
boolean slideDown = gamepad2.dpad_left;
boolean hang = gamepad2.left_bumper;
boolean hangRelease = gamepad2.y;
boolean hangPlane = gamepad2.dpad_right;
//Drive
robot.getDrive().setInput(gamepad1, gamepad2);
//Hang
if (hang) {
this.robot.getHang().release();
//slides
if (slideUp) {
this.robot.getSlides().slideUp();
} else if (slideDown) {
this.robot.getSlides().slideDown();
} else {
this.robot.getSlides().slideStop();
}
//Arm
if (pickupArm) {
@ -39,8 +46,6 @@ public class MainTeleOp extends OpMode {
this.robot.getArm().armRest();
} else if (scoreArm) {
this.robot.getArm().armScore();
} else if (accurateScoreArm) {
this.robot.getArm().armAccurateScore();
}
//Claw
if (claw) {
@ -54,10 +59,23 @@ public class MainTeleOp extends OpMode {
} else if (scoreWrist) {
this.robot.getWrist().wristScore();
}
//Plane
if (launch) {
this.robot.getPlane().planeLaunch();
//Hang
if (hang) {
this.robot.getHang().hang();
} else if (hangRelease){
this.robot.getHang().hangRelease();
} else if (hangPlane) {
this.robot.getHang().hangPlane();
} 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();
}
}

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);
//Servo Positions
public static double UNLOCKSPEED = .1;
public static double LOCKSPEED = .25;
public static double UNLOCK = .6;
public static double LOCK = 0.4;
public static double ARMREST = .91;
public static double ARMSCORE = 0.14 ;
public static double ARMACCSCORE = 0.04;
public static double PICKUP = 0.97;
public static double ARMREST = 0.8;
public static double ARMSCORE = 0.38;
public static double ARMACCSCORE = 0.6;
public static double ARMACCSCOREAUTO = 0.6;
public static double PICKUP = 0.84;
public static double WRISTPICKUP = 0.28;
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 CLOSE = 0.6;
public static double CLOSE = 0.95;
public static double PLANELOCK = 0.1;
public static double PLANELAUNCH = 0.9;
@ -36,4 +33,13 @@ public class Configurables {
public static double TURN = 1;
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 WRIST = "wrist";
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";
}