Skip to content

Commit

Permalink
auto!
Browse files Browse the repository at this point in the history
  • Loading branch information
@richpant
richpant committed Nov 4, 2024
1 parent 0757618 commit c99a699
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 16 deletions.
8 changes: 5 additions & 3 deletions TeamCode/src/main/java/indubitables/config/runmodes/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,11 +193,11 @@ public void buildPaths() {
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Point(56.000, 16.000, Point.CARTESIAN),new Point(24, 16.000, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierCurve(new Point(24, 16.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN), new Point(56.000, 8.000, Point.CARTESIAN)))
.addPath(new BezierCurve(new Point(24, 16.000, Point.CARTESIAN), new Point(56.000, 16.000, Point.CARTESIAN), new Point(56.000, 9.50, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Point(56.000, 8.00, Point.CARTESIAN), new Point(24, 8, Point.CARTESIAN)))
.addPath(new BezierLine(new Point(56.000, 9.50, Point.CARTESIAN), new Point(24, 9.5, Point.CARTESIAN)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Point(24, 8, Point.CARTESIAN), new Point(blueObservationSpecimenSetPose)))
.addPath(new BezierLine(new Point(24, 9.5, Point.CARTESIAN), new Point(blueObservationSpecimenSetPose)))
.setLinearHeadingInterpolation(Math.toRadians(0), blueObservationSpecimenSetPose.getHeading())
.build();

Expand Down Expand Up @@ -320,6 +320,7 @@ public void chamber() {
switch (chamberState) {
case 1:
actionBusy = true;
lift.manual = false;
intake.pivotTransfer();
intake.spinStop();
claw.close();
Expand Down Expand Up @@ -361,6 +362,7 @@ public void specimen() {
switch (specimenState) {
case 1:
actionBusy = true;
lift.manual = false;
claw.open();
lift.toHumanPlayer();
extend.toZero();
Expand Down
6 changes: 3 additions & 3 deletions TeamCode/src/main/java/indubitables/config/util/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ public enum RobotStart {
public static final Pose blueObservationElement2ControlPose = new Pose(20, 96);
public static final Pose blueObservationElement3Pose = new Pose(20, 96, Math.toRadians(180));
public static final Pose blueObservationElement3ControlPose = new Pose(20, 110);
public static final Pose blueObservationSpecimenSetPose = new Pose(20, 25, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickupPose = new Pose(16, 27.25, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickup2Pose = new Pose(16, 27.25, Math.toRadians(0));
public static final Pose blueObservationSpecimenSetPose = new Pose(22, 27.5, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickupPose = new Pose(17, 27.5, Math.toRadians(0));
public static final Pose blueObservationSpecimenPickup2Pose = new Pose(18.5, 27.5, Math.toRadians(0));
public static final Pose blueObservationSpecimen1Pose = new Pose(30.25, 70.625, Math.toRadians(180));
public static final Pose blueObservationSpecimen2Pose = new Pose(30.25, 73.625, Math.toRadians(180));
public static final Pose blueObservationSpecimen3Pose = new Pose(30.25, 76.625, Math.toRadians(180));
Expand Down
20 changes: 10 additions & 10 deletions TeamCode/src/main/java/indubitables/opmode/BlueObservation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,18 +62,18 @@ public void pathUpdate() {
break;
case 2: //Once Chamber State Machine Machine finishes, begins Pathchain to push elements to the submersible
if(auto.actionNotBusy()) {
auto.follower.setMaxPower(0.8);
auto.follower.setMaxPower(0.7);
auto.follower.followPath(auto.pushSamples, true);
setPathState(3); }
break;
case 3: //Waits until follower reaches it's position then begins the Specimen State Machine
if(!auto.follower.isBusy()) {
auto.follower.setMaxPower(0.5);
auto.startSpecimen();
setPathState(4); }
break;
case 4: //Runs to the position of the grab1 and holds it's point at full power
if(auto.actionNotBusy() && !auto.follower.isBusy()) {
auto.follower.setMaxPower(0.5);
auto.follower.followPath(auto.grab1, true);
setPathState(5); }
break;
Expand All @@ -91,23 +91,24 @@ public void pathUpdate() {
break;
case 7: //Resets the encoders and begins driving to the chamber
if(pathTimer.getElapsedTimeSeconds() > 1) {
auto.liftPIDF = true;
auto.liftManual = 0;
auto.liftPIDF = true;
auto.lift.rightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
auto.lift.rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
auto.follower.setMaxPower(1);
auto.follower.followPath(auto.specimen1, true);
setPathState(8); }
break;
case 8: //Waits until follower reaches it's position then begins the Chamber State Machine
if((auto.follower.getPose().getX() > auto.specimen1Pose.getX()) && (auto.follower.getPose().getY() >= auto.specimen1Pose.getY())) {
if(pathTimer.getElapsedTimeSeconds() > 0.5) {
auto.liftPIDF = true;
auto.startChamber();
setPathState(9); }
break;
case 9: //Runs to the position of the where it will pick up and holds it's point at 0.7 power
if(auto.actionNotBusy()) {
auto.lift.toZero();
auto.follower.setMaxPower(0.7);
auto.follower.setMaxPower(0.5);
auto.follower.followPath(auto.lineUp2);
setPathState(10); }
break;
Expand All @@ -118,9 +119,9 @@ public void pathUpdate() {
break;
case 11: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds
if(auto.actionNotBusy() && !auto.follower.isBusy()) {
auto.follower.setMaxPower(0.7);
auto.follower.followPath(auto.grab2, true);
pathTimer.resetTimer();
if(pathTimer.getElapsedTimeSeconds() > 0.75) {
if(pathTimer.getElapsedTimeSeconds() > 2.5) {
auto.claw.close();
setPathState(12); } }
break;
Expand All @@ -130,13 +131,12 @@ public void pathUpdate() {
setPathState(13); }
break;
case 13: //Drives to chamber once action finishes
if(auto.actionNotBusy()) {
auto.follower.setMaxPower(1);
auto.follower.followPath(auto.specimen2, true);
setPathState(14); }
setPathState(14);
break;
case 14: //Starts the Chamber State Machine
if(auto.follower.getPose().getX() > auto.specimen2Pose.getX()) {
if(pathTimer.getElapsedTimeSeconds() > 0.5) {
auto.startChamber();
setPathState(15); }
break;
Expand Down

0 comments on commit c99a699

Please sign in to comment.