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

RED SIDE AUTO WORKDS COMPLETELY

This commit is contained in:
sihan 2023-11-08 16:18:08 -06:00
parent e84fe97cb4
commit 655f41219e
4 changed files with 145 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,7 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMDOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMUP;
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.ARMREST;
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;
@ -30,13 +31,20 @@ import org.firstinspires.ftc.teamcode.vision.Camera;
import lombok.Getter;
public class Robot {
@Getter private MecanumDrive drive;
@Getter private Intake intake;
@Getter private Arm arm;
@Getter private Wrist wrist;
@Getter private Claw claw;
@Getter private Hang hang;
@Getter private Camera camera;
@Getter
private MecanumDrive drive;
@Getter
private Intake intake;
@Getter
private Arm arm;
@Getter
private Wrist wrist;
@Getter
private Claw claw;
@Getter
private Hang hang;
@Getter
private Camera camera;
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
@ -49,7 +57,7 @@ public class Robot {
return this;
}
public static class Intake{
public static class Intake {
private DcMotor intake = null;
public Intake init(HardwareMap hardwareMap) {
@ -59,11 +67,17 @@ public class Robot {
return this;
}
public void spinIn() {this.intake.setPower(0.7);}
public void spinIn() {
this.intake.setPower(0.7);
}
public void spinOut() {this.intake.setPower(-0.7);}
public void spinOut() {
this.intake.setPower(-0.7);
}
public void stop() {this.intake.setPower(0);}
public void stop() {
this.intake.setPower(0);
}
}
@ -95,12 +109,12 @@ public class Robot {
private Servo rightArm;
public Arm init(HardwareMap hardwareMap){
public Arm init(HardwareMap hardwareMap) {
this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
this.rightArm = hardwareMap.get(Servo.class, RIGHTARM);
this.rightArm.setDirection(Servo.Direction.REVERSE);
this.rightArm.setPosition(ARMUP);
this.leftArm.setPosition(ARMUP);
this.rightArm.setPosition(ARMREST);
this.leftArm.setPosition(ARMREST);
return this;
}
@ -110,20 +124,25 @@ public class Robot {
}
public void armScore() {
this.leftArm.setPosition(ARMDOWN);
this.rightArm.setPosition(ARMDOWN);
this.leftArm.setPosition(ARMSCORE);
this.rightArm.setPosition(ARMSCORE);
}
public void armAccurateScore() {
this.leftArm.setPosition(ARMACCSCORE);
this.rightArm.setPosition(ARMACCSCORE);
}
public void armRest() {
this.leftArm.setPosition(ARMUP);
this.rightArm.setPosition(ARMUP);
this.leftArm.setPosition(ARMREST);
this.rightArm.setPosition(ARMREST);
}
}
public static class Wrist {
private Servo wrist;
public Wrist init(HardwareMap hardwareMap){
public Wrist init(HardwareMap hardwareMap) {
this.wrist = hardwareMap.get(Servo.class, WRIST);
this.wrist.setPosition(WRISTPICKUP);
return this;
@ -141,8 +160,9 @@ public class Robot {
public static class Claw {
private Servo claw;
public Claw init (HardwareMap hardwareMap) {
public Claw init(HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW);
this.claw.setPosition(CLOSE);
return this;
}

116
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java
View File

@ -11,15 +11,32 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
@Autonomous(name = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!")
public class Auto extends LinearOpMode {
protected Pose2d initialPosition;
protected Trajectory preloadOne;
protected Trajectory scoreOne;
protected Trajectory boardOne;
protected Trajectory backOffOne;
protected Trajectory preloadTwo;
protected Trajectory scoreTwo;
protected Trajectory backOffTwo;
protected Trajectory preloadThree;
protected Trajectory scoreThree;
protected Trajectory boardThree;
protected Trajectory backOffThree;
protected Trajectory park1;
protected Trajectory park2;
private Robot robot;
private String randomization;
private int random;
protected Pose2d initialPosition;
protected Trajectory preloadOne;
protected Trajectory scoreOne;
protected Trajectory preloadTwo;
protected Trajectory preloadThree;
@Override
public void runOpMode() throws InterruptedException {
@ -27,27 +44,68 @@ public class Auto extends LinearOpMode {
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().initTargetingCamera();
//Trajectories
//Trajectories
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
//Preload One
this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
.build();
//Randomization One
this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
.build();
this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
.lineToLinearHeading(new Pose2d(33,-30, Math.toRadians(360)))
.lineToLinearHeading(new Pose2d(33, -30, Math.toRadians(360)))
.build();
//Preload Two
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
.lineToLinearHeading(new Pose2d(60, -26, Math.toRadians(360)))
.build();
//Randomization Two
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(35,-27, Math.toRadians(270)))
.lineToLinearHeading(new Pose2d(35, -27, Math.toRadians(270)))
.build();
//Preload Three
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
.lineToLinearHeading(new Pose2d(60, -34, Math.toRadians(360)))
.build();
//Randomization Three
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(46,-35, Math.toRadians(270)))
.lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270)))
.build();
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
.lineToLinearHeading(new Pose2d(60, -40, Math.toRadians(360)))
.build();
//Park
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
.lineToLinearHeading(new Pose2d(65, -10, 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(80, -10, Math.toRadians(360)))
.build();
// Do super fancy chinese shit
@ -61,17 +119,37 @@ public class Auto extends LinearOpMode {
case "LEFT":
this.robot.getDrive().followTrajectory(preloadOne);
this.robot.getDrive().followTrajectory(scoreOne);
this.robot.getDrive().followTrajectory(boardOne);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffOne);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
case "CENTER":
this.robot.getDrive().followTrajectory(preloadTwo);
this.robot.getDrive().followTrajectory(scoreTwo);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffTwo);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
case "RIGHT":
this.robot.getDrive().followTrajectory(preloadThree);
this.robot.getDrive().followTrajectory(scoreThree);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffThree);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
}
}
}
}

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

@ -21,6 +21,7 @@ public class MainTeleOp extends OpMode {
boolean restArm = gamepad2.dpad_left || gamepad2.x;
boolean pickupArm = gamepad2.dpad_down;
boolean scoreArm = gamepad2.dpad_right || gamepad2.a;
boolean accurateScoreArm = gamepad2.y;
boolean claw = gamepad2.b;
boolean pickupWrist = gamepad2.left_bumper || gamepad2.x;
boolean scoreWrist = gamepad2.right_bumper || gamepad2.a;
@ -37,6 +38,8 @@ 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) {

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

@ -18,8 +18,9 @@ public class Configurables {
public static double LOCKSPEED = .25;
public static double UNLOCK = .6;
public static double LOCK = 0.4;
public static double ARMUP = .88;
public static double ARMDOWN = 0.15;
public static double ARMREST = .88;
public static double ARMSCORE = 0.15;
public static double ARMACCSCORE = 0.02;
public static double PICKUP = .935;
public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96;