Skip to content

Commit a3c7eb3

Browse files
sophiep29sophiep29
committed
tuning
1 parent dfd648e commit a3c7eb3

10 files changed

Lines changed: 99 additions & 488 deletions

File tree

‎.idea/modules.xml‎

Lines changed: 0 additions & 1 deletion
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,17 +4,26 @@
44

55
package frc.robot;
66

7+
import static frc.robot.config.VisionConfig.photonPoseEstimatorForward;
8+
import static frc.robot.config.VisionConfig.result;
9+
710
import edu.wpi.first.epilogue.Epilogue;
811
import edu.wpi.first.epilogue.Logged;
912
import edu.wpi.first.wpilibj.DataLogManager;
1013
import edu.wpi.first.wpilibj.DriverStation;
1114
import edu.wpi.first.wpilibj.TimedRobot;
1215
import edu.wpi.first.wpilibj2.command.Command;
1316
import edu.wpi.first.wpilibj2.command.CommandScheduler;
17+
import frc.robot.config.TunerConstants;
18+
import frc.robot.config.VisionConfig;
19+
import frc.robot.subsystems.CommandSwerveDrivetrain;
20+
import java.util.Optional;
21+
import org.photonvision.EstimatedRobotPose;
1422

1523
@Logged
1624
public class Robot extends TimedRobot {
1725
private Command m_autonomousCommand;
26+
private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
1827

1928
private final RobotContainer m_robotContainer;
2029

@@ -28,6 +37,19 @@ public Robot() {
2837
@Override
2938
public void robotPeriodic() {
3039
CommandScheduler.getInstance().run();
40+
Optional<EstimatedRobotPose> visionEst =
41+
VisionConfig.photonPoseEstimatorForward.estimateCoprocMultiTagPose(result);
42+
if (visionEst.isEmpty()) {
43+
visionEst = VisionConfig.photonPoseEstimatorForward.estimateLowestAmbiguityPose(result);
44+
} else {
45+
drivetrain.addVisionMeasurement(
46+
photonPoseEstimatorForward
47+
.estimateAverageBestTargetsPose(result)
48+
.get()
49+
.estimatedPose
50+
.toPose2d(),
51+
visionEst.get().timestampSeconds);
52+
}
3153
}
3254

3355
@Override

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

Lines changed: 31 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ public class RobotContainer {
2424
private Climb climb = new Climb();
2525
private Intake intake = new Intake();
2626
private Kicker kicker = new Kicker();
27-
private CommandXboxController controller = new CommandXboxController(0);
27+
private CommandXboxController controller = new CommandXboxController(1);
2828
private SwerveDriveState driveState = new SwerveDriveState();
2929

3030
private Command shootGroup =
@@ -52,8 +52,8 @@ private void configureBindings() {
5252
controller::getLeftY,
5353
controller::getRightX));
5454
// Flywheel
55-
flywheel.setDefaultCommand(
56-
new FlywheelCommand(flywheel, FlywheelCommand.Position.DEFAULT_SHOT, drivetrain));
55+
// flywheel.setDefaultCommand(
56+
// new FlywheelCommand(flywheel, FlywheelCommand.Position.DEFAULT_SHOT, drivetrain));
5757

5858
/* Controls */
5959
// Y = Shoot Toggle
@@ -67,31 +67,36 @@ private void configureBindings() {
6767
controller
6868
.rightStick()
6969
.and(() -> !shootGroup.isScheduled())
70-
.onTrue(new HopperCommand(hopper, HopperCommand.Position.EXTEND_RETRACT));
71-
// Right Trigger = Climb Retract
72-
controller
73-
.rightTrigger()
74-
.whileTrue(new ClimbCommand(climb, ClimbCommand.Position.DIRECTIONAL_CLIMB));
75-
// Left Trigger = Climb Extend
76-
controller
77-
.leftTrigger()
78-
.whileTrue(new ClimbCommand(climb, ClimbCommand.Position.DIRECTIONAL_UNCLIMB));
70+
.onTrue(
71+
new HopperCommand(hopper, HopperCommand.Position.EXTEND_RETRACT)
72+
.alongWith(Commands.runOnce(() -> System.out.println("Hopper Pressed"))));
73+
// // Right Trigger = Climb Retract
74+
// controller
75+
// .rightTrigger()
76+
// .whileTrue(new ClimbCommand(climb, ClimbCommand.Position.DIRECTIONAL_CLIMB));
77+
// // Left Trigger = Climb Extend
78+
// controller
79+
// .leftTrigger()
80+
// .whileTrue(new ClimbCommand(climb, ClimbCommand.Position.DIRECTIONAL_UNCLIMB));
7981
// Back button = Zero Pigeon gyro
8082
controller.back().onTrue(Commands.runOnce(() -> zeroPigeon()));
81-
// Left Bumper = Flywheel Toggle for hub shot speeds
82-
controller
83-
.leftBumper()
84-
.toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain));
85-
// Right Bumper = Flywheel Toggle for tower shot speeds
86-
controller
87-
.rightBumper()
88-
.toggleOnTrue(
89-
new FlywheelCommand(flywheel, FlywheelCommand.Position.TOWER_SHOT, drivetrain));
90-
// A = Flywheel Toggle of calculated shots
91-
controller
92-
.a()
93-
.toggleOnTrue(
94-
new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain));
83+
// // Left Bumper = Flywheel Toggle for hub shot speeds
84+
// controller
85+
// .leftBumper()
86+
// .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT,
87+
// drivetrain));
88+
// // Right Bumper = Flywheel Toggle for tower shot speeds
89+
// controller
90+
// .rightBumper()
91+
// .toggleOnTrue(
92+
// new FlywheelCommand(flywheel, FlywheelCommand.Position.TOWER_SHOT, drivetrain));
93+
// // A = Flywheel Toggle of calculated shots
94+
// controller
95+
// .a()
96+
// .toggleOnTrue(
97+
// new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT,
98+
// drivetrain));
99+
controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper));
95100
}
96101

97102
public Command getAutonomousCommand() {

‎src/main/java/frc/robot/config/FlywheelConfig.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ public class FlywheelConfig {
3131

3232
// Set
3333
public static final double FLYWHEEL_OUTTAKE_SPEED = 0;
34-
public static final double FLYWHEEL_HUB_SHOT_SPEED = 0;
34+
public static final double FLYWHEEL_HUB_SHOT_SPEED = 40;
3535
public static final double FLYWHEEL_TOWER_SHOT_SPEED = 0;
36-
public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 0;
36+
public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40;
3737
public static final double FLYWHEEL_TOLERANCE = 5;
3838
}

‎src/main/java/frc/robot/config/HopperConfig.java‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,17 +8,20 @@ public class HopperConfig {
88
// Set
99
public static final double HOPPER_MAX_CRUISE_VELOCITY = 3000;
1010
public static final double HOPPER_TARGET_ACCELERATION = 500;
11-
public static final double HOPPER_P = 0;
11+
public static final double HOPPER_P = 2;
1212
public static final double HOPPER_I = 0;
1313
public static final double HOPPER_D = 0;
14+
public static final double HOPPER_S = 0;
15+
public static final double HOPPER_V = 0;
16+
public static final double HOPPER_A = 0;
1417

15-
public static final double SLOW_HOPPER_P = 0;
18+
public static final double SLOW_HOPPER_P = 2;
1619
public static final double SLOW_HOPPER_I = 0;
1720
public static final double SLOW_HOPPER_D = 0;
1821

19-
public static final double HOPPER_TOLERANCE = 0.01;
22+
public static final double HOPPER_TOLERANCE = 0.2;
2023

2124
// Set
22-
public static final double HOPPER_EXTEND_ROTATION = 0.0;
25+
public static final double HOPPER_EXTEND_ROTATION = 3.65;
2326
public static final double HOPPER_RETRACT_ROTATION = 0.0;
2427
}

‎src/main/java/frc/robot/config/IntakeConfig.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ public class IntakeConfig {
66
public static final double INTAKE_GEAR_RATIO = 1;
77

88
// Set
9-
public static final double INTAKE_INTAKE_SPEED = 0;
9+
public static final double INTAKE_INTAKE_SPEED = 5;
1010
public static final double INTAKE_OUTTAKE_SPEED = 0;
1111
public static final double INTAKE_SLOW_SPEED = 0;
1212
}

‎src/main/java/frc/robot/config/VisionConfig.java‎

Lines changed: 27 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,29 @@
11
package frc.robot.config;
22

3-
public class VisionConfig {}
3+
import edu.wpi.first.apriltag.AprilTagFieldLayout;
4+
import edu.wpi.first.apriltag.AprilTagFields;
5+
import edu.wpi.first.math.geometry.Rotation3d;
6+
import edu.wpi.first.math.geometry.Transform3d;
7+
import edu.wpi.first.math.util.Units;
8+
import java.util.Optional;
9+
import org.photonvision.EstimatedRobotPose;
10+
import org.photonvision.PhotonPoseEstimator;
11+
import org.photonvision.targeting.PhotonPipelineResult;
12+
13+
public class VisionConfig {
14+
public static PhotonPipelineResult result = new PhotonPipelineResult();
15+
public static final String CAMERA_NAME = "forwardAprilTag"; // left
16+
public static final AprilTagFieldLayout LAYOUT =
17+
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
18+
public static final PhotonPoseEstimator.PoseStrategy STRATEGY =
19+
PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY;
20+
public static final Transform3d FORWARD_CAMERA_POSITION =
21+
new Transform3d(
22+
Units.inchesToMeters(-0.79),
23+
Units.inchesToMeters(-11.55),
24+
Units.inchesToMeters(14.88),
25+
new Rotation3d(0.0, Units.degreesToRadians(16), 0.0));
26+
Optional<EstimatedRobotPose> visionEst = Optional.empty();
27+
public static PhotonPoseEstimator photonPoseEstimatorForward =
28+
new PhotonPoseEstimator(LAYOUT, FORWARD_CAMERA_POSITION);
29+
}

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package frc.robot.subsystems;
22

33
import com.ctre.phoenix6.configs.TalonFXConfiguration;
4-
import com.ctre.phoenix6.controls.DutyCycleOut;
4+
import com.ctre.phoenix6.controls.VoltageOut;
55
import com.ctre.phoenix6.hardware.TalonFX;
66
import com.ctre.phoenix6.signals.InvertedValue;
77
import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -31,12 +31,12 @@ public Intake() {
3131

3232
public void intake(double speed) {
3333
speed = -Math.abs(speed);
34-
intake.setControl(new DutyCycleOut(speed));
34+
intake.setControl(new VoltageOut(-speed));
3535
}
3636

3737
public void outtake(double speed) {
3838
speed = Math.abs(speed);
39-
intake.setControl(new DutyCycleOut(speed));
39+
intake.setControl(new VoltageOut(speed));
4040
}
4141

4242
public void stop() {

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,10 @@
11
package frc.robot.subsystems;
22

33
import edu.wpi.first.wpilibj2.command.SubsystemBase;
4+
import frc.robot.config.VisionConfig;
5+
import org.photonvision.PhotonCamera;
6+
import org.photonvision.estimation.*;
47

5-
public class Vision extends SubsystemBase {}
8+
public class Vision extends SubsystemBase {
9+
public static final PhotonCamera FrontCameraApril = new PhotonCamera(VisionConfig.CAMERA_NAME);
10+
}

0 commit comments

Comments
 (0)