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
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static int SCORING_DURATION_MS = 5000;
public static double SCORING_DURATION_S = 5f;
protected Robot robot;
protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry;
@ -56,14 +56,14 @@ public abstract class AutoBase extends LinearOpMode {
switch (this.propLocation) {
case Left:
// TODO Tommy: Place the pixel on the left tape and move to rendezvous position
break;
return;
case Unknown:
case Center:
dislodgePropAndPlacePixel();
break;
case Right:
// TODO Tommy: Place the pixel on the right tape and move to rendezvous position
break;
return;
}
moveToBackstage();
@ -77,12 +77,18 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) {
this.robot.getGantry().update();
this.robot.update();
sleep(20);
}
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().setSlideTarget(0);
this.robot.getGantry().armInSync();
}
private void prepareToScore() {
@ -94,6 +100,7 @@ public abstract class AutoBase extends LinearOpMode {
private void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.turn(Math.toRadians(90));
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
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 LeftAuto() {
super(
CenterStageCommon.Alliance.Red,
CenterStageCommon.Alliance.Blue,
new Pose2d(-36, 63, Math.toRadians(-90)),
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;
}
public void armOutSync() {
this.armOut();
while (this.armServo.getPosition() < this.armControllerTarget - 0.001) {
this.update();
}
}
public void armIn() {
this.armControllerTarget = GANTRY_ARM_MIN;
}
public void armInSync() {
this.armIn();
while (this.armServo.getPosition() > this.armControllerTarget + 0.001) {
this.update();
}
}
public void intake() {
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_MAX = 0.94;
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_MAX = 0.84;
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_ACCEL = 45;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);
public static double MAX_ANG_VEL = Math.toRadians(720);
public static double MAX_ANG_ACCEL = Math.toRadians(720);
/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.