Skip to content

Commit b1435ca

Browse files
NoobinOutNoobinOut
committed
Adding a plan B
1 parent 7a0f6da commit b1435ca

File tree

5 files changed

+264
-101
lines changed

5 files changed

+264
-101
lines changed

‎src/main/deploy/pathplanner/paths/New Path.path‎

Lines changed: 0 additions & 54 deletions
This file was deleted.

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
import edu.wpi.first.apriltag.AprilTagFields;
1111
import edu.wpi.first.math.Matrix;
1212
import edu.wpi.first.math.VecBuilder;
13+
import edu.wpi.first.math.geometry.Pose2d;
14+
import edu.wpi.first.math.geometry.Rotation2d;
1315
import edu.wpi.first.math.geometry.Rotation3d;
1416
import edu.wpi.first.math.geometry.Transform3d;
1517
import edu.wpi.first.math.geometry.Translation3d;
@@ -31,6 +33,119 @@
3133
*/
3234
public final class Constants {
3335

36+
private static double distanceAway = 1;
37+
38+
public static Pose2d[] reefFaces = {
39+
new Pose2d(
40+
Units.inchesToMeters(144.003), Units.inchesToMeters(158.500), Rotation2d.fromDegrees(180)),
41+
new Pose2d(
42+
Units.inchesToMeters(160.373), Units.inchesToMeters(186.857), Rotation2d.fromDegrees(120)),
43+
new Pose2d(
44+
Units.inchesToMeters(193.116), Units.inchesToMeters(186.858), Rotation2d.fromDegrees(60)),
45+
new Pose2d(
46+
Units.inchesToMeters(209.489), Units.inchesToMeters(158.502), Rotation2d.fromDegrees(0)),
47+
new Pose2d(
48+
Units.inchesToMeters(193.118), Units.inchesToMeters(130.145), Rotation2d.fromDegrees(-60)),
49+
new Pose2d(
50+
Units.inchesToMeters(160.375), Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)),
51+
new Pose2d(
52+
Units.inchesToMeters(68) + 0.5,
53+
Units.inchesToMeters(71) + 0.5,
54+
Rotation2d.fromDegrees(-126)), // this is Right source, angle could be inaccurate
55+
new Pose2d(
56+
Units.inchesToMeters(67.5) + 0.5,
57+
Units.inchesToMeters(244.5) - 0.5,
58+
Rotation2d.fromDegrees(126)), // this is left source, angle could be inaccurate
59+
new Pose2d(
60+
Units.inchesToMeters(240),
61+
Units.inchesToMeters(55) + 0.75,
62+
Rotation2d.fromDegrees(-90)) // this is processer, angle could be inaccurate
63+
};
64+
65+
public enum ReefScorePositions {
66+
FRONT(
67+
new Pose2d(
68+
Math.cos(reefFaces[0].getRotation().getRadians()) * distanceAway
69+
+ reefFaces[0].getTranslation().getX(),
70+
Math.sin(reefFaces[0].getRotation().getRadians()) * distanceAway
71+
+ reefFaces[0].getTranslation().getY(),
72+
reefFaces[0].getRotation()),
73+
18),
74+
FRONTLEFT(
75+
new Pose2d(
76+
Math.cos(reefFaces[1].getRotation().getRadians()) * distanceAway
77+
+ reefFaces[1].getTranslation().getX(),
78+
Math.sin(reefFaces[1].getRotation().getRadians()) * distanceAway
79+
+ reefFaces[1].getTranslation().getY(),
80+
reefFaces[1].getRotation()),
81+
19),
82+
83+
BACKLEFT(
84+
new Pose2d(
85+
Math.cos(reefFaces[2].getRotation().getRadians()) * distanceAway
86+
+ reefFaces[2].getTranslation().getX(),
87+
Math.sin(reefFaces[2].getRotation().getRadians()) * distanceAway
88+
+ reefFaces[2].getTranslation().getY(),
89+
reefFaces[2].getRotation()),
90+
20),
91+
BACK(
92+
new Pose2d(
93+
Math.cos(reefFaces[3].getRotation().getRadians()) * distanceAway
94+
+ reefFaces[3].getTranslation().getX(),
95+
Math.sin(reefFaces[3].getRotation().getRadians()) * distanceAway
96+
+ reefFaces[3].getTranslation().getY(),
97+
reefFaces[3].getRotation()),
98+
21),
99+
BACKRIGHT(
100+
new Pose2d(
101+
Math.cos(reefFaces[4].getRotation().getRadians()) * distanceAway
102+
+ reefFaces[4].getTranslation().getX(),
103+
Math.sin(reefFaces[4].getRotation().getRadians()) * distanceAway
104+
+ reefFaces[4].getTranslation().getY(),
105+
reefFaces[4].getRotation()),
106+
22),
107+
FRONTRIGHT(
108+
new Pose2d(
109+
Math.cos(reefFaces[5].getRotation().getRadians()) * distanceAway
110+
+ reefFaces[5].getTranslation().getX(),
111+
Math.sin(reefFaces[5].getRotation().getRadians()) * distanceAway
112+
+ reefFaces[5].getTranslation().getY(),
113+
reefFaces[5].getRotation()),
114+
17),
115+
RIGHTSOURCE(
116+
new Pose2d(
117+
Math.cos(reefFaces[6].getRotation().getRadians()) * distanceAway
118+
+ reefFaces[6].getTranslation().getX(),
119+
Math.sin(reefFaces[6].getRotation().getRadians()) * distanceAway
120+
+ reefFaces[6].getTranslation().getY(),
121+
reefFaces[6].getRotation()),
122+
12),
123+
LEFTSOURCE(
124+
new Pose2d(
125+
Math.cos(reefFaces[7].getRotation().getRadians()) * distanceAway
126+
+ reefFaces[7].getTranslation().getX(),
127+
Math.sin(reefFaces[7].getRotation().getRadians()) * distanceAway
128+
+ reefFaces[7].getTranslation().getY(),
129+
reefFaces[7].getRotation()),
130+
13),
131+
PROCESSER(
132+
new Pose2d(
133+
Math.cos(reefFaces[8].getRotation().getRadians()) * distanceAway
134+
+ reefFaces[8].getTranslation().getX(),
135+
Math.sin(reefFaces[8].getRotation().getRadians()) * distanceAway
136+
+ reefFaces[8].getTranslation().getY(),
137+
reefFaces[8].getRotation()),
138+
16);
139+
140+
public Pose2d scorePosition;
141+
public int aprilTagID;
142+
143+
private ReefScorePositions(Pose2d pose, int aprilTagID) {
144+
this.scorePosition = pose;
145+
this.aprilTagID = aprilTagID;
146+
}
147+
}
148+
34149
public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
35150
public static final Matter CHASSIS =
36151
new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1717
import edu.wpi.first.wpilibj2.command.CommandScheduler;
1818
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
19+
import frc.robot.Constants.ReefScorePositions;
1920
import frc.robot.subsystems.FlexAutoSubsystem;
2021
import frc.robot.subsystems.UltrasonicSensor;
2122
import java.util.List;
@@ -41,6 +42,10 @@ public class Robot extends TimedRobot {
4142
private final SendableChooser<String> m_chooser = new SendableChooser<>();
4243
private final SendableChooser<String> m_CoralStationChooser = new SendableChooser<>();
4344

45+
public static final SendableChooser<String> assistSendableChooser = new SendableChooser<>();
46+
public static final SendableChooser<ReefScorePositions> desiredScoreSendableChooser =
47+
new SendableChooser<>();
48+
4449
public Robot() {
4550
SmartDashboard.putBoolean("FlexAuto", false);
4651
SmartDashboard.putBoolean("", false);
@@ -66,6 +71,9 @@ public Robot() {
6671
m_chooser.addOption("Wait", "wait");
6772
SmartDashboard.putData(m_chooser);
6873

74+
assistSendableChooser.setDefaultOption("Plan A", "A");
75+
assistSendableChooser.addOption("Plan B", "B");
76+
6977
m_CoralStationChooser.setDefaultOption("LeftCoralStation", "left");
7078
m_CoralStationChooser.addOption("RightCoralStation", "right");
7179
m_CoralStationChooser.addOption("Stop", "stop");
@@ -121,6 +129,16 @@ public void robotPeriodic() {
121129
// to set the levels
122130
// SmartDashboard.putNumber("ElevEncoder:",
123131
// RobotContainer.elevatorSubsystem.getPositionEncoder());
132+
if (assistSendableChooser.getSelected().equals("B")) {
133+
SmartDashboard.putNumber("getAutoAlignOffsetX", 0);
134+
desiredScoreSendableChooser.setDefaultOption("FRONTLEFT", ReefScorePositions.FRONTLEFT);
135+
desiredScoreSendableChooser.addOption("FRONT", ReefScorePositions.FRONT);
136+
desiredScoreSendableChooser.addOption("FRONTRIGHT", ReefScorePositions.FRONTRIGHT);
137+
desiredScoreSendableChooser.addOption("BACKRIGHT", ReefScorePositions.BACKRIGHT);
138+
desiredScoreSendableChooser.addOption("BACK", ReefScorePositions.BACK);
139+
desiredScoreSendableChooser.addOption("BACKLEFT", ReefScorePositions.BACKLEFT);
140+
desiredScoreSendableChooser.addOption("PROCESSER", ReefScorePositions.PROCESSER);
141+
}
124142
}
125143

126144
/** This function is called once each time the robot enters Disabled mode. */

‎src/main/java/frc/robot/commands/AlignWithNearest.java‎

Lines changed: 76 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,35 @@
11
package frc.robot.commands;
22

3+
import com.pathplanner.lib.auto.AutoBuilder;
4+
import com.pathplanner.lib.path.PathConstraints;
35
import edu.wpi.first.math.controller.PIDController;
46
import edu.wpi.first.math.filter.SlewRateLimiter;
57
import edu.wpi.first.math.geometry.Pose2d;
68
import edu.wpi.first.math.geometry.Rotation2d;
79
import edu.wpi.first.math.kinematics.ChassisSpeeds;
810
import edu.wpi.first.math.util.Units;
11+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
912
import edu.wpi.first.wpilibj2.command.Command;
13+
import frc.robot.Robot;
1014
import frc.robot.RobotContainer;
15+
import frc.robot.subsystems.AllianceFlipUtil;
1116
import frc.robot.subsystems.VisionSubsystem;
1217

1318
public class AlignWithNearest extends Command {
1419

20+
public Pose2d getSelectedPose() {
21+
return AllianceFlipUtil.apply(Robot.desiredScoreSendableChooser.getSelected().scorePosition);
22+
}
23+
1524
public static String name = frc.robot.Constants.VisionSubsystemConstants.limelightName;
1625
private final SlewRateLimiter xLimiter, yLimiter, giroLimiter;
1726
private final PIDController drivePID, strafePID, rotationPID;
1827
private final double driveOffset, strafeOffset, rotationOffset;
28+
private Command pathCommand;
29+
30+
private Pose2d targetPose;
31+
private Pose2d robotPose;
32+
private double distanceAway = -0.55;
1933

2034
public static Pose2d[] TagPos = {
2135
new Pose2d(16.408, 1.048, new Rotation2d(-0.9075712)),
@@ -90,65 +104,80 @@ public AlignWithNearest() {
90104

91105
@Override
92106
public void initialize() {
93-
/*
94-
if (DriverStation.getAlliance().get().equals(Alliance.Red)) {
95-
targetCommand = RobotContainer.drivebase.driveToPose(new Pose2d(13, 4, new Rotation2d(0)));
96-
} else {
97-
targetCommand = RobotContainer.drivebase.driveToPose(new Pose2d(4, 4, new Rotation2d(0)));
107+
if (Robot.assistSendableChooser.getSelected().equals("B")) {
108+
Pose2d selectedPosition = getSelectedPose();
109+
110+
targetPose =
111+
new Pose2d(
112+
Math.cos(selectedPosition.getRotation().getRadians()) * distanceAway
113+
- Math.sin(selectedPosition.getRotation().getRadians())
114+
* SmartDashboard.getNumber("getAutoAlignOffsetX", 0)
115+
+ selectedPosition.getTranslation().getX(),
116+
Math.sin(selectedPosition.getRotation().getRadians()) * distanceAway
117+
+ Math.cos(selectedPosition.getRotation().getRadians())
118+
* SmartDashboard.getNumber("getAutoAlignOffsetX", 0)
119+
+ selectedPosition.getTranslation().getY(),
120+
selectedPosition.getRotation());
121+
122+
pathCommand = AutoBuilder.pathfindToPose(targetPose, new PathConstraints(1, 1, 180, 180));
98123
}
99-
targetCommand.schedule();
100-
*/
101124
}
102125

103126
public void execute() {
104-
double velForward = 0;
105-
double velStrafe = 0;
106-
double velGiro = 0;
107-
108-
/**
109-
* If there is a seen target, calculate the PIDs velocities, otherwise, rotate so the robot can
110-
* search the target
111-
*/
112-
if (VisionSubsystem.getTags().length != 0) {
113-
114-
velForward = drivePID.calculate(VisionSubsystem.getTagArea(), driveOffset);
115-
velStrafe = strafePID.calculate(VisionSubsystem.getXDistance(), strafeOffset);
116-
velGiro =
117-
-rotationPID.calculate(
118-
VisionSubsystem.getTagPose2d().getRotation().getDegrees(), rotationOffset);
119-
} else if (VisionSubsystem.getTags().length == 0) {
120-
velForward = 0;
121-
velStrafe = 0;
122-
velGiro = 0.4;
123-
} else {
124-
velForward = 0;
125-
velStrafe = 0;
126-
velGiro = 0;
127+
if (Robot.assistSendableChooser.getSelected().equals("A")) {
128+
double velForward = 0;
129+
double velStrafe = 0;
130+
double velGiro = 0;
131+
132+
/*
133+
* If there is a seen target, calculate the PIDs velocities, otherwise, rotate so the robot can
134+
* search the target
135+
*/
136+
if (VisionSubsystem.getTags().length != 0) {
137+
138+
velForward = drivePID.calculate(VisionSubsystem.getTagArea(), driveOffset);
139+
velStrafe = strafePID.calculate(VisionSubsystem.getXDistance(), strafeOffset);
140+
velGiro =
141+
-rotationPID.calculate(
142+
VisionSubsystem.getTagPose2d().getRotation().getDegrees(), rotationOffset);
143+
} else if (VisionSubsystem.getTags().length == 0) {
144+
velForward = 0;
145+
velStrafe = 0;
146+
velGiro = 0.4;
147+
} else {
148+
velForward = 0;
149+
velStrafe = 0;
150+
velGiro = 0;
151+
}
152+
153+
// 3. Make the driving smoother
154+
velForward = xLimiter.calculate(velForward) * 3;
155+
velStrafe = yLimiter.calculate(velStrafe) * 3;
156+
velGiro = giroLimiter.calculate(velGiro) * 5;
157+
158+
// 4. Construct desired chassis speeds
159+
ChassisSpeeds chassisSpeeds;
160+
161+
// Relative to robot
162+
chassisSpeeds = new ChassisSpeeds(velForward, velStrafe, velGiro);
163+
164+
RobotContainer.drivebase.drive(chassisSpeeds);
165+
} else if (Robot.assistSendableChooser.getSelected().equals("B")) {
166+
robotPose = VisionSubsystem.getRobotPoseInFieldSpace().toPose2d();
167+
168+
if (!robotPose.equals(new Pose2d())) RobotContainer.drivebase.driveToPose(robotPose);
169+
170+
pathCommand.schedule();
127171
}
128-
129-
// 3. Make the driving smoother
130-
velForward = xLimiter.calculate(velForward) * 3;
131-
velStrafe = yLimiter.calculate(velStrafe) * 3;
132-
velGiro = giroLimiter.calculate(velGiro) * 5;
133-
134-
// 4. Construct desired chassis speeds
135-
ChassisSpeeds chassisSpeeds;
136-
137-
// Relative to robot
138-
chassisSpeeds = new ChassisSpeeds(velForward, velStrafe, velGiro);
139172
}
140173

141174
@Override
142175
public void end(boolean inter) {
143-
targetCommand.cancel();
144-
RobotContainer.drivebase.drive(new ChassisSpeeds(0, 0, 0));
176+
pathCommand.end(inter);
145177
}
146178

147179
@Override
148180
public boolean isFinished() {
149-
return RobotContainer.driverXbox.getLeftX() >= 0.3
150-
|| RobotContainer.driverXbox.getLeftY() >= 0.3
151-
|| RobotContainer.driverXbox.getLeftX() <= -0.3
152-
|| RobotContainer.driverXbox.getLeftY() <= -0.3;
181+
return pathCommand.isFinished();
153182
}
154183
}

0 commit comments

Comments
 (0)