Skip to content

Commit 0b9498a

Browse files
ronanbennettronanbennett
committed
2 parents 6a601b2 + 95d594a commit 0b9498a

File tree

8 files changed

+43
-32
lines changed

8 files changed

+43
-32
lines changed

‎src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,9 @@ private AutoModeCreator(String dashboardName, Supplier<AutoModeBase> creator)
4848
new AutoModeCreator("Middle: Drive Off Middle of Hab and Place Left Close Bay", () -> new PlaceCargoFromMiddleMode(true)),
4949
new AutoModeCreator("Middle: Drive Off Middle of Hab and Palce Right Close Bay", () -> new PlaceCargoFromMiddleMode(false)),
5050

51+
52+
new AutoModeCreator("Other: Drive Back to Depot Test (Left)", () -> new DriveToDepotTestMode(true)),
53+
new AutoModeCreator("Other: Drive Back to Depot Test (Right)", () -> new DriveToDepotTestMode(false)),
5154
new AutoModeCreator("Other: Drive Off HAB Test", () -> new DriveOffHABTestMode()),
5255
new AutoModeCreator("Other: Straight Path Test", () -> new PathTestMode(false)),
5356
new AutoModeCreator("Other: Curved Path Test", () -> new PathTestMode(true)),

‎src/main/java/com/spartronics4915/frc2019/Constants.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ private ScorableLandmark(ScorableLandmark other)
6868
public static final Pose2d kMiddleRobotLocationOffPlatformReverse;
6969
public static final Pose2d kMiddleRobotLocationOffPlatformForward;
7070

71-
public static final double kDriveOffHabXFudgeAmount = -33.0; // inches
71+
public static final double kDriveOffHabXFudgeAmount = -10.0; // inches
7272

7373
public static Pose2d correctPoseForRobotLength(Pose2d oldpose)
7474
{

‎src/main/java/com/spartronics4915/frc2019/auto/modes/DriveToDepotTestMode.java

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package com.spartronics4915.frc2019.auto.modes;
2+
3+
import com.spartronics4915.frc2019.Constants;
4+
import com.spartronics4915.frc2019.auto.AutoModeBase;
5+
import com.spartronics4915.frc2019.auto.AutoModeEndedException;
6+
import com.spartronics4915.frc2019.auto.actions.DriveTrajectory;
7+
import com.spartronics4915.frc2019.paths.TrajectoryGenerator;
8+
import com.spartronics4915.frc2019.paths.TrajectoryGenerator.TrajectorySet;
9+
import com.spartronics4915.frc2019.subsystems.RobotStateEstimator;
10+
11+
public class DriveToDepotTestMode extends AutoModeBase
12+
{
13+
14+
private final boolean mIsLeft;
15+
16+
public DriveToDepotTestMode(boolean isLeft)
17+
{
18+
mIsLeft = isLeft;
19+
}
20+
21+
@Override
22+
protected void routine() throws AutoModeEndedException
23+
{
24+
TrajectorySet tSet = TrajectoryGenerator.getInstance().getTrajectorySet();
25+
26+
runAction(new DriveTrajectory(tSet.driveToDepotFromClosestCargoBay.get(mIsLeft), true));
27+
runAction(new DriveTrajectory(tSet.driveToMiddleCargoBayFromDepot.get(mIsLeft)));
28+
29+
}
30+
31+
}

‎src/main/java/com/spartronics4915/frc2019/auto/modes/PlaceCargoFromSideMode.java

Lines changed: 2 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,7 @@
55
import com.spartronics4915.frc2019.paths.TrajectoryGenerator;
66
import com.spartronics4915.frc2019.paths.TrajectoryGenerator.TrajectorySet;
77
import com.spartronics4915.frc2019.auto.actions.DriveTrajectory;
8-
import com.spartronics4915.frc2019.auto.actions.ZeroOdometryOnHAB;
9-
import com.spartronics4915.frc2019.auto.actions.ZeroOdometryOnHAB.StartPosition;
8+
import com.spartronics4915.frc2019.auto.actions.RunFunctionOnceUntilAction;
109

1110
public class PlaceCargoFromSideMode extends AutoModeBase
1211
{
@@ -21,15 +20,7 @@ public PlaceCargoFromSideMode(boolean isLeft)
2120
protected void routine() throws AutoModeEndedException
2221
{
2322
TrajectorySet tSet = TrajectoryGenerator.getInstance().getTrajectorySet();
24-
if (mIsLeft)
25-
{
26-
runAction(new ZeroOdometryOnHAB(StartPosition.LEFT_PLATFORM));
27-
}
28-
else
29-
{
30-
runAction(new ZeroOdometryOnHAB(StartPosition.RIGHT_PLATFORM));
31-
}
32-
runAction(new DriveTrajectory(tSet.driveToClosestCargoBayFromSide.get(mIsLeft)));
23+
runAction(new DriveTrajectory(tSet.driveToClosestCargoBayFromSide.get(mIsLeft), true));
3324
}
3425

3526
}

‎src/main/java/com/spartronics4915/frc2019/auto/modes/PlaceHatchFromMiddleMode.java

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,7 @@ public PlaceHatchFromMiddleMode(boolean isLeft)
2525
protected void routine() throws AutoModeEndedException
2626
{
2727
TrajectorySet tSet = TrajectoryGenerator.getInstance().getTrajectorySet();
28-
runAction(new ZeroOdometryOnHAB(StartPosition.MIDDLE_PLATFORM));
29-
runAction(new DriveTrajectory(tSet.driveToParallelCargoBayFromMiddle.get(mIsLeft)));
28+
runAction(new DriveTrajectory(tSet.driveToParallelCargoBayFromMiddle.get(mIsLeft), true));
3029
}
3130

3231
}

‎src/main/java/com/spartronics4915/frc2019/auto/modes/PlaceHatchFromSideMode.java

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,15 +22,7 @@ public PlaceHatchFromSideMode(boolean isLeft)
2222
protected void routine() throws AutoModeEndedException
2323
{
2424
TrajectorySet tSet = TrajectoryGenerator.getInstance().getTrajectorySet();
25-
if (mIsLeft)
26-
{
27-
runAction(new ZeroOdometryOnHAB(StartPosition.LEFT_PLATFORM));
28-
}
29-
else
30-
{
31-
runAction(new ZeroOdometryOnHAB(StartPosition.RIGHT_PLATFORM));
32-
}
33-
runAction(new DriveTrajectory(tSet.driveToParallelCargoBayFromSide.get(mIsLeft)));
25+
runAction(new DriveTrajectory(tSet.driveToParallelCargoBayFromSide.get(mIsLeft), true));
3426
// runAction(new RunFunctionOnceUntilAction(() -> Superstructure.getInstance().setWantedState(Superstructure.WantedState.EJECT_PANEL),
3527
// () -> Superstructure.getInstance().isDriverControlled()));
3628
// runAction(new DriveTrajectory(TrajectoryGenerator.getInstance().getTrajectorySet().driveToDepot.get(mIsLeft)));

‎src/main/java/com/spartronics4915/frc2019/paths/TrajectoryGenerator.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public class TrajectoryGenerator
2929
private static final double kMaxVoltage = 9.0; // volts
3030
private static final List<TimingConstraint<Pose2dWithCurvature>> kHabMaxVelocityRegionConstraint =
3131
new ArrayList<TimingConstraint<Pose2dWithCurvature>>(
32-
Arrays.asList(new VelocityLimitRegionConstraint<>(new Translation2d(0, 173), new Translation2d(140, -173), 40)));
32+
Arrays.asList(new VelocityLimitRegionConstraint<>(new Translation2d(0, -173), new Translation2d(120, 173), 15)));
3333

3434
private static TrajectoryGenerator mInstance = new TrajectoryGenerator();
3535
private final DriveMotionPlanner mMotionPlanner;
@@ -218,7 +218,7 @@ private Trajectory<TimedState<Pose2dWithCurvature>> getDriveToClosestCargoBayFro
218218
List<Pose2d> waypoints = new ArrayList<>();
219219
waypoints.add(Constants.kRightRobotLocationOnPlatform);
220220
waypoints.add(Constants.kRightRobotLocationOffPlatform);
221-
// waypoints.add(new Pose2d(190, -90, Rotation2d.fromDegrees(140)));
221+
waypoints.add(new Pose2d(190, -90, Rotation2d.fromDegrees(140)));
222222
waypoints.add(Constants.ScorableLandmark.RIGHT_CLOSE_CARGO_BAY.robotLengthCorrectedPose);
223223
return generateTrajectory(true, waypoints, kHabMaxVelocityRegionConstraint, kMaxVelocity, kMaxAccel, kMaxVoltage);
224224
}
@@ -253,6 +253,7 @@ private Trajectory<TimedState<Pose2dWithCurvature>> getDriveToDepotFromClosestCa
253253
{
254254
List<Pose2d> waypoints = new ArrayList<>();
255255
waypoints.add(Constants.ScorableLandmark.RIGHT_CLOSE_CARGO_BAY.robotLengthCorrectedPose);
256+
waypoints.add(new Pose2d(170, -100, Rotation2d.fromDegrees(178)));
256257
waypoints.add(kRightCargoDepotIntakePose);
257258
return generateTrajectory(false, waypoints);
258259
}

‎src/test/java/com/spartronics4915/frc2019/TestSystemTest.java

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,6 @@
44

55
import static org.junit.jupiter.api.Assertions.assertEquals;
66

7-
import java.util.ArrayList;
8-
import java.util.List;
9-
10-
import com.spartronics4915.lib.geometry.Pose2d;
11-
import com.spartronics4915.lib.geometry.Rotation2d;
12-
137
public class TestSystemTest
148
{
159

@@ -19,7 +13,7 @@ public void aTest()
1913
// assert statements
2014
assertEquals(0, 0, "0 must be 0");
2115

22-
System.out.println(Constants.ScorableLandmark.RIGHT_MIDDLE_CARGO_BAY.robotLengthCorrectedPose);
16+
System.out.println(Constants.ScorableLandmark.RIGHT_CLOSE_CARGO_BAY.robotLengthCorrectedPose);
2317
System.out.println(Constants.kMiddleRobotLocationOffPlatformReverse);
2418
}
2519
}

0 commit comments

Comments
 (0)