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

Working refactor

This commit is contained in:
Scott Barnes 2023-11-18 12:40:31 -06:00
parent a335c50c9b
commit cc8cf18ff0
5 changed files with 30 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,7 +22,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection;
@Config @Config
public abstract class AutoBase extends LinearOpMode { public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100; public static int DEPOSIT_HEIGHT = 100;
public static int SCORING_DURATION_MS = 5000; public static double SCORING_DURATION_S = 5f;
protected Robot robot; protected Robot robot;
protected FtcDashboard dashboard; protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry; protected Telemetry dashboardTelemetry;
@ -56,14 +56,14 @@ public abstract class AutoBase extends LinearOpMode {
switch (this.propLocation) { switch (this.propLocation) {
case Left: case Left:
// TODO Tommy: Place the pixel on the left tape and move to rendezvous position // TODO Tommy: Place the pixel on the left tape and move to rendezvous position
break; return;
case Unknown: case Unknown:
case Center: case Center:
dislodgePropAndPlacePixel(); dislodgePropAndPlacePixel();
break; break;
case Right: case Right:
// TODO Tommy: Place the pixel on the right tape and move to rendezvous position // TODO Tommy: Place the pixel on the right tape and move to rendezvous position
break; return;
} }
moveToBackstage(); moveToBackstage();
@ -77,12 +77,18 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut(); this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) { while(this.robot.getGantry().isIn()) {
this.robot.getGantry().update(); this.robot.update();
sleep(20); sleep(20);
} }
this.robot.getGantry().deposit(); this.robot.getGantry().deposit();
this.sleep(SCORING_DURATION_MS); double startTime = this.getRuntime();
while (this.getRuntime() < (startTime + SCORING_DURATION_S)) {
this.robot.update();
}
this.robot.getGantry().stop(); this.robot.getGantry().stop();
this.robot.getGantry().setSlideTarget(0);
this.robot.getGantry().armInSync();
} }
private void prepareToScore() { private void prepareToScore() {
@ -94,6 +100,7 @@ public abstract class AutoBase extends LinearOpMode {
private void moveToBackstage() { private void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0)); builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());

2
TeamCode/src/main/java/opmodes/LeftAuto.java
View File

@ -10,7 +10,7 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
public class LeftAuto extends AutoBase { public class LeftAuto extends AutoBase {
public LeftAuto() { public LeftAuto() {
super( super(
CenterStageCommon.Alliance.Red, CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)), new Pose2d(-36, 63, Math.toRadians(-90)),
new Vector2d(-36, 11)); new Vector2d(-36, 11));
} }

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

@ -82,10 +82,24 @@ public class Gantry {
this.armControllerTarget = GANTRY_ARM_MAX; this.armControllerTarget = GANTRY_ARM_MAX;
} }
public void armOutSync() {
this.armOut();
while (this.armServo.getPosition() < this.armControllerTarget - 0.001) {
this.update();
}
}
public void armIn() { public void armIn() {
this.armControllerTarget = GANTRY_ARM_MIN; this.armControllerTarget = GANTRY_ARM_MIN;
} }
public void armInSync() {
this.armIn();
while (this.armServo.getPosition() > this.armControllerTarget + 0.001) {
this.update();
}
}
public void intake() { public void intake() {
this.screwServo.setPower(-1); this.screwServo.setPower(-1);
} }

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

@ -41,7 +41,7 @@ public class RobotConfig {
public static double GANTRY_ARM_MIN = 0.435; public static double GANTRY_ARM_MIN = 0.435;
public static double GANTRY_ARM_MAX = 0.94; public static double GANTRY_ARM_MAX = 0.94;
public static int GANTRY_LIFT_DELTA = 50; public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_ARM_KP = 0.06; public static double GANTRY_ARM_KP = 0.1;
public static double X_KP = 0.1; public static double X_KP = 0.1;
public static double X_MAX = 0.84; public static double X_MAX = 0.84;
public static double X_MIN = 0.16; public static double X_MIN = 0.16;

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

@ -67,8 +67,8 @@ public class DriveConstants {
*/ */
public static double MAX_VEL = 45; public static double MAX_VEL = 45;
public static double MAX_ACCEL = 45; public static double MAX_ACCEL = 45;
public static double MAX_ANG_VEL = Math.toRadians(60); public static double MAX_ANG_VEL = Math.toRadians(720);
public static double MAX_ANG_ACCEL = Math.toRadians(60); public static double MAX_ANG_ACCEL = Math.toRadians(720);
/* /*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details. * Adjust the orientations here to match your robot. See the FTC SDK documentation for details.