Skip to content

Commit 6bbe677

Browse files
lessfoodlessfood
committed
Finalized orbit drive and added robot-relative drive
Y is bound to orbiting april tag 25, and POV down is bound to swapping robot relative.
1 parent da25dd0 commit 6bbe677

File tree

3 files changed

+105
-70
lines changed

3 files changed

+105
-70
lines changed

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

Lines changed: 25 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import edu.wpi.first.math.geometry.Rotation2d;
1515
import edu.wpi.first.wpilibj.GenericHID;
1616
import edu.wpi.first.wpilibj.XboxController;
17+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1718
import edu.wpi.first.wpilibj2.command.Command;
1819
import edu.wpi.first.wpilibj2.command.Commands;
1920
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -49,6 +50,8 @@ public class RobotContainer {
4950
// Dashboard inputs
5051
private final LoggedDashboardChooser<Command> autoChooser;
5152

53+
private boolean robotRelative;
54+
5255
/** The container for the robot. Contains subsystems, OI devices, and commands. */
5356
public RobotContainer() {
5457
switch (Constants.currentMode) {
@@ -147,13 +150,15 @@ public RobotContainer() {
147150
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
148151
*/
149152
private void configureButtonBindings() {
153+
robotRelative = false;
150154
// Default command, normal field-relative drive
151155
drive.setDefaultCommand(
152156
DriveCommands.joystickDrive(
153157
drive,
154158
() -> -controller.getLeftY(),
155159
() -> -controller.getLeftX(),
156-
() -> -controller.getRightX()));
160+
() -> -controller.getRightX(),
161+
() -> robotRelative));
157162

158163
// Lock to 0° when A button is held
159164
controller
@@ -163,7 +168,8 @@ private void configureButtonBindings() {
163168
drive,
164169
() -> -controller.getLeftY(),
165170
() -> -controller.getLeftX(),
166-
() -> Rotation2d.kZero));
171+
() -> Rotation2d.kZero,
172+
() -> robotRelative));
167173

168174
// Switch to X pattern when X button is pressed
169175
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
@@ -185,7 +191,23 @@ private void configureButtonBindings() {
185191
drive,
186192
() -> -controller.getLeftY(),
187193
() -> -controller.getLeftX(),
188-
new Pose2d(5, 5, Rotation2d.kZero)));
194+
aprilTagLayout.getTagPose(25).get().toPose2d(),
195+
() -> robotRelative) // / get position of april tag on blue basket
196+
);
197+
198+
controller
199+
.povDown()
200+
.onTrue(
201+
Commands.runOnce(
202+
() -> {
203+
robotRelative = !robotRelative;
204+
SmartDashboard.putBoolean("Robot Relative Drive", robotRelative);
205+
}));
206+
/**
207+
* DriveCommands.joystickOrbitDrive( drive, () -> -controller.getLeftY(), () ->
208+
* -controller.getLeftX(), aprilTagLayout.getTagPose(28).get().toPose2d()));// Pose2d(5, 5,
209+
* Rotation2d.kZero)));
210+
*/
189211
}
190212

191213
/**

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

Lines changed: 65 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -27,11 +27,10 @@
2727
import java.text.NumberFormat;
2828
import java.util.LinkedList;
2929
import java.util.List;
30+
import java.util.function.BooleanSupplier;
3031
import java.util.function.DoubleSupplier;
3132
import java.util.function.Supplier;
3233

33-
import org.photonvision.PhotonUtils;
34-
3534
public class DriveCommands {
3635
private static final double DEADBAND = 0.1;
3736
private static final double ANGLE_KP = 5.0;
@@ -43,7 +42,6 @@ public class DriveCommands {
4342
private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
4443
private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
4544

46-
4745
private DriveCommands() {}
4846

4947
private static Translation2d getLinearVelocityFromJoysticks(double x, double y) {
@@ -67,7 +65,8 @@ public static Command joystickDrive(
6765
Drive drive,
6866
DoubleSupplier xSupplier,
6967
DoubleSupplier ySupplier,
70-
DoubleSupplier omegaSupplier) {
68+
DoubleSupplier omegaSupplier,
69+
BooleanSupplier robotRelative) {
7170
return Commands.run(
7271
() -> {
7372
// Get linear velocity
@@ -86,15 +85,19 @@ public static Command joystickDrive(
8685
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
8786
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
8887
omega * drive.getMaxAngularSpeedRadPerSec());
88+
8989
boolean isFlipped =
9090
DriverStation.getAlliance().isPresent()
9191
&& DriverStation.getAlliance().get() == Alliance.Red;
92+
9293
drive.runVelocity(
93-
ChassisSpeeds.fromFieldRelativeSpeeds(
94-
speeds,
95-
isFlipped
96-
? drive.getRotation().plus(new Rotation2d(Math.PI))
97-
: drive.getRotation()));
94+
robotRelative.getAsBoolean()
95+
? ChassisSpeeds.fromRobotRelativeSpeeds(speeds, Rotation2d.fromDegrees(0))
96+
: ChassisSpeeds.fromFieldRelativeSpeeds(
97+
speeds,
98+
isFlipped
99+
? drive.getRotation().plus(new Rotation2d(Math.PI))
100+
: drive.getRotation()));
98101
},
99102
drive);
100103
}
@@ -108,7 +111,8 @@ public static Command joystickDriveAtAngle(
108111
Drive drive,
109112
DoubleSupplier xSupplier,
110113
DoubleSupplier ySupplier,
111-
Supplier<Rotation2d> rotationSupplier) {
114+
Supplier<Rotation2d> rotationSupplier,
115+
BooleanSupplier robotRelative) {
112116

113117
// Create PID controller
114118
ProfiledPIDController angleController =
@@ -137,15 +141,19 @@ public static Command joystickDriveAtAngle(
137141
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
138142
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
139143
omega);
144+
140145
boolean isFlipped =
141146
DriverStation.getAlliance().isPresent()
142147
&& DriverStation.getAlliance().get() == Alliance.Red;
148+
143149
drive.runVelocity(
144-
ChassisSpeeds.fromFieldRelativeSpeeds(
145-
speeds,
146-
isFlipped
147-
? drive.getRotation().plus(new Rotation2d(Math.PI))
148-
: drive.getRotation()));
150+
robotRelative.getAsBoolean()
151+
? ChassisSpeeds.fromRobotRelativeSpeeds(speeds, Rotation2d.fromDegrees(0))
152+
: ChassisSpeeds.fromFieldRelativeSpeeds(
153+
speeds,
154+
isFlipped
155+
? drive.getRotation().plus(new Rotation2d(Math.PI))
156+
: drive.getRotation()));
149157
},
150158
drive)
151159

@@ -154,48 +162,54 @@ public static Command joystickDriveAtAngle(
154162
}
155163

156164
/**
157-
* Field relative drive command where joystick controlls linear velocity and robot is continuously towards a point on the field.
158-
* Useful for orbitting shooting targets
159-
* @param orbitPose field-relative point to orbit
165+
* Field relative drive command where joystick controlls linear velocity and robot is continuously
166+
* towards a point on the field. Useful for orbitting shooting targets
167+
*
168+
* @param target Pose2d to look at
160169
*/
161170
public static Command joystickOrbitDrive(
162171
Drive drive,
163172
DoubleSupplier xSupplier,
164173
DoubleSupplier ySupplier,
165-
Pose2d orbitPose) {
166-
167-
//create angle PID controller
168-
ProfiledPIDController angleController =
169-
new ProfiledPIDController(
170-
1,
171-
0,
172-
0.5,
173-
new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
174-
175-
angleController.enableContinuousInput(-Math.PI, Math.PI);
176-
angleController.setTolerance(Units.degreesToRadians(5));
177-
178-
179-
return Commands.run(
180-
() -> {
181-
Rotation2d rotationTarget = PhotonUtils.getYawToPose(drive.getPose(), orbitPose);//gets angle from robot position to tag position
182-
183-
double radiansOff = drive.getPose().getRotation().getRadians() - rotationTarget.getRadians();
184-
185-
double orbitControllerOutput = angleController.calculate(radiansOff, 0);
186-
double orbitPIDXOut = xSupplier.getAsDouble();
187-
double orbitPIDYOut = ySupplier.getAsDouble();
188-
189-
drive.runVelocity(
190-
ChassisSpeeds.fromFieldRelativeSpeeds(orbitPIDXOut, orbitPIDYOut, orbitControllerOutput, drive.getPose().getRotation())
191-
);
192-
193-
194-
}, drive)
174+
Pose2d target,
175+
BooleanSupplier robotRelative) {
176+
177+
return joystickDriveAtAngle(
178+
drive,
179+
xSupplier,
180+
ySupplier,
181+
() ->
182+
Rotation2d.fromRadians(
183+
Math.atan2(
184+
target.getY() - drive.getPose().getY(),
185+
target.getX() - drive.getPose().getX())),
186+
robotRelative);
187+
}
195188

196-
// Reset PID controller when command starts
197-
.beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
198-
}
189+
// create angle PID controller
190+
/**
191+
* ProfiledPIDController angleController = new ProfiledPIDController( 1, 0, 0.5, new
192+
* TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
193+
*
194+
* <p>angleController.enableContinuousInput(-Math.PI, Math.PI);
195+
* angleController.setTolerance(Units.degreesToRadians(5));
196+
*
197+
* <p>return Commands.run( () -> { Rotation2d rotationTarget = Rotation2d.fromRadians( Math.atan2(
198+
* // find relative angle of robot to orbit pose target.getY() - drive.getPose().getY(),
199+
* target.getX() - drive.getPose().getX()));
200+
*
201+
* <p>double radiansOff = drive.getPose().getRotation().getRadians() -
202+
* rotationTarget.getRadians();
203+
*
204+
* <p>double orbitControllerOutput = angleController.calculate(radiansOff, 0); double orbitPIDXOut
205+
* = xSupplier.getAsDouble(); double orbitPIDYOut = ySupplier.getAsDouble();
206+
*
207+
* <p>drive.runVelocity( ChassisSpeeds.fromRobotRelativeSpeeds( orbitPIDXOut, orbitPIDYOut,
208+
* orbitControllerOutput, drive.getPose().getRotation())); }, drive)
209+
*
210+
* <p>// Reset PID controller when command starts .beforeStarting(() ->
211+
* angleController.reset(drive.getRotation().getRadians())); }
212+
*/
199213
/** pid look at thingy */
200214
public static Command lookAt(
201215
Drive drive,

‎src/main/java/frc/robot/subsystems/vision/Vision.java‎

Lines changed: 15 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@
1818
import edu.wpi.first.math.numbers.N3;
1919
import edu.wpi.first.wpilibj.Alert;
2020
import edu.wpi.first.wpilibj.Alert.AlertType;
21-
import edu.wpi.first.wpilibj.DriverStation;
2221
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2322
import frc.robot.subsystems.vision.VisionIO.PoseObservationType;
2423
import java.util.LinkedList;
@@ -56,22 +55,22 @@ public Vision(VisionConsumer consumer, VisionIO... io) {
5655
* @param cameraIndex The index of the camera to use.
5756
*/
5857
public Rotation2d getTargetX(int cameraIndex) {
59-
DriverStation.reportWarning(Double.toString(inputs[cameraIndex].latestTargetObservation.tx().getDegrees()), false);
58+
59+
// DriverStation.reportWarning(
60+
// Double.toString(inputs[cameraIndex].latestTargetObservation.tx().getDegrees()), false);
6061
return inputs[cameraIndex].latestTargetObservation.tx();
61-
62-
/**for (int i : inputs[0].tagIds) {
63-
DriverStation.reportWarning(Integer.toString(i), false);
64-
var tagPose = aprilTagLayout.getTagPose(i);
65-
DriverStation.reportWarning(
66-
Double.toString(
67-
tagPose.get().getRotation().toRotation2d().plus(Rotation2d.kPi).getDegrees()),
68-
false);
69-
if (i == cameraIndex) {
70-
return tagPose.get().getRotation().toRotation2d().plus(Rotation2d.kPi);
71-
}
72-
}
73-
return Rotation2d.kZero;
74-
*/
62+
/**
63+
* var prevPose = Pose2d.kZero;
64+
*
65+
* <p>for (int i : inputs[0].tagIds) { DriverStation.reportWarning(Integer.toString(i), false);
66+
* var tagPose = aprilTagLayout.getTagPose(i);
67+
*
68+
* <p>if (i == cameraIndex) { return tagPose.get().toPose2d(); }
69+
*
70+
* <p>prevPose = tagPose.get().toPose2d(); }
71+
*
72+
* <p>return prevPose;
73+
*/
7574
}
7675

7776
@Override

0 commit comments

Comments
 (0)