Skip to content
This repository has been archived by the owner on Dec 15, 2023. It is now read-only.

Commit

Permalink
Saterdays progress
Browse files Browse the repository at this point in the history
  • Loading branch information
Sean-Borneman committed Jul 9, 2023
1 parent b7fdabd commit beefab6
Showing 2 changed files with 20 additions and 24 deletions.
Original file line number Diff line number Diff line change
@@ -296,7 +296,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(47, -44, Math.toRadians(180)), Math.toRadians(-95));
.splineToLinearHeading(new Pose2d(50.7, -5, Math.toRadians(180)), Math.toRadians(85));
}

@Override
@@ -316,7 +316,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(50.5, -25, Math.toRadians(180)), Math.toRadians(-90));
.splineToLinearHeading(new Pose2d(50.5, -25, Math.toRadians(180)), Math.toRadians(90));
}

@Override
@@ -336,9 +336,10 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(50.7, 5, Math.toRadians(180)), Math.toRadians(-90));
.splineToLinearHeading(new Pose2d(50.5, -44, Math.toRadians(90)), Math.toRadians(90));
}


@Override
public RobotSide GetSide() {
return RobotSide.Red;
Original file line number Diff line number Diff line change
@@ -29,6 +29,7 @@ public class RightAutoAllHigh extends RoadRunnerAutoBase {
Pose2d pickupPosition = new Pose2d(55, -29.0, Math.toRadians(270));
Pose2d highLocation = new Pose2d(57.5, 5, 4.23);//4.23
Pose2d intermediate1 = new Pose2d();

@Override
public void ResolveSubsystems() throws InvocationTargetException, IllegalAccessException, InstantiationException {
arm = (Arm) Container.resolve(Arm.class);
@@ -66,8 +67,8 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
})

//Move through intermediate positions
.splineTo(new Vector2d(40 , -2), 0)
.splineTo(new Vector2d(49.2, -15 ), Math.toRadians(270))
.splineTo(new Vector2d(40, -2), 0)
.splineTo(new Vector2d(49.2, -15), Math.toRadians(270))
.addTemporalMarkerOffset(0, () -> {
claw.setPos(RobotConfig.Presets.WristPickup);
arm.toPosition(RobotConfig.Presets.Arm1HighRev);
@@ -86,7 +87,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
}
})
//Travel to preload place
.lineToLinearHeading(new Pose2d(highLocation.getX() + 5, highLocation.getY()-3, highLocation.getHeading() -0.2))
.lineToLinearHeading(new Pose2d(highLocation.getX() + 5, highLocation.getY() - 3, highLocation.getHeading() - 0.2))
.setConstraints(new TrajectoryVelocityConstraint() {
@Override
public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @NonNull Pose2d pose2d2) {
@@ -117,7 +118,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No


//goto pickup 1st cone------------------------------------------------------------------------------
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY() ), pickupPosition.getHeading())
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY()), pickupPosition.getHeading())
.addTemporalMarkerOffset(0, () -> {
claw.grab();
})
@@ -139,18 +140,16 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Presets.WristPickup);
arm.toPosition(RobotConfig.Presets.Arm1Pickup);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop+151);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop + 151);
//claw.grab();
})
.addTemporalMarkerOffset(0.08, () -> {
claw.ungrab();
})




//goto pickup 2nd cone------------------------------------------------------------------------------
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY() ), pickupPosition.getHeading())
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY()), pickupPosition.getHeading())
.addTemporalMarkerOffset(0, () -> {
claw.grab();
})
@@ -173,15 +172,15 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Presets.WristPickup);
arm.toPosition(RobotConfig.Presets.Arm1Pickup);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop+302);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop + 302);
//claw.grab();
})
.addTemporalMarkerOffset(0.08, () -> {
claw.ungrab();
})

//goto pickup 3rd cone------------------------------------------------------------------------------
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY() ), pickupPosition.getHeading())
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY()), pickupPosition.getHeading())
.addTemporalMarkerOffset(0, () -> {
claw.grab();
})
@@ -204,17 +203,16 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Presets.WristPickup);
arm.toPosition(RobotConfig.Presets.Arm1Pickup);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop+453);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop + 453);
//claw.grab();
})
.addTemporalMarkerOffset(0.08, () -> {
claw.ungrab();
})



//goto pickup 4th cone------------------------------------------------------------------------------
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY() ), pickupPosition.getHeading())
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY()), pickupPosition.getHeading())
.addTemporalMarkerOffset(0, () -> {
claw.grab();
})
@@ -237,15 +235,15 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Presets.WristPickup);
arm.toPosition(RobotConfig.Presets.Arm1Pickup);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop+604);
slides.setTargetPosition(RobotConfig.Presets.SlidesPickupTop + 604);
//claw.grab();
})
.addTemporalMarkerOffset(0.08, () -> {
claw.ungrab();
})

//goto pickup 5th cone------------------------------------------------------------------------------
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY() ), pickupPosition.getHeading())
.splineTo(new Vector2d(pickupPosition.getX(), pickupPosition.getY()), pickupPosition.getHeading())
.addTemporalMarkerOffset(0, () -> {
claw.grab();
})
@@ -277,9 +275,6 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
});





}

@Override
@@ -299,7 +294,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(47, 44 , Math.toRadians(180)), Math.toRadians(85));
.splineToLinearHeading(new Pose2d(47, 44, Math.toRadians(180)), Math.toRadians(85));
}

@Override
@@ -319,7 +314,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(50.5, 25 , Math.toRadians(180)), Math.toRadians(90));
.splineToLinearHeading(new Pose2d(50.5, 25, Math.toRadians(180)), Math.toRadians(90));
}

@Override
@@ -339,7 +334,7 @@ public double get(double v, @NonNull Pose2d pose2d, @NonNull Pose2d pose2d1, @No
.addTemporalMarkerOffset(0.1, () -> {
claw.setPos(RobotConfig.Wrist.startingPosition);
})
.splineToLinearHeading(new Pose2d(50.7 , -5 , Math.toRadians(180)), Math.toRadians(90));
.splineToLinearHeading(new Pose2d(50.7, -5, Math.toRadians(180)), Math.toRadians(90));
}

@Override

0 comments on commit beefab6

Please sign in to comment.