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

Speed up auto

This commit is contained in:
Scott Barnes 2024-02-08 19:01:26 -06:00
parent a036de64ab
commit 6cfd2876bd
6 changed files with 56 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -24,7 +24,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection;
@Config
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static double SCORING_DURATION_S = 3f; // for spin of axle
public static double SCORING_DURATION_S = 2f; // for spin of axle
public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 7.0;
protected static double Delay = 5000;
@ -61,10 +61,11 @@ public abstract class AutoBase extends LinearOpMode {
this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance);
this.dashboard = FtcDashboard.getInstance();
this.dashboardTelemetry = dashboard.getTelemetry();
this.park = parkLeft;
// Wait for match to start
while(!isStarted() && !isStopRequested()) {
this.robot.update();
this.sleep(20);
boolean leftPressed = gamepad1.dpad_left;
boolean rightPressed = gamepad1.dpad_right;
@ -73,21 +74,22 @@ public abstract class AutoBase extends LinearOpMode {
this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", "");
if(leftPressed && !leftWasPressed) {
this.park = parkLeft;
this.telemetry.addData("Park set to", "Left");
} else if(rightPressed && !rightWasPressed) {
this.park = parkRight;
this.telemetry.addData("Park set to", "Right");
} else if(upPressed && !upWasPressed) {
this.delay += 1000;
this.telemetry.addData("Delay increased by", "1000");
this.sleep(200);
} else if(downPressed && !downWasPressed) {
this.delay -= 1000;
this.telemetry.addData("Delay decreased by", "1000");
this.sleep(200);
}
this.leftWasPressed = leftPressed;
this.rightWasPressed = rightPressed;
this.upWasPressed = upPressed;
this.downWasPressed = downPressed;
this.telemetry.addData("Delay", this.delay);
this.telemetry.addData("Park set to", this.park);
this.sleep(20);
}
if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering
return;
@ -109,7 +111,7 @@ public abstract class AutoBase extends LinearOpMode {
break;
}
this.sleep(delay);
moveToEasel();
moveBackstage();
prepareToScore();
scorePreloadedPixel();
park();
@ -145,6 +147,35 @@ public abstract class AutoBase extends LinearOpMode {
}
protected void prepareToScore() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
this.robot.getGantry().armOut();
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToLinearHeading(new Pose2d(35, 36, 0));
} else if (this.alliance == CenterStageCommon.Alliance.Red) {
builder.lineToLinearHeading(new Pose2d(35, -35, 0));
}
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
Pose2d inferredPos = null;
while(this.robot.getDrive().isBusy()) {
this.robot.update();
inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
if (inferredPos != null) {
this.robot.getDrive().breakFollowing();
break;
}
}
sleep(200);
inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
if (inferredPos == null) {
inferredPos = this.robot.getDrive().getPoseEstimate();
}
// At this point we know that Y = 38
// For 2 -> Ydelta = 0
// For 3 -> 3 5/8
@ -163,20 +194,19 @@ public abstract class AutoBase extends LinearOpMode {
break;
}
Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
// inferredPos = inferredPos != null ? inferredPos : robot.getDrive().getPoseEstimate(); // new code (maybe get rid of)
this.robot.getDrive().setPoseEstimate(inferredPos);
builder = this.robot.getDrive().trajectorySequenceBuilder(inferredPos);
Pose2d target = new Pose2d(
60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
inferredPos.getY() + delta,
36 + delta,
0);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while(this.robot.getDrive().isBusy()) {
this.robot.update();
}
}
protected void moveToEasel() {
protected void moveBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
if (!this.isBackstage()) {
@ -187,21 +217,14 @@ public abstract class AutoBase extends LinearOpMode {
builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI));
builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI));
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
if (this.alliance == CenterStageCommon.Alliance.Blue) {
builder.lineToLinearHeading(new Pose2d(35, 36, 0));
} else if (this.alliance == CenterStageCommon.Alliance.Red) {
builder.lineToLinearHeading(new Pose2d(35, -35, 0));
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void determinePropLocation() {
this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync
while (!this.robot.getClaw().isArmAtPosition()) {
while (!this.robot.getClaw().isArmAtPosition() && !this.robot.getCamera().getProp().isValid()) {
this.robot.update();
sleep(20);
}

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java
View File

@ -74,6 +74,7 @@ public class Claw implements Updatable {
this.clawController.setP(CLAW_KP);
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(0.008);
this.armController.setP(CLAW_ARM_KP);
if (!this.clawController.atSetPoint()) {

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -41,6 +41,7 @@ public class Gantry {
this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME);
this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME);
this.armServo.setPosition(GANTRY_ARM_MIN);
this.armControllerTarget = GANTRY_ARM_MIN;
this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME);
this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@ -127,7 +128,7 @@ public class Gantry {
public void update() {
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(0.001);
this.armController.setTolerance(0.0001);
this.armController.setP(GANTRY_ARM_KP);
this.xController.setSetPoint(this.xControllerTarget);

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

@ -41,7 +41,7 @@ public class RobotConfig {
public static double CLAW_ARM_KP = 0.15;
// Gantry
public static double GANTRY_ARM_MIN = 0.43; // Changed 0.42 --> 0.45 pickup position
public static double GANTRY_ARM_MIN = 0.435; // Changed 0.42 --> 0.45 pickup position
public static double GANTRY_ARM_MAX = 0.96;
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_ARM_KP = 0.1;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -65,8 +65,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 45;
public static double MAX_ACCEL = 45;
public static double MAX_VEL = 55;
public static double MAX_ACCEL = 55;
public static double MAX_ANG_VEL = Math.toRadians(720);
public static double MAX_ANG_ACCEL = Math.toRadians(720);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -94,7 +94,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.1);
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);