Skip to content

Commit ac070af

Browse files
levyishailevyishai
committed
init
1 parent b9e9659 commit ac070af

File tree

20 files changed

+80
-301
lines changed

20 files changed

+80
-301
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ dependencies {
8888
simulationRelease wpi.sim.enableRelease()
8989

9090
implementation 'com.google.code.gson:gson:2.10.1'
91-
implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.13'
91+
implementation 'com.github.Programming-TRIGON:TRIGONLib:v2024.2.13'
9292

9393
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
9494
annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import com.pathplanner.lib.pathfinding.Pathfinding;
99
import edu.wpi.first.wpilibj2.command.Command;
1010
import edu.wpi.first.wpilibj2.command.CommandScheduler;
11+
import frc.trigon.robot.constants.CameraConstants;
1112
import frc.trigon.robot.constants.RobotConstants;
1213
import org.littletonrobotics.junction.LogFileUtil;
1314
import org.littletonrobotics.junction.LoggedRobot;
@@ -34,6 +35,7 @@ public void robotInit() {
3435

3536
@Override
3637
public void robotPeriodic() {
38+
CameraConstants.REAR_TAG_CAMERA.update();
3739
commandScheduler.run();
3840
}
3941

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

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,6 @@
1414
import frc.trigon.robot.commands.factories.AutonomousCommands;
1515
import frc.trigon.robot.commands.factories.GeneralCommands;
1616
import frc.trigon.robot.commands.factories.ShootingCommands;
17-
import frc.trigon.robot.constants.CameraConstants;
1817
import frc.trigon.robot.constants.OperatorConstants;
1918
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
2019
import frc.trigon.robot.subsystems.MotorSubsystem;
@@ -31,10 +30,7 @@
3130
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
3231

3332
public class RobotContainer {
34-
public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator(
35-
CameraConstants.FRONT_TAG_CAMERA,
36-
CameraConstants.REAR_TAG_CAMERA
37-
);
33+
public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator();
3834
public static final Swerve SWERVE = new Swerve();
3935
public static final Intake INTAKE = new Intake();
4036
public static final Climber CLIMBER = new Climber();
@@ -72,7 +68,6 @@ private void bindDefaultCommands() {
7268

7369
private void bindControllerCommands() {
7470
OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
75-
OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND);
7671
OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
7772
OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand());
7873
OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
@@ -97,7 +92,7 @@ private void bindControllerCommands() {
9792
OperatorConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND);
9893
OperatorConstants.OVERRIDE_IS_CLIMBING_TRIGGER.onTrue(CommandConstants.OVERRIDE_IS_CLIMBING_COMMAND);
9994

100-
OperatorConstants.SPEAKER_SHOT_TRIGGER.whileTrue(CommandConstants.SHOOT_AT_SPEAKER_COMMAND);
95+
OperatorConstants.SPEAKER_SHOT_TRIGGER.whileTrue(ShootingCommands.getShootAtTagCommand());
10196
OperatorConstants.CLOSE_SPEAKER_SHOT_TRIGGER.whileTrue(ShootingCommands.getCloseSpeakerShotCommand());
10297
OperatorConstants.WARM_SPEAKER_SHOT_TRIGGER.whileTrue(ShootingCommands.getWarmSpeakerShotCommand());
10398
OperatorConstants.DELIVERY_TRIGGER.whileTrue(CommandConstants.DELIVERY_COMMAND);

‎src/main/java/frc/trigon/robot/commands/CommandConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,6 @@ public class CommandConstants {
4646
() -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
4747
),
4848
RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())),
49-
SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true),
5049
SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
5150
() -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
5251
() -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
@@ -135,7 +134,8 @@ public static double calculateRotationStickAxisValue(double axisValue) {
135134
* @return the power to apply to the robot
136135
*/
137136
public static double calculateShiftModeValue(double minimumPower) {
138-
final double squaredShiftModeValue = DRIVER_CONTROLLER.getRightTriggerAxis() * DRIVER_CONTROLLER.getRightTriggerAxis();
137+
// final double squaredShiftModeValue = DRIVER_CONTROLLER.getRightTriggerAxis() * DRIVER_CONTROLLER.getRightTriggerAxis();
138+
final double squaredShiftModeValue = 1;
139139
final double minimumShiftValueCoefficient = 1 - (1 / minimumPower);
140140

141141
return 1 - squaredShiftModeValue * minimumShiftValueCoefficient;

‎src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,7 @@ private static Command getPrepareForAmpCommand() {
6464
}
6565

6666
private static Command getFeedToAmpCommand() {
67-
return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && RobotContainer.AMP_ALIGNER.atTargetState());
67+
return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch()/* && RobotContainer.AMP_ALIGNER.atTargetState()*/);
6868
}
6969

7070
private static Command getPathfindToAmpCommand() {

‎src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java‎

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,25 @@
1515
import frc.trigon.robot.subsystems.shooter.ShooterCommands;
1616
import frc.trigon.robot.subsystems.shooter.ShooterConstants;
1717
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
18+
import org.trigon.utilities.mirrorable.MirrorableRotation2d;
1819

1920
public class ShootingCommands {
2021
private static final ShootingCalculations SHOOTING_CALCULATIONS = ShootingCalculations.getInstance();
2122

23+
public static Command getShootAtTagCommand() {
24+
return new ParallelCommandGroup(
25+
PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand(),
26+
ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(),
27+
SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
28+
() -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
29+
() -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
30+
() -> new MirrorableRotation2d(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle().get().plus(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()), false)
31+
),
32+
new RunCommand(SHOOTING_CALCULATIONS::updateCalculationsForTagShot),
33+
GeneralCommands.runWhenContinueTriggerPressed(getFeedNoteForShootingCommand())
34+
);
35+
}
36+
2237
/**
2338
* Creates a command that adjusts the shooting mechanism to aim at a target (either delivery target or speaker target), and feeds the note once the shooting mechanism is ready to shoot (all setpoints were reached).
2439
*
@@ -91,7 +106,7 @@ private static Command getFeedNoteForShootingCommand() {
91106
.alongWith(GeneralCommands.getVisualizeNoteShootingCommand()),
92107
() -> RobotContainer.SHOOTER.atTargetVelocity() &&
93108
RobotContainer.PITCHER.atTargetPitch() &&
94-
RobotContainer.SWERVE.atAngle(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle())
109+
RobotContainer.SWERVE.atAngle(new MirrorableRotation2d(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle().get().plus(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()), false))
95110
);
96111
}
97112

‎src/main/java/frc/trigon/robot/constants/CameraConstants.java‎

Lines changed: 4 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -25,20 +25,11 @@ public class CameraConstants {
2525
);
2626

2727
public static final AprilTagCamera
28-
FRONT_TAG_CAMERA = new AprilTagCamera(
29-
AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA,
30-
"FrontTagCamera",
31-
FRONT_CENTER_TO_CAMERA,
32-
THETA_STD_EXPONENT,
33-
TRANSLATIONS_STD_EXPONENT
34-
),
3528
REAR_TAG_CAMERA = new AprilTagCamera(
36-
AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA,
37-
"RearTagCamera",
38-
REAR_CENTER_TO_CAMERA,
39-
THETA_STD_EXPONENT,
40-
TRANSLATIONS_STD_EXPONENT
41-
);
29+
AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA,
30+
"RearTagCamera",
31+
REAR_CENTER_TO_CAMERA
32+
);
4233

4334
public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera");
4435
}

‎src/main/java/frc/trigon/robot/constants/RobotConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
public class RobotConstants {
88
private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE;
99
private static final double PERIODIC_TIME_SECONDS = 0.02;
10-
public static final String CANIVORE_NAME = "CANivore";
10+
public static final String CANIVORE_NAME = "SwerveCANivore";
1111
public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/";
1212

1313
public static void init() {

‎src/main/java/frc/trigon/robot/constants/ShootingConstants.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,7 @@ public class ShootingConstants {
1010
public static final double
1111
SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45,
1212
DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35,
13-
FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10,
14-
SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3;
13+
FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10;
1514

1615
public static final double
1716
CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45,

‎src/main/java/frc/trigon/robot/misc/ShootingCalculations.java‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import edu.wpi.first.math.geometry.*;
44
import edu.wpi.first.math.kinematics.ChassisSpeeds;
55
import frc.trigon.robot.RobotContainer;
6+
import frc.trigon.robot.constants.CameraConstants;
67
import frc.trigon.robot.constants.FieldConstants;
78
import frc.trigon.robot.constants.ShootingConstants;
89
import frc.trigon.robot.subsystems.pitcher.PitcherConstants;
@@ -41,6 +42,10 @@ public void updateCalculationsForSpeakerShot() {
4142
targetShootingState = calculateTargetShootingState(FieldConstants.SPEAKER_TRANSLATION, ShootingConstants.SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND, false);
4243
}
4344

45+
public void updateCalculationsForTagShot() {
46+
targetShootingState = calculateTargetShootingState(new MirrorableTranslation3d(CameraConstants.REAR_TAG_CAMERA.getTagPose().getTranslation(), false), 34, false);
47+
}
48+
4449
/**
4550
* @return the target state of the robot to shoot at the provided shooting target
4651
*/
@@ -101,7 +106,7 @@ private Translation2d predictFutureTranslation(double predictionTime) {
101106
* @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState}
102107
*/
103108
private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) {
104-
final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation();
109+
final Translation2d currentTranslation = new Translation2d();
105110
final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget);
106111
final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond);
107112
final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget);
@@ -137,7 +142,7 @@ private TargetShootingState calculateTargetShootingState(TargetShootingState sta
137142
private TargetShootingState calculateTargetShootingState(Translation3d shootingVector) {
138143
final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false);
139144
final Rotation2d targetPitch = getPitch(shootingVector);
140-
final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND;
145+
final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm());
141146

142147
return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity);
143148
}

0 commit comments

Comments
 (0)