Skip to content

Commit

Permalink
[#475] - more cleanup on chase pieces and added tuning
Browse files Browse the repository at this point in the history
HeeistHo committed Mar 21, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 7889811 commit 2e620e0
Showing 1 changed file with 71 additions and 47 deletions.
118 changes: 71 additions & 47 deletions src/main/java/frc/robot/command/ChasePieces.java
Original file line number Diff line number Diff line change
@@ -18,6 +18,8 @@
import frc.robot.subsystems.Indexer;
import frc.robot.subsystems.Pivot;
import frc.robot.subsystems.Limelights;
import frc.thunder.LightningContainer;
import frc.thunder.shuffleboard.LightningShuffleboard;
import frc.thunder.vision.Limelight;

public class ChasePieces extends Command {
@@ -148,55 +150,14 @@ public void execute() {

pidOutput = headingController.calculate(0, targetHeading);

if (!DriverStation.isFMSAttached()){
debugging();
}

if (DriverStation.isAutonomousEnabled()) {
collectPower = maxCollectPower;
if (!hasPiece) {
if (hasTarget) {
if (trustValues()) {
hasSeenTarget = true;
if (!onTarget) {
drivetrain.setRobot(drivePower, 0, -pidOutput);
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
} else {
if (!hasSeenTarget) {
if (drivetrain.getPose().getY() > VisionConstants.HALF_FIELD_HEIGHT) {
if (DriverStation.getAlliance().get() == Alliance.Blue) {
drivetrain.setRobot(0, 0, -rotPower);
} else {
drivetrain.setRobot(0, 0, rotPower);
}
} else {
if (DriverStation.getAlliance().get() == Alliance.Blue) {
drivetrain.setRobot(0.5, 0, rotPower);
} else {
drivetrain.setRobot(0.5, 0, -rotPower);
}
}
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
}
autoChase();
} else {
if (!hasPiece) {
if (hasTarget) {
if (trustValues()) {
collectPower = maxCollectPower;
if (!onTarget) {
drivetrain.setRobot(drivePower, 0, -pidOutput);
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
} else {
drivetrain.setRobot(0, 0, 0);
}
teleopChase();
}

updateLogging();
@@ -219,6 +180,69 @@ private void updateLogging() {
maxCollectPowerLog.append(maxCollectPower);
}

private void autoChase(){
collectPower = maxCollectPower;
if (!hasPiece) {
if (hasTarget) {
if (trustValues()) {
hasSeenTarget = true;
if (!onTarget) {
drivetrain.setRobot(drivePower, 0, -pidOutput);
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
} else {
if (!hasSeenTarget) {
if (drivetrain.getPose().getY() > VisionConstants.HALF_FIELD_HEIGHT) {
if (DriverStation.getAlliance().get() == Alliance.Blue) {
drivetrain.setRobot(0, 0, -rotPower);
} else {
drivetrain.setRobot(0, 0, rotPower);
}
} else {
if (DriverStation.getAlliance().get() == Alliance.Blue) {
drivetrain.setRobot(0.5, 0, rotPower);
} else {
drivetrain.setRobot(0.5, 0, -rotPower);
}
}
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
}
}

private void teleopChase(){
if (!hasPiece) {
if (hasTarget) {
if (trustValues()) {
collectPower = maxCollectPower;
if (!onTarget) {
drivetrain.setRobot(drivePower, 0, -pidOutput);
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
}
} else {
drivetrain.setRobot(drivePower, 0, 0);
}
} else {
drivetrain.setRobot(0, 0, 0);
}
}

private void debugging(){
headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", headingController.getP()));
headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", headingController.getI()));
headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", headingController.getD()));

drivePower = LightningShuffleboard.getDouble("ChasePieces", "Drive Power", drivePower);
rotPower = LightningShuffleboard.getDouble("ChasePieces", "Rot Power", rotPower);
maxCollectPower = LightningShuffleboard.getDouble("ChasePieces", "Max Collect Power", maxCollectPower);
}

@Override
public void end(boolean interrupted) {
collectPower = 0d;

0 comments on commit 2e620e0

Please sign in to comment.