Skip to content

Commit 9edc12c

Browse files
jhhbrown1jhhbrown1
committed
absolute orientation
1 parent 973683a commit 9edc12c

4 files changed

Lines changed: 92 additions & 30 deletions

File tree

‎simgui-ds.json‎

Lines changed: 11 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -16,27 +16,23 @@
1616
"incKey": 83
1717
},
1818
{
19-
"decayRate": 0.0,
20-
"keyRate": 0.009999999776482582
19+
"decKey": 70,
20+
"incKey": 72
2121
},
22+
{},
23+
{},
2224
{
23-
"incKey": 69
24-
},
25-
{
26-
"incKey": 82
25+
"decKey": 84,
26+
"incKey": 71
2727
}
2828
],
29-
"axisCount": 5,
30-
"buttonCount": 8,
29+
"axisCount": 6,
30+
"buttonCount": 4,
3131
"buttonKeys": [
3232
90,
3333
88,
3434
67,
35-
86,
36-
66,
37-
78,
38-
77,
39-
44
35+
86
4036
],
4137
"povConfig": [
4238
{
@@ -66,8 +62,8 @@
6662
"axisCount": 2,
6763
"buttonCount": 4,
6864
"buttonKeys": [
69-
-1,
70-
-1,
65+
77,
66+
44,
7167
46,
7268
47
7369
],

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,8 @@ public static final class DriveConstants {
5050

5151
public static final double kDriveGearRatio = 6.12;
5252
public static final double kSteerGearRatio = 150.0 / 7;
53-
public static final double kWheelDiameter = Units.inchesToMeters(3.67);
53+
// public static final double kWheelDiameter = Units.inchesToMeters(3.67);
54+
public static final double kWheelDiameter = Units.inchesToMeters(3.74);
5455
public static final double kWheelCircumference = Math.PI * kWheelDiameter;
5556

5657
public static final double kMetersPerMotorRotation = kWheelCircumference / kDriveGearRatio;
@@ -76,9 +77,9 @@ public static final class DriveConstants {
7677
public static final double kDriveD = 0;
7778
public static final double kDriveMaxAcceleration = 0.75 * kDriveMaxSpeed; // kDriveMaxSpeed in 1.5 sec
7879

79-
public static final double kTurnP = 5;
80+
public static final double kTurnP = 10;
8081
public static final double kTurnI = 0;
81-
public static final double kTurnD = 0;
82+
public static final double kTurnD = 0.1;
8283
public static final double kTurnMaxAcceleration = 0.75 * kTurnMaxAngularSpeed; // kTurnMaxAngularSpeed in 1.5
8384
// sec
8485

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

Lines changed: 26 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ public class Robot extends TimedRobot {
5353
ControllerConstants.kDriverControllerPort);
5454
private final PowerDistribution m_pdh = new PowerDistribution();
5555
private final VisionSimulator m_visionSimulator = new VisionSimulator(m_driveSubsystem,
56-
pose(kFieldLayout.getFieldLength() / 2 + 2.5, 1.91 + .3, 180), 0.01);
56+
pose(kFieldLayout.getFieldLength() / 2 - 4.5, 1.91 + .3, 0), 0.01);
5757
SimCameraProperties cameraProp = new SimCameraProperties() {
5858
{
5959
setCalibration(640, 480, Rotation2d.fromDegrees(100));
@@ -87,11 +87,30 @@ public Robot() {
8787

8888
double distanceTolerance = 0.01;
8989
double angleToleranceInDegrees = 1.0;
90-
double intermediateDistanceTolerance = 0.16;
91-
double intermediateAngleToleranceInDegrees = 16.0;
90+
double intermediateDistanceTolerance = 0.08;
91+
double intermediateAngleToleranceInDegrees = 8.0;
9292

9393
m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
9494
m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
95+
m_testSelector
96+
.addOption(
97+
"Quickly Align to AprilTags 12, 13, 17, 18, and 19",
98+
CommandComposer.alignToTags(
99+
distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
100+
intermediateAngleToleranceInDegrees,
101+
List.of(transform(1.5, 0, 180), transform(1.0, 0, 180), transform(.8, 0, 180)),
102+
transform(1.5, 0, 180), 18, 17, 12, 17, 18, 19, 13, 19, 18));
103+
m_testSelector
104+
.addOption(
105+
"Quickly Align AprilTags 17, 18, 19, 20, 21, and 22",
106+
CommandComposer.alignToTags(
107+
distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
108+
intermediateAngleToleranceInDegrees,
109+
List.of(
110+
transform(1.5, 0.33 / 2, 180), transform(1.0, 0.33 / 2, 180),
111+
transform(.5, 0.33 / 2, 180)),
112+
transform(1.5, 0.33 / 2, 180),
113+
17, 18, 19, 20, 21, 22, 17));
95114
m_testSelector
96115
.addOption(
97116
"Quickly Align to AprilTags 1, 2, 6, 7, and 8",
@@ -153,8 +172,10 @@ public void bindDriveControls() {
153172
m_driveSubsystem.driveCommand(
154173
() -> -m_driverController.getLeftY(),
155174
() -> -m_driverController.getLeftX(),
156-
() -> m_driverController.getL2Axis() - m_driverController.getR2Axis(),
157-
() -> !m_driverController.getHID().getSquareButton()));
175+
() -> -m_driverController.getRightY(),
176+
() -> -m_driverController.getRightX(),
177+
m_driverController.getHID()::getSquareButton)); // makes the robot robot-oriented
178+
m_driverController.options().onTrue(m_driveSubsystem.resetHeading());
158179
m_driverController.square().whileTrue(
159180
driveWithAlignmentCommand(
160181
() -> -m_driverController.getLeftY(),

‎src/main/java/frc/robot/subsystems/DriveSubsystem.java‎

Lines changed: 51 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,10 @@
1616

1717
import edu.wpi.first.hal.SimDouble;
1818
import edu.wpi.first.math.MathUtil;
19+
import edu.wpi.first.math.controller.PIDController;
1920
import edu.wpi.first.math.geometry.Pose2d;
2021
import edu.wpi.first.math.geometry.Rotation2d;
22+
import edu.wpi.first.math.geometry.Translation2d;
2123
import edu.wpi.first.math.kinematics.ChassisSpeeds;
2224
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
2325
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -55,9 +57,12 @@ public class DriveSubsystem extends SubsystemBase {
5557
private final StructPublisher<ChassisSpeeds> m_currentChassisSpeedsPublisher;
5658
private final StructArrayPublisher<SwerveModuleState> m_targetModuleStatePublisher;
5759
private final StructArrayPublisher<SwerveModuleState> m_currentModuleStatePublisher;
60+
private final PIDController m_orientationController = new PIDController(kTurnP, kTurnI,
61+
kTurnD);
5862

5963
/** Creates a new DriveSubsystem. */
6064
public DriveSubsystem() {
65+
m_orientationController.enableContinuousInput(0, 2 * Math.PI);
6166
m_posePublisher = NetworkTableInstance.getDefault()
6267
.getStructTopic("/SmartDashboard/Pose@DriveSubsystem", Pose2d.struct)
6368
.publish();
@@ -272,23 +277,62 @@ public void periodic() {
272277
m_posePublisher.set(m_odometry.update(getHeading(), getModulePositions()));
273278
}
274279

280+
/**
281+
* Creates a {@code ChassisSpeeds} instance to drive the robot with joystick
282+
* input.
283+
*
284+
* @param forwardSpeed Forward speed supplier. Positive values make the robot
285+
* go forward (+X direction).
286+
* @param strafeSpeed Strafe speed supplier. Positive values make the robot
287+
* go to the left (+Y direction).
288+
* @param forwardOrientation Forward orientation supplier. Positive values make
289+
* the robot face forward (+X direction).
290+
* @param strafeOrientation Strafe orientation supplier. Positive values make
291+
* the robot face left (+Y direction).
292+
* @param isRobotRelative Supplier for determining if driving should be robot
293+
* relative.
294+
* @return a {@code ChassisSpeeds} instance to drive the robot with joystick
295+
* input
296+
*/
297+
public ChassisSpeeds chassisSpeeds(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
298+
DoubleSupplier forwardOrientation, DoubleSupplier strafeOrientation) {
299+
var orientation = new Translation2d(forwardOrientation.getAsDouble(), strafeOrientation.getAsDouble());
300+
double rotSpeed = 0;
301+
if (orientation.getNorm() > 0.05)
302+
rotSpeed = m_orientationController
303+
.calculate(getHeading().getRadians(), orientation.getAngle().getRadians());
304+
305+
double fwdSpeed = MathUtil.applyDeadband(forwardSpeed.getAsDouble(), ControllerConstants.kDeadzone);
306+
fwdSpeed = Math.signum(fwdSpeed) * Math.pow(fwdSpeed, 2) * kTeleopDriveMaxSpeed;
307+
308+
double strSpeed = MathUtil.applyDeadband(strafeSpeed.getAsDouble(), ControllerConstants.kDeadzone);
309+
strSpeed = Math.signum(strSpeed) * Math.pow(strSpeed, 2) * kTeleopDriveMaxSpeed;
310+
311+
return chassisSpeeds(fwdSpeed, strSpeed, rotSpeed);
312+
}
313+
275314
/**
276315
* Creates a command to drive the robot with joystick input.
277316
*
278317
* @param forwardSpeed Forward speed supplier. Positive values make the robot
279318
* go forward (+X direction).
280319
* @param strafeSpeed Strafe speed supplier. Positive values make the robot
281320
* go to the left (+Y direction).
282-
* @param rotation Rotation speed supplier. Positive values make the
283-
* robot rotate CCW.
284-
* @param isFieldRelative Supplier for determining if driving should be field
321+
* @param forwardOrientation Forward orientation supplier. Positive values make
322+
* the robot face forward (+X direction).
323+
* @param strafeOrientation Strafe orientation supplier. Positive values make
324+
* the
325+
* robot face left (+Y direction).
326+
* @param isRobotRelative Supplier for determining if driving should be robot
285327
* relative.
286328
* @return A command to drive the robot.
287329
*/
288330
public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed,
289-
DoubleSupplier rotation, BooleanSupplier isFieldRelative) {
331+
DoubleSupplier forwardOrientation, DoubleSupplier strafeOrientation, BooleanSupplier isRobotRelative) {
290332
return run(() -> {
291-
drive(chassisSpeeds(forwardSpeed, strafeSpeed, rotation), isFieldRelative.getAsBoolean());
333+
drive(
334+
chassisSpeeds(forwardSpeed, strafeSpeed, forwardOrientation, strafeOrientation),
335+
!isRobotRelative.getAsBoolean());
292336
}).withName("DefaultDriveCommand");
293337
}
294338

@@ -297,11 +341,11 @@ public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSp
297341
*
298342
* @return A command to reset the gyro heading.
299343
*/
300-
public Command resetHeadingCommand() {
344+
public Command resetHeading() {
301345
return runOnce(m_gyro::zeroYaw).withName("ResetHeadingCommand");
302346
}
303347

304-
public Command resetOdometryCommand(Pose2d pose) {
348+
public Command resetOdometry(Pose2d pose) {
305349
return runOnce(() -> m_odometry.resetPosition(getHeading(), getModulePositions(), pose))
306350
.withName("ResetOdometryCommand");
307351
}

0 commit comments

Comments
 (0)