Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Vision alignment #80

Draft
wants to merge 4 commits into
base: main
Choose a base branch
from
Draft
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -32,6 +32,7 @@
import frc.robot.commands.balancing.BaseBalancerCommand;
import frc.robot.commands.balancing.DefaultBalancerCommand;
import frc.robot.commands.dropping.DropperChooserCommand;
import frc.robot.commands.grabber.VisionAlignmentCommand;
import frc.robot.controllers.BaseDriveController;
import frc.robot.controllers.DualJoystickDriveController;
import frc.robot.controllers.TwistJoystickDriveController;
@@ -100,7 +101,7 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
driveController = new DualJoystickDriveController();
driveController = new XboxDriveController();

photonWrapper = new PhotonWrapper();
switchableCamera = new SwitchableCamera(shuffleboardTab);
@@ -141,6 +142,7 @@ public RobotContainer() {
autonChooser.addOption("Box auton", new BoxAutonSequence(swerveSubsystem));
autonChooser.addOption("No-stopping box auton", new ContinuousBoxAutonSequence(swerveSubsystem));
autonChooser.addOption("GRT path", new GRTAutonSequence(swerveSubsystem));
autonChooser.addOption("Vision align auton", new VisionAlignmentCommand(swerveSubsystem));
}

shuffleboardTab.add("Auton", autonChooser)
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
package frc.robot.commands.auton.test;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

import frc.robot.commands.grabber.VisionAlignmentCommand;
import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;

public class AutoAlignmentAuton extends SequentialCommandGroup {
public AutoAlignmentAuton(BaseSwerveSubsystem swerveSubsystem) {
addRequirements(swerveSubsystem);
addCommands(
new VisionAlignmentCommand(swerveSubsystem)
);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
package frc.robot.commands.grabber;

import org.photonvision.PhotonCamera;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.CommandBase;

import frc.robot.subsystems.RollerSubsystem;
import frc.robot.subsystems.RollerSubsystem.HeldPiece;
import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
import frc.robot.vision.PhotonWrapper;

/**
* Intakes 1 game piece with the roller mech. This command runs the rollers at a set power
* until the limit switch is triggered.
*/
public class VisionAlignmentCommand extends CommandBase {
private final BaseSwerveSubsystem driveSubsystem;

private double angularPower;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also doesn't have to be a state; you only need to store things in fields if you need to reference them across multiple methods or you need the previous value when rerunning the same method.

private final PIDController turnPID;
PhotonCamera recognitionCamera;

PhotonPipelineResult result;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Doesn't have to be a state?

boolean hasTargets;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also doesn't have to be a state

PhotonTrackedTarget target;

public VisionAlignmentCommand(BaseSwerveSubsystem driveSubsystem){
this.driveSubsystem = driveSubsystem;

recognitionCamera = new PhotonCamera("Microsoft_LifeCam_HD-3000");
turnPID = new PIDController(0.2/35, 0.0, 0.0);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd use a ProfiledPIDController for this; see FollowPathCommand.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe after #76 gets tuned / merged this can use the setDrivePowersWithHeadingLock() method and not have to implement its own PID as well.

addRequirements(driveSubsystem);
}

@Override
public void initialize() {
}

@Override
public void execute() {
result = recognitionCamera.getLatestResult();
hasTargets = result.hasTargets();
target = result.getBestTarget();
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What does this object look like if hasTargets is false? Does the command end execution at all if that happens?


if(hasTargets){
angularPower = turnPID.calculate(target.getYaw(), 0.0);
}
else{
angularPower = 0.0;
}

driveSubsystem.setDrivePowers(0.1, 0.0, angularPower, true);
}

@Override
public void end(boolean interrupted) {
System.out.println("Intake done");
}

@Override
public boolean isFinished() {
return Math.abs(target.getYaw()) <= 1.0;
}
}