Skip to content

Commit 97e8381

Browse files
karmikhaelkarmikhael
authored
turret: add pose-supplier, targeting helpers, and default command (#14)
* turret: add pose-supplier, targeting helpers, and default command Add Turret(Supplier<Pose2d>) constructor and default ctor; store pose supplier for odometry-based targeting. Implement getRobotRelativeAngle() which uses TurretHelpers.getTarget/getFieldRelativeAngle/getRobotRelative to compute the desired robot-relative turret angle. Record the last chosen Translation2d target and expose getLastTarget() for dashboarding. Add setTurretSetpoint() stub and getLastSetpoint() for future motor control. Add TurretDefaultCommand which calls turret.getRobotRelativeAngle(), publishes the target and setpoint, and set it as the turret default command in RobotContainer. Wire Turret instantiation in RobotContainer (pass drivebase::getPose).
1 parent c023cb0 commit 97e8381

File tree

5 files changed

+330
-1
lines changed

5 files changed

+330
-1
lines changed

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

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@
2222
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
2323
import edu.wpi.first.wpilibj2.command.button.Trigger;
2424
import frc.robot.Constants.OperatorConstants;
25+
import frc.robot.commands.turret.DefaultCommand;
26+
import frc.robot.subsystems.Turret;
2527
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
2628
import java.io.File;
2729
import swervelib.SwerveInputStream;
@@ -40,6 +42,9 @@ public class RobotContainer {
4042
private final SwerveSubsystem drivebase =
4143
new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/7660-chassis0"));
4244

45+
// Turret subsystem, constructed with a supplier that returns the current odometry pose
46+
private final Turret turret = new Turret(drivebase::getPose);
47+
4348
// Establish a Sendable Chooser that will be able to be sent to the SmartDashboard, allowing
4449
// selection of desired auto
4550
private final SendableChooser<Command> autoChooser;
@@ -125,6 +130,9 @@ public RobotContainer() {
125130

126131
// Put the autoChooser on the SmartDashboard
127132
SmartDashboard.putData("Auto Chooser", autoChooser);
133+
134+
// Set the turret default command to compute targets from odometry
135+
turret.setDefaultCommand(new DefaultCommand(turret));
128136
}
129137

130138
/**

‎src/main/java/frc/robot/commands/turret/DefaultCommand.java‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
package frc.robot.commands.turret;
2+
3+
import edu.wpi.first.math.geometry.Rotation2d;
4+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
5+
import edu.wpi.first.wpilibj2.command.Command;
6+
import frc.robot.subsystems.Turret;
7+
8+
/** Default command for the turret: periodically compute a target and set a turret setpoint. */
9+
public class DefaultCommand extends Command {
10+
private final Turret turret;
11+
12+
public DefaultCommand(Turret turret) {
13+
this.turret = turret;
14+
addRequirements(turret);
15+
}
16+
17+
@Override
18+
public void execute() {
19+
Rotation2d robotRelative = turret.getRobotRelativeAngle();
20+
21+
// Send the setpoint to the turret subsystem (stubbed implementation).
22+
turret.setTurretSetpoint(robotRelative);
23+
24+
// Publish the setpoint for debugging.
25+
SmartDashboard.putNumber("Turret/SetpointRad", robotRelative.getRadians());
26+
// Also publish the chosen target from the turret for visibility
27+
var target = turret.getLastTarget();
28+
SmartDashboard.putNumber("Turret/TargetX", target.getX());
29+
SmartDashboard.putNumber("Turret/TargetY", target.getY());
30+
}
31+
32+
@Override
33+
public boolean isFinished() {
34+
return false; // Default command never finishes
35+
}
36+
}

‎src/main/java/frc/robot/lib/TurretHelpers.java‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
package frc.robot.lib;
2+
3+
import edu.wpi.first.math.geometry.Rotation2d;
4+
import edu.wpi.first.math.geometry.Translation2d;
5+
import edu.wpi.first.math.util.Units;
6+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
7+
8+
public final class TurretHelpers {
9+
private TurretHelpers() {}
10+
11+
// Field dimensions / thresholds (converted from inches to meters)
12+
public static final double MAX_X = Units.inchesToMeters(651.22);
13+
public static final double BLUE_THRESHOLD_X = Units.inchesToMeters(182.11);
14+
public static final double RED_THRESHOLD_X = Units.inchesToMeters(469.11);
15+
public static final double THRESHOLD_Y = Units.inchesToMeters(158.84);
16+
// Frequently used goal Y positions (in inches) and their meter conversions
17+
public static final double GOAL_Y_IN_LOW = 100.0;
18+
public static final double GOAL_Y_IN_HIGH = 220.0;
19+
public static final double GOAL_Y_LOW = Units.inchesToMeters(GOAL_Y_IN_LOW);
20+
public static final double GOAL_Y_HIGH = Units.inchesToMeters(GOAL_Y_IN_HIGH);
21+
22+
// Representative centers for each goal area (meters). These are placeholders
23+
// suitable for computing angles; replace with precise coordinates if available.
24+
// Provide distinct hub centers for Red and Blue teams so callers can differentiate.
25+
public static final Translation2d HUB_CENTER_RED =
26+
new Translation2d(RED_THRESHOLD_X + (MAX_X - RED_THRESHOLD_X) / 2.0, THRESHOLD_Y);
27+
public static final Translation2d HUB_CENTER_BLUE =
28+
new Translation2d(BLUE_THRESHOLD_X / 2.0, THRESHOLD_Y);
29+
public static final Translation2d GOAL_R1_CENTER = new Translation2d(MAX_X, GOAL_Y_LOW);
30+
public static final Translation2d GOAL_R2_CENTER = new Translation2d(MAX_X, GOAL_Y_HIGH);
31+
public static final Translation2d GOAL_B1_CENTER = new Translation2d(0.0, GOAL_Y_LOW);
32+
public static final Translation2d GOAL_B2_CENTER = new Translation2d(0.0, GOAL_Y_HIGH);
33+
34+
/**
35+
* Decide which goal area to aim at based purely on the robot position and alliance. Mirrors the
36+
* decision tree provided by the user.
37+
*
38+
* @param x robot X position (meters)
39+
* @param y robot Y position (meters)
40+
* @param alliance robot alliance (Red or Blue)
41+
* @return the chosen goal area as a Translation2d (representative center)
42+
*/
43+
public static Translation2d getTarget(double x, double y, Alliance alliance) {
44+
if (alliance == Alliance.Red) {
45+
if (x > RED_THRESHOLD_X) {
46+
return HUB_CENTER_RED;
47+
} else {
48+
if (y < THRESHOLD_Y) {
49+
return GOAL_R1_CENTER;
50+
} else {
51+
return GOAL_R2_CENTER;
52+
}
53+
}
54+
} else { // Blue
55+
if (x < BLUE_THRESHOLD_X) {
56+
return HUB_CENTER_BLUE;
57+
} else {
58+
if (y < THRESHOLD_Y) {
59+
return GOAL_B1_CENTER;
60+
} else {
61+
return GOAL_B2_CENTER;
62+
}
63+
}
64+
}
65+
}
66+
67+
/** Backwards-compatible overload that accepts a Translation2d robot position. */
68+
public static Translation2d getTarget(Translation2d turretPosition, Alliance alliance) {
69+
return getTarget(turretPosition.getX(), turretPosition.getY(), alliance);
70+
}
71+
72+
/** Compute the field-relative angle (radians) from turret position to target position. */
73+
public static Rotation2d getFieldRelativeAngle(
74+
Translation2d turretPosition, Translation2d targetPosition) {
75+
double dx = targetPosition.getX() - turretPosition.getX();
76+
double dy = targetPosition.getY() - turretPosition.getY();
77+
return new Rotation2d(Math.atan2(dy, dx));
78+
}
79+
80+
/**
81+
* Convert a field-relative desired turret angle into a robot-relative turret angle. This returns
82+
* the angle the turret must take relative to the robot's forward direction. This is a pure
83+
* computation; it does not command the Neo.
84+
*/
85+
public static Rotation2d getRobotRelative(Rotation2d fieldRelativeAngle, Rotation2d robotAngle) {
86+
double turretRadians = fieldRelativeAngle.getRadians() - robotAngle.getRadians();
87+
// normalize to [-pi, pi]
88+
turretRadians = Math.atan2(Math.sin(turretRadians), Math.cos(turretRadians));
89+
return new Rotation2d(turretRadians);
90+
}
91+
}

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

Lines changed: 70 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,38 @@
66
import com.revrobotics.spark.SparkMax;
77
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
88
import com.revrobotics.spark.config.SparkMaxConfig;
9+
import edu.wpi.first.math.geometry.Pose2d;
10+
import edu.wpi.first.math.geometry.Rotation2d;
11+
import edu.wpi.first.math.geometry.Translation2d;
12+
import edu.wpi.first.wpilibj.DriverStation;
13+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
914
import frc.robot.Constants;
15+
import frc.robot.lib.TurretHelpers;
16+
import java.util.function.Supplier;
1017

11-
public class Turret {
18+
/** Simple turret subsystem that holds a Neo and a supplier for the robot pose. */
19+
public class Turret extends SubsystemBase {
1220
private final SparkMax turretMotor =
1321
new SparkMax(Constants.Turret.MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
22+
private final Supplier<Pose2d> getPose;
1423

24+
// Most recent setpoint (robot-relative radians). Useful for debugging and tests.
25+
private Rotation2d lastSetpoint = new Rotation2d();
26+
// Most recently chosen target (field coordinates, meters)
27+
private Translation2d lastTarget = new Translation2d();
28+
29+
/** Default constructor. Uses a trivial Pose2d supplier (origin) when no supplier is provided. */
1530
public Turret() {
31+
this(() -> new Pose2d());
32+
}
33+
34+
/**
35+
* Construct a Turret with a pose supplier function.
36+
*
37+
* @param getPose a Supplier that returns the current {@link Pose2d} of the robot
38+
*/
39+
public Turret(Supplier<Pose2d> getPose) {
40+
this.getPose = getPose;
1641
SparkMaxConfig turretConfig = configureTurretMotor();
1742
turretMotor.configure(
1843
turretConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -24,4 +49,48 @@ public SparkMaxConfig configureTurretMotor() {
2449
turretConfig.idleMode(IdleMode.kBrake).inverted(false);
2550
return turretConfig;
2651
}
52+
53+
/**
54+
* Set the turret desired angle relative to the robot forward direction. This is a stub that
55+
* records the setpoint; convert to motor commands in a follow-up change.
56+
*
57+
* @param robotRelativeAngle turret angle relative to robot forward
58+
*/
59+
public void setTurretSetpoint(Rotation2d robotRelativeAngle) {
60+
this.lastSetpoint = robotRelativeAngle;
61+
// TODO: convert Rotation2d to motor position/velocity and command the Neo here.
62+
}
63+
64+
/** Get the most recent turret setpoint (robot-relative angle). Primarily useful for testing. */
65+
public Rotation2d getLastSetpoint() {
66+
return lastSetpoint;
67+
}
68+
69+
/**
70+
* Compute the desired turret angle relative to the robot using the stored pose supplier and
71+
* TurretHelpers decision logic.
72+
*
73+
* @return desired turret angle relative to the robot forward direction
74+
*/
75+
public Rotation2d getRobotRelativeAngle() {
76+
Pose2d pose = getPose.get();
77+
Translation2d robotPos = pose.getTranslation();
78+
var alliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
79+
80+
Translation2d target = TurretHelpers.getTarget(robotPos, alliance);
81+
// record the chosen target for external inspection / dashboard
82+
this.lastTarget = target;
83+
Rotation2d fieldAngle = TurretHelpers.getFieldRelativeAngle(robotPos, target);
84+
Rotation2d robotRelative = TurretHelpers.getRobotRelative(fieldAngle, pose.getRotation());
85+
return robotRelative;
86+
}
87+
88+
/**
89+
* Returns the last computed target translation (field coordinates).
90+
*
91+
* @return last chosen {@link Translation2d} target
92+
*/
93+
public Translation2d getLastTarget() {
94+
return lastTarget;
95+
}
2796
}

‎src/test/java/frc/robot/lib/TurretTest.java‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
package frc.robot.lib;
2+
3+
import static org.junit.jupiter.api.Assertions.assertEquals;
4+
import static org.junit.jupiter.api.Assertions.assertNotNull;
5+
6+
import edu.wpi.first.math.geometry.Translation2d;
7+
import edu.wpi.first.math.util.Units;
8+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
9+
import java.util.stream.Stream;
10+
import org.junit.jupiter.params.ParameterizedTest;
11+
import org.junit.jupiter.params.provider.Arguments;
12+
import org.junit.jupiter.params.provider.MethodSource;
13+
14+
class TurretTest {
15+
16+
@ParameterizedTest
17+
@MethodSource("targetProvider")
18+
void getTarget_tableDriven(
19+
double turretX, double turretY, Alliance alliance, double expX, double expY) {
20+
Translation2d turretPos = new Translation2d(turretX, turretY);
21+
Translation2d target = TurretHelpers.getTarget(turretPos, alliance);
22+
assertNotNull(target);
23+
assertEquals(expX, target.getX(), 1e-9);
24+
assertEquals(expY, target.getY(), 1e-9);
25+
}
26+
27+
static Stream<Arguments> targetProvider() {
28+
return Stream.of(
29+
// A: (78.305, 79.423)
30+
// Red -> GOAL_R1
31+
Arguments.of(
32+
Units.inchesToMeters(78.305),
33+
Units.inchesToMeters(79.423),
34+
Alliance.Red,
35+
TurretHelpers.MAX_X,
36+
TurretHelpers.GOAL_Y_LOW),
37+
// A: (78.305, 79.423) Blue -> HUB
38+
Arguments.of(
39+
Units.inchesToMeters(78.305),
40+
Units.inchesToMeters(79.423),
41+
Alliance.Blue,
42+
TurretHelpers.HUB_CENTER_BLUE.getX(),
43+
TurretHelpers.HUB_CENTER_BLUE.getY()),
44+
45+
// C: (78.305, 238.263)
46+
// Red -> GOAL_R2
47+
Arguments.of(
48+
Units.inchesToMeters(78.305),
49+
Units.inchesToMeters(238.263),
50+
Alliance.Red,
51+
TurretHelpers.MAX_X,
52+
TurretHelpers.GOAL_Y_HIGH),
53+
// C: (78.305, 238.263) Blue -> HUB
54+
Arguments.of(
55+
Units.inchesToMeters(78.305),
56+
Units.inchesToMeters(238.263),
57+
Alliance.Blue,
58+
TurretHelpers.HUB_CENTER_BLUE.getX(),
59+
TurretHelpers.HUB_CENTER_BLUE.getY()),
60+
61+
// D: (325.61, 79.423)
62+
// Red -> GOAL_R1
63+
Arguments.of(
64+
Units.inchesToMeters(325.61),
65+
Units.inchesToMeters(79.423),
66+
Alliance.Red,
67+
TurretHelpers.MAX_X,
68+
TurretHelpers.GOAL_Y_LOW),
69+
// D: (325.61, 79.423) Blue -> GOAL_B1
70+
Arguments.of(
71+
Units.inchesToMeters(325.61),
72+
Units.inchesToMeters(79.423),
73+
Alliance.Blue,
74+
0.0,
75+
TurretHelpers.GOAL_Y_LOW),
76+
77+
// E: (325.61, 238.263)
78+
// Red -> GOAL_R2
79+
Arguments.of(
80+
Units.inchesToMeters(325.61),
81+
Units.inchesToMeters(238.263),
82+
Alliance.Red,
83+
TurretHelpers.MAX_X,
84+
TurretHelpers.GOAL_Y_HIGH),
85+
// E: (325.61, 238.263) Blue -> GOAL_B2
86+
Arguments.of(
87+
Units.inchesToMeters(325.61),
88+
Units.inchesToMeters(238.263),
89+
Alliance.Blue,
90+
0.0,
91+
TurretHelpers.GOAL_Y_HIGH),
92+
93+
// F: (560.165, 79.423)
94+
// Red -> HUB
95+
Arguments.of(
96+
Units.inchesToMeters(560.165),
97+
Units.inchesToMeters(79.423),
98+
Alliance.Red,
99+
TurretHelpers.HUB_CENTER_RED.getX(),
100+
TurretHelpers.HUB_CENTER_RED.getY()),
101+
// F: (560.165, 79.423) Blue -> GOAL_B1
102+
Arguments.of(
103+
Units.inchesToMeters(560.165),
104+
Units.inchesToMeters(79.423),
105+
Alliance.Blue,
106+
0.0,
107+
TurretHelpers.GOAL_Y_LOW),
108+
109+
// H: (560.165, 238.263)
110+
// Red -> HUB
111+
Arguments.of(
112+
Units.inchesToMeters(560.165),
113+
Units.inchesToMeters(238.263),
114+
Alliance.Red,
115+
TurretHelpers.HUB_CENTER_RED.getX(),
116+
TurretHelpers.HUB_CENTER_RED.getY()),
117+
// H: (560.165, 238.263) Blue -> GOAL_B2
118+
Arguments.of(
119+
Units.inchesToMeters(560.165),
120+
Units.inchesToMeters(238.263),
121+
Alliance.Blue,
122+
0.0,
123+
TurretHelpers.GOAL_Y_HIGH));
124+
}
125+
}

0 commit comments

Comments
 (0)