Skip to content

Commit

Permalink
Merge pull request #87 from DeepBlueRobotics/intake-ll-fixes
Browse files Browse the repository at this point in the history 
Intake ll fixes
  • Loading branch information
@ProfessorAtomicManiac
ProfessorAtomicManiac authored Jul 12, 2024
2 parents 258806b + 3212cab commit 1f4a584
Show file tree
Hide file tree
Showing 9 changed files with 184 additions and 259 deletions.
6 changes: 3 additions & 3 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -394,14 +394,14 @@ public static final class Limelightc {
// to represent unreliable heading
public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity


public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
public static final double ERROR_TOLERANCE_RAD = 0.1;
public static final double HORIZONTAL_FOV_DEG = 0; // unused
public static final double RESOLUTION_WIDTH_PIX = 640; // unused
public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446;
public static final double MOUNT_ANGLE_DEG_INTAKE = -29;
public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891);
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52);
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE =
Units.inchesToMeters(13);
public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused
public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused
public static final double MIN_MOVEMENT_METERSPSEC = 0.5;
Expand Down
17 changes: 10 additions & 7 deletions src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,17 +181,20 @@ private void setBindingsDriver() {
new JoystickButton(driverController, Driver.resetFieldOrientationButton)
.onTrue(new InstantCommand(drivetrain::resetFieldOrientation));
// axisTrigger(driverController, Axis.kRightTrigger)
// .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
// new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)));
new POVButton(driverController, 0).whileTrue(
new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight));
// .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"),
// new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)));

new POVButton(driverController, 0)
.whileTrue(new ParallelCommandGroup(new Intake(intakeShooter),
new AutoMATICALLYGetNote(drivetrain, limelight)));

axisTrigger(driverController, Axis.kLeftTrigger)
// .onTrue(new AlignToApriltag(drivetrain, limelight));
.onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false)))
.onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true)));

axisTrigger(driverController, Manipulator.SHOOTER_BUTTON)
.whileTrue(new AlignToApriltagMegatag2(drivetrain, limelight));
.whileTrue(new AlignToApriltag(drivetrain, limelight));
new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue(
new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain));
new JoystickButton(driverController, Driver.rotateFieldRelative90Deg)
Expand Down Expand Up @@ -346,9 +349,9 @@ private void registerAutoCommands() {
NamedCommands.registerCommand("PassToOuttake",
new PassToOuttake(intakeShooter));
NamedCommands.registerCommand("AimArmSpeakerMT2",
new AimArmSpeakerMT2(arm, limelight));
new AimArmSpeaker(arm, limelight));
NamedCommands.registerCommand("AlignToAprilTagMegaTag2",
new AlignToApriltagMegatag2(drivetrain, limelight));
new AlignToApriltag(drivetrain, limelight));



Expand Down
62 changes: 30 additions & 32 deletions src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,36 +9,34 @@
import edu.wpi.first.wpilibj2.command.Command;

public class AimArmSpeaker extends Command {
private final Arm arm;
private final Limelight ll;

/** Creates a new AimOuttakeSpeaker. */
public AimArmSpeaker(Arm arm, Limelight ll) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(this.arm = arm);
this.ll = ll;
}


// Called when the command is initially scheduled.
@Override
public void initialize() {
double goal = ll.getOptimizedArmAngleRadsMT2();
arm.setArmTarget(goal);
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return arm.armAtSetpoint();
}
private final Arm arm;
private final Limelight ll;

/** Creates a new AimOuttakeSpeaker. */
public AimArmSpeaker(Arm arm, Limelight ll) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(this.arm = arm);
this.ll = ll;
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
double goal = ll.getOptimizedArmAngleRadsMT2();
arm.setArmTarget(goal);
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return arm.armAtSetpoint();
}
}
44 changes: 0 additions & 44 deletions src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java
View file

This file was deleted.

69 changes: 59 additions & 10 deletions src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

public class AlignToApriltag extends Command {
Expand All @@ -21,34 +22,82 @@ public class AlignToApriltag extends Command {
public final Drivetrain drivetrain;
private Limelight limelight;

public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
thetaPIDController[2]);
public final PIDController rotationPID = new PIDController(
SmartDashboard.getNumber("apriltag align kp",
thetaPIDController[0]),
SmartDashboard.getNumber("apriltag align ki",
thetaPIDController[1]),
SmartDashboard.getNumber("apriltag align kd",
thetaPIDController[2]));

public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
this.limelight = limelight;
this.limelight = limelight;
this.drivetrain = drivetrain;
this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();

rotationPID.enableContinuousInput(-180, 180);
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
.plus(Rotation2d.fromRadians(
limelight.getRotateAngleRadMT2()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
-180, 180));
rotationPID.setTolerance(
SmartDashboard.getNumber("apriltag align pos tolerance",
positionTolerance[2]),
SmartDashboard.getNumber("apriltag align vel tolerance",
velocityTolerance[2]));
SendableRegistry.addChild(this, rotationPID);
addRequirements(drivetrain);
}

@Override
public void execute() {
double kp = SmartDashboard.getNumber("apriltag align kp",
rotationPID.getP());
double ki = SmartDashboard.getNumber("apriltag align ki",
rotationPID.getI());
double kd = SmartDashboard.getNumber("apriltag align kd",
rotationPID.getD());

if (kp != rotationPID.getP())
rotationPID.setP(kp);
if (ki != rotationPID.getI())
rotationPID.setI(ki);
if (kd != rotationPID.getD())
rotationPID.setD(kd);

double posTolerance =
SmartDashboard.getNumber("apriltag align pos tolerance",
rotationPID.getPositionTolerance());
double velTolerance =
SmartDashboard.getNumber("apriltag align vel tolerance",
rotationPID.getVelocityTolerance());

if (posTolerance != rotationPID.getPositionTolerance()
|| velTolerance != rotationPID.getVelocityTolerance())
rotationPID.setTolerance(posTolerance, velTolerance);

SmartDashboard.putNumber("apriltag align pos error (rad)",
rotationPID.getPositionError());
SmartDashboard.putNumber("apriltag align vel error (rad/s)",
rotationPID.getVelocityError());

Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
.plus(Rotation2d.fromRadians(
limelight.getRotateAngleRadMT2()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
-180, 180));
double rotationDrive = rotationPID.calculate(drivetrain.getHeading());
if (!limelight.seesTag()) {
rotationDrive = 0;
}

if (teleopDrive == null)
drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
drivetrain.drive(0, 0, rotationDrive);
else {
double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
rotationPID.calculate(drivetrain.getHeading()));
rotationDrive);
}
}

Expand Down
70 changes: 0 additions & 70 deletions src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java
View file

This file was deleted.

Loading

0 comments on commit 1f4a584

Please sign in to comment.