Skip to content

Commit 1f4a584

Browse files
ProfessorAtomicManiacProfessorAtomicManiac
authored
Merge pull request #87 from DeepBlueRobotics/intake-ll-fixes
Intake ll fixes
2 parents 258806b + 3212cab commit 1f4a584

File tree

9 files changed

+184
-259
lines changed

9 files changed

+184
-259
lines changed

‎src/main/java/org/carlmontrobotics/Constants.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -394,14 +394,14 @@ public static final class Limelightc {
394394
// to represent unreliable heading
395395
public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity
396396

397-
398-
public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
397+
public static final double ERROR_TOLERANCE_RAD = 0.1;
399398
public static final double HORIZONTAL_FOV_DEG = 0; // unused
400399
public static final double RESOLUTION_WIDTH_PIX = 640; // unused
401400
public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446;
402401
public static final double MOUNT_ANGLE_DEG_INTAKE = -29;
403402
public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891);
404-
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52);
403+
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE =
404+
Units.inchesToMeters(13);
405405
public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused
406406
public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused
407407
public static final double MIN_MOVEMENT_METERSPSEC = 0.5;

‎src/main/java/org/carlmontrobotics/RobotContainer.java‎

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -181,17 +181,20 @@ private void setBindingsDriver() {
181181
new JoystickButton(driverController, Driver.resetFieldOrientationButton)
182182
.onTrue(new InstantCommand(drivetrain::resetFieldOrientation));
183183
// axisTrigger(driverController, Axis.kRightTrigger)
184-
// .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
185-
// new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)));
186-
new POVButton(driverController, 0).whileTrue(
187-
new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight));
184+
// .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
185+
// new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)));
186+
187+
new POVButton(driverController, 0)
188+
.whileTrue(new ParallelCommandGroup(new Intake(intakeShooter),
189+
new AutoMATICALLYGetNote(drivetrain, limelight)));
190+
188191
axisTrigger(driverController, Axis.kLeftTrigger)
189192
// .onTrue(new AlignToApriltag(drivetrain, limelight));
190193
.onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false)))
191194
.onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true)));
192195

193196
axisTrigger(driverController, Manipulator.SHOOTER_BUTTON)
194-
.whileTrue(new AlignToApriltagMegatag2(drivetrain, limelight));
197+
.whileTrue(new AlignToApriltag(drivetrain, limelight));
195198
new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue(
196199
new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain));
197200
new JoystickButton(driverController, Driver.rotateFieldRelative90Deg)
@@ -346,9 +349,9 @@ private void registerAutoCommands() {
346349
NamedCommands.registerCommand("PassToOuttake",
347350
new PassToOuttake(intakeShooter));
348351
NamedCommands.registerCommand("AimArmSpeakerMT2",
349-
new AimArmSpeakerMT2(arm, limelight));
352+
new AimArmSpeaker(arm, limelight));
350353
NamedCommands.registerCommand("AlignToAprilTagMegaTag2",
351-
new AlignToApriltagMegatag2(drivetrain, limelight));
354+
new AlignToApriltag(drivetrain, limelight));
352355

353356

354357

‎src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java‎

Lines changed: 30 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -9,36 +9,34 @@
99
import edu.wpi.first.wpilibj2.command.Command;
1010

1111
public class AimArmSpeaker extends Command {
12-
private final Arm arm;
13-
private final Limelight ll;
14-
15-
/** Creates a new AimOuttakeSpeaker. */
16-
public AimArmSpeaker(Arm arm, Limelight ll) {
17-
// Use addRequirements() here to declare subsystem dependencies.
18-
addRequirements(this.arm = arm);
19-
this.ll = ll;
20-
}
21-
22-
23-
// Called when the command is initially scheduled.
24-
@Override
25-
public void initialize() {
26-
double goal = ll.getOptimizedArmAngleRadsMT2();
27-
arm.setArmTarget(goal);
28-
}
29-
30-
// Called every time the scheduler runs while the command is scheduled.
31-
@Override
32-
public void execute() {
33-
}
34-
35-
// Called once the command ends or is interrupted.
36-
@Override
37-
public void end(boolean interrupted) {}
38-
39-
// Returns true when the command should end.
40-
@Override
41-
public boolean isFinished() {
42-
return arm.armAtSetpoint();
43-
}
12+
private final Arm arm;
13+
private final Limelight ll;
14+
15+
/** Creates a new AimOuttakeSpeaker. */
16+
public AimArmSpeaker(Arm arm, Limelight ll) {
17+
// Use addRequirements() here to declare subsystem dependencies.
18+
addRequirements(this.arm = arm);
19+
this.ll = ll;
20+
}
21+
22+
// Called when the command is initially scheduled.
23+
@Override
24+
public void initialize() {
25+
double goal = ll.getOptimizedArmAngleRadsMT2();
26+
arm.setArmTarget(goal);
27+
}
28+
29+
// Called every time the scheduler runs while the command is scheduled.
30+
@Override
31+
public void execute() {}
32+
33+
// Called once the command ends or is interrupted.
34+
@Override
35+
public void end(boolean interrupted) {}
36+
37+
// Returns true when the command should end.
38+
@Override
39+
public boolean isFinished() {
40+
return arm.armAtSetpoint();
41+
}
4442
}

‎src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java‎

Lines changed: 0 additions & 44 deletions
This file was deleted.

‎src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java‎

Lines changed: 59 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
import edu.wpi.first.math.controller.PIDController;
1414
import edu.wpi.first.math.geometry.Rotation2d;
1515
import edu.wpi.first.util.sendable.SendableRegistry;
16+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1617
import edu.wpi.first.wpilibj2.command.Command;
1718

1819
public class AlignToApriltag extends Command {
@@ -21,34 +22,82 @@ public class AlignToApriltag extends Command {
2122
public final Drivetrain drivetrain;
2223
private Limelight limelight;
2324

24-
public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
25-
thetaPIDController[2]);
25+
public final PIDController rotationPID = new PIDController(
26+
SmartDashboard.getNumber("apriltag align kp",
27+
thetaPIDController[0]),
28+
SmartDashboard.getNumber("apriltag align ki",
29+
thetaPIDController[1]),
30+
SmartDashboard.getNumber("apriltag align kd",
31+
thetaPIDController[2]));
2632

2733
public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
28-
this.limelight = limelight;
34+
this.limelight = limelight;
2935
this.drivetrain = drivetrain;
3036
this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();
3137

3238
rotationPID.enableContinuousInput(-180, 180);
3339
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
34-
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
35-
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
36-
rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
40+
.plus(Rotation2d.fromRadians(
41+
limelight.getRotateAngleRadMT2()));
42+
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
43+
-180, 180));
44+
rotationPID.setTolerance(
45+
SmartDashboard.getNumber("apriltag align pos tolerance",
46+
positionTolerance[2]),
47+
SmartDashboard.getNumber("apriltag align vel tolerance",
48+
velocityTolerance[2]));
3749
SendableRegistry.addChild(this, rotationPID);
3850
addRequirements(drivetrain);
3951
}
4052

4153
@Override
4254
public void execute() {
55+
double kp = SmartDashboard.getNumber("apriltag align kp",
56+
rotationPID.getP());
57+
double ki = SmartDashboard.getNumber("apriltag align ki",
58+
rotationPID.getI());
59+
double kd = SmartDashboard.getNumber("apriltag align kd",
60+
rotationPID.getD());
61+
62+
if (kp != rotationPID.getP())
63+
rotationPID.setP(kp);
64+
if (ki != rotationPID.getI())
65+
rotationPID.setI(ki);
66+
if (kd != rotationPID.getD())
67+
rotationPID.setD(kd);
68+
69+
double posTolerance =
70+
SmartDashboard.getNumber("apriltag align pos tolerance",
71+
rotationPID.getPositionTolerance());
72+
double velTolerance =
73+
SmartDashboard.getNumber("apriltag align vel tolerance",
74+
rotationPID.getVelocityTolerance());
75+
76+
if (posTolerance != rotationPID.getPositionTolerance()
77+
|| velTolerance != rotationPID.getVelocityTolerance())
78+
rotationPID.setTolerance(posTolerance, velTolerance);
79+
80+
SmartDashboard.putNumber("apriltag align pos error (rad)",
81+
rotationPID.getPositionError());
82+
SmartDashboard.putNumber("apriltag align vel error (rad/s)",
83+
rotationPID.getVelocityError());
84+
4385
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
44-
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
45-
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
86+
.plus(Rotation2d.fromRadians(
87+
limelight.getRotateAngleRadMT2()));
88+
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
89+
-180, 180));
90+
double rotationDrive = rotationPID.calculate(drivetrain.getHeading());
91+
if (!limelight.seesTag()) {
92+
rotationDrive = 0;
93+
}
94+
4695
if (teleopDrive == null)
47-
drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
96+
drivetrain.drive(0, 0, rotationDrive);
4897
else {
4998
double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
5099
drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
51-
rotationPID.calculate(drivetrain.getHeading()));
100+
rotationDrive);
52101
}
53102
}
54103

‎src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java‎

Lines changed: 0 additions & 70 deletions
This file was deleted.

0 commit comments

Comments
 (0)