-
Notifications
You must be signed in to change notification settings - Fork 0
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
base: main
Are you sure you want to change the base?
Vision alignment #80
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
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; | ||
private final PIDController turnPID; | ||
PhotonCamera recognitionCamera; | ||
|
||
PhotonPipelineResult result; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Doesn't have to be a state? |
||
boolean hasTargets; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe 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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I'd use a There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Maybe after #76 gets tuned / merged this can use the |
||
addRequirements(driveSubsystem); | ||
} | ||
|
||
@Override | ||
public void initialize() { | ||
} | ||
|
||
@Override | ||
public void execute() { | ||
result = recognitionCamera.getLatestResult(); | ||
hasTargets = result.hasTargets(); | ||
target = result.getBestTarget(); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. What does this object look like if |
||
|
||
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; | ||
} | ||
} |
There was a problem hiding this comment.
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.