Skip to content

Commit

Permalink
Cause I'm hopeful, yes I am, hopeful for today,
Browse files Browse the repository at this point in the history 
Take this music and use it
Let it take you away,
And be hopeful, hopeful and he'll make a way
I know it ain't easy but that's okay.
  • Loading branch information
@kuek64
kuek64 committed Nov 22, 2024
1 parent faa4cf4 commit 8366470
Show file tree
Hide file tree
Showing 3 changed files with 78 additions and 104 deletions.
37 changes: 13 additions & 24 deletions TeamCode/src/main/java/indubitables/config/runmodes/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ public void update() {
telemetryUpdate();
}

public void createPoses() {
public void createPoses() { //Able to be cut
switch (startLocation) {
case BLUE_BUCKET:
startPose = blueBucketStartPose;
Expand Down Expand Up @@ -151,7 +151,6 @@ public void createPoses() {
}

public void buildPaths() {

if((startLocation == RobotStart.BLUE_BUCKET) || (startLocation == RobotStart.RED_BUCKET)) {
preload = follower.pathBuilder()
.addPath(new BezierLine(new Point(startPose), new Point(preloadPose)))
Expand Down Expand Up @@ -186,7 +185,7 @@ public void buildPaths() {
preload = follower.pathBuilder()
.addPath(new BezierLine(new Point(startPose), new Point(preloadPose)))
.setLinearHeadingInterpolation(startPose.getHeading(), preloadPose.getHeading())
.setZeroPowerAccelerationMultiplier(1)
.setZeroPowerAccelerationMultiplier(0.25)
.build();

pushSamples = follower.pathBuilder()
Expand All @@ -210,7 +209,7 @@ public void buildPaths() {
grab1 = follower.pathBuilder()
.addPath(new BezierLine(new Point(specimenSetPose), new Point(grab1Pose)))
.setLinearHeadingInterpolation(specimenSetPose.getHeading(), grab1Pose.getHeading())
//.setZeroPowerAccelerationMultiplier(0.25)
.setZeroPowerAccelerationMultiplier(0.25)
.build();

specimen1 = follower.pathBuilder()
Expand All @@ -222,7 +221,7 @@ public void buildPaths() {
grab2 = follower.pathBuilder()
.addPath(new BezierLine(new Point(specimen1Pose), new Point(grab2Pose)))
.setLinearHeadingInterpolation(specimen1Pose.getHeading(), grab2Pose.getHeading())
//.setZeroPowerAccelerationMultiplier(1)
.setZeroPowerAccelerationMultiplier(0.25)
.build();

specimen2 = follower.pathBuilder()
Expand All @@ -234,7 +233,7 @@ public void buildPaths() {
grab3 = follower.pathBuilder()
.addPath(new BezierLine(new Point(specimen2Pose), new Point(grab3Pose)))
.setLinearHeadingInterpolation(specimen2Pose.getHeading(), grab3Pose.getHeading())
//.setZeroPowerAccelerationMultiplier(1)
.setZeroPowerAccelerationMultiplier(0.25)
.build();

specimen3 = follower.pathBuilder()
Expand All @@ -246,7 +245,7 @@ public void buildPaths() {
grab4 = follower.pathBuilder()
.addPath(new BezierLine(new Point(specimen3Pose), new Point(grab4Pose)))
.setLinearHeadingInterpolation(specimen3Pose.getHeading(), grab4Pose.getHeading())
//.setZeroPowerAccelerationMultiplier(1)
.setZeroPowerAccelerationMultiplier(0.25)
.build();

specimen4 = follower.pathBuilder()
Expand All @@ -258,7 +257,7 @@ public void buildPaths() {
park = follower.pathBuilder()
.addPath(new BezierLine(new Point(specimen4Pose), new Point(parkPose)))
.setLinearHeadingInterpolation(specimen4Pose.getHeading(), parkPose.getHeading())
//.setZeroPowerAccelerationMultiplier(0.5)
//.setZeroPowerAccelerationMultiplier(0.25)
.build();

}
Expand Down Expand Up @@ -357,7 +356,7 @@ public void chamber() {
actionBusy = true;
arm.specimenScore();
claw.close();
claw.score();
claw.specimenScore();
extend.toZero();
chamberTimer.resetTimer();
setChamberState(2);
Expand All @@ -369,29 +368,19 @@ public void chamber() {
}
break;
case 3:
if(chamberTimer.getElapsedTimeSeconds() > 0.35) {
claw.specimenScore();
arm.score();
chamberTimer.resetTimer();
setChamberState(4);
}
break;
case 4:
if(chamberTimer.getElapsedTimeSeconds() > 0.25) {
claw.score();
claw.open();
arm.score();
actionBusy = false;
setChamberState(-1);
}
break;
}
}

public void setChamberState(int x) {
chamberState = x;
}

public void startChamber2() {
public void startChamber() {
if(actionNotBusy()) {
setChamberState(1);
}
Expand Down Expand Up @@ -524,9 +513,9 @@ public void telemetryUpdate() {
telemetry.addData("Y: ", follower.getPose().getY());
telemetry.addData("Heading: ", follower.getPose().getHeading());
telemetry.addData("Action Busy?: ", actionBusy);
telemetry.addData("Lift Pos", lift.getPos());
telemetry.addData("Extend Pos", extend.leftExtend.getPosition());
telemetry.addData("Extend Limit", extend.extendLimit);
//telemetry.addData("Lift Pos", lift.getPos());
//telemetry.addData("Extend Pos", extend.leftExtend.getPosition());
//telemetry.addData("Extend Limit", extend.extendLimit);
telemetry.addData("Claw Grab State", claw.grabState);
telemetry.addData("Claw Pivot State", claw.pivotState);
// telemetry.addData("Intake Spin State", intakeSpinState);
Expand Down
12 changes: 6 additions & 6 deletions TeamCode/src/main/java/indubitables/config/util/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@ public enum RobotStart {
public static final Pose redBucketStartPose = new Pose(144-blueBucketStartPose.getX(), blueBucketStartPose.getY(), 0);
public static final Pose redObservationStartPose = new Pose(144-blueObservationStartPose.getX(), blueObservationStartPose.getY(), 0);

// Preload Poses+
// Preload Poses
public static final Pose blueBucketPreloadPose = new Pose(29.25, 78.375, Math.toRadians(180));
public static final Pose blueObservationPreloadPose = new Pose(30.75, 60.625, Math.toRadians(180));
public static final Pose blueObservationPreloadPose = new Pose(32, 60.625, Math.toRadians(180));
public static final Pose redBucketPreloadPose = new Pose(144-blueBucketPreloadPose.getX(), blueBucketPreloadPose.getY(), 0);
public static final Pose redObservationPreloadPose = new Pose(144-blueObservationPreloadPose.getX(), blueObservationPreloadPose.getY(), 0);

Expand All @@ -40,10 +40,10 @@ public enum RobotStart {
public static final Pose blueObservationSpecimenPickup2Pose = new Pose(7, 35, Math.toRadians(180));
public static final Pose blueObservationSpecimenPickup3Pose = new Pose(7, 35, Math.toRadians(180));
public static final Pose blueObservationSpecimenPickup4Pose = new Pose(7, 35, Math.toRadians(180));
public static final Pose blueObservationSpecimen1Pose = new Pose(30.75, 66.25, Math.toRadians(180));
public static final Pose blueObservationSpecimen2Pose = new Pose(30.75, 70.75, Math.toRadians(180));
public static final Pose blueObservationSpecimen3Pose = new Pose(30.75, 74.25, Math.toRadians(180));
public static final Pose blueObservationSpecimen4Pose = new Pose(30.75, 78.25, Math.toRadians(180));
public static final Pose blueObservationSpecimen1Pose = new Pose(32, 66.25, Math.toRadians(180));
public static final Pose blueObservationSpecimen2Pose = new Pose(32, 70.75, Math.toRadians(180));
public static final Pose blueObservationSpecimen3Pose = new Pose(32, 74.25, Math.toRadians(180));
public static final Pose blueObservationSpecimen4Pose = new Pose(32, 78.25, Math.toRadians(180));


public static final Pose blueBucketParkPose = new Pose(62, 97.75, Math.toRadians(90));
Expand Down
Loading

0 comments on commit 8366470

Please sign in to comment.