Skip to content

Commit

Permalink
Merge pull request #48 from chopshop-166/vision-refactor
Browse files Browse the repository at this point in the history 
Rotate to tag  works / organizing drive
  • Loading branch information
@bot190
bot190 authored Jun 27, 2024
2 parents 593d161 + ffa810c commit 5970197
Show file tree
Hide file tree
Showing 5 changed files with 51 additions and 89 deletions.
Binary file added photonvision-settings-export.zip
View file
Binary file not shown.
9 changes: 2 additions & 7 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,8 @@
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -31,8 +28,8 @@
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Led;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Undertaker;
import frc.robot.subsystems.Shooter.Speeds;
import frc.robot.subsystems.Undertaker;

public class Robot extends CommandRobot {

Expand Down Expand Up @@ -131,9 +128,7 @@ public void disabledInit() {

@Override
public void configureButtonBindings() {
driveController.back().onTrue(drive.resetGyroCommand());
// Magic numbers for auto testing
driveController.start().onTrue(drive.setPoseCommand(() -> drive.visionEstimator.getEstimatedPosition()));
driveController.back().onTrue(drive.resetCmd());
driveController.leftBumper()
.whileTrue(drive.robotCentricDrive(() -> {
return driveScaler.applyAsDouble(-driveController.getLeftX());
Expand Down
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/subsystems/ArmRotate.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
import com.chopshop166.chopshoplib.motors.Modifier;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -19,11 +18,7 @@

public class ArmRotate extends LoggedSubsystem<Data, ArmRotateMap> {

// Intake Position: 0 degrees <- base everything else off of it

final ProfiledPIDController pid;
// Set to zero until able to test
private boolean useAbsolute = true;
final double RAISE_SPEED = .85;
final double MANUAL_LOWER_SPEED_COEF = 0.5;
final double SLOW_DOWN_COEF = 0.5;
Expand All @@ -32,7 +27,6 @@ public class ArmRotate extends LoggedSubsystem<Data, ArmRotateMap> {
final double INTAKE_TOLERANCE = 4;
double holdAngle = 0;

private Constraints rotateConstraints = new Constraints(150, 200);
ArmPresets level = ArmPresets.OFF;

public ArmRotate(ArmRotateMap armRotateMap) {
Expand Down
124 changes: 49 additions & 75 deletions src/main/java/frc/robot/subsystems/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;

import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
import com.chopshop166.chopshoplib.logging.data.SwerveDriveData;
Expand Down Expand Up @@ -51,12 +50,10 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
double rotationCoef = 1;
double rotationKp = 0.05;
double rotationKs = 0.19;
ProfiledPIDController rotationPID = new ProfiledPIDController(0.09, 0.0, 0.0, new Constraints(240, 270));
ProfiledPIDController rotationPID = new ProfiledPIDController(0.065, 0.0, 0.0, new Constraints(240, 270));
double visionMaxError = 1;
Optional<PhotonTrackedTarget> tgt = Optional.empty();

SwerveDrivePoseEstimator estimator;
public SwerveDrivePoseEstimator visionEstimator;

// Vision objects
private PhotonCamera camera = null;
Expand All @@ -66,8 +63,8 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
// Cam mounted facing forward, half a meter forward of center, half a meter up
// from center.
public static final Transform3d kRobotToCam = new Transform3d(
new Translation3d(Units.inchesToMeters(3.029), Units.inchesToMeters(-6.9965), Units.inchesToMeters(12.445)),
new Rotation3d(0, Units.degreesToRadians(-16.875), Units.degreesToRadians(-4.5 - 180)));
new Translation3d(Units.inchesToMeters(-6.9965), Units.inchesToMeters(-3.029), Units.inchesToMeters(12.445)),
new Rotation3d(0, Units.degreesToRadians(-16.875), Units.degreesToRadians(-6.5)));

// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
Expand All @@ -93,11 +90,6 @@ public Drive(SwerveDriveMap map) {
new Pose2d(),
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.01));
visionEstimator = new SwerveDrivePoseEstimator(kinematics, getMap().gyro.getRotation2d(),
getData().getModulePositions(),
new Pose2d(),
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.01));

AutoBuilder.configureHolonomic(estimator::getEstimatedPosition, this::setPose,
this::getSpeeds, this::move, // Method that will drive the robot given ROBOT
Expand All @@ -116,8 +108,6 @@ public Drive(SwerveDriveMap map) {
public void setPose(Pose2d pose) {
estimator.resetPosition(getMap().gyro.getRotation2d(),
getData().getModulePositions(), pose);
visionEstimator.resetPosition(getMap().gyro.getRotation2d(),
getData().getModulePositions(), pose);
}

public Command setPoseCommand(Supplier<Pose2d> pose) {
Expand All @@ -129,7 +119,7 @@ public Command setPoseCommand(Supplier<Pose2d> pose) {
private void deadbandMove(final double xSpeed, final double ySpeed,
final double rotation, boolean isRobotCentric) {

var deadband = Modifier.scalingDeadband(DriverStation.isFMSAttached() ? 0.05 : 0.15);
var deadband = Modifier.scalingDeadband(0.05);
double rotationInput = deadband.applyAsDouble(rotation);
double xInput = deadband.applyAsDouble(xSpeed);
double yInput = deadband.applyAsDouble(ySpeed);
Expand Down Expand Up @@ -185,10 +175,6 @@ public Command robotCentricDrive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, D
.withName("Robot Centric Drive");
}

public Command resetGyroCommand() {
return runOnce(this::resetGyro).ignoringDisable(true);
}

public Command moveInDirection(double xSpeed, double ySpeed, double seconds) {
return run(() -> {
move(xSpeed, ySpeed, 0, false);
Expand All @@ -197,62 +183,45 @@ public Command moveInDirection(double xSpeed, double ySpeed, double seconds) {

public Command rotateToAngle(double targetAngle) {
return run(() -> {
double newTarget = targetAngle;
if (Math.abs(getMap().gyro.getRotation2d().getDegrees() - targetAngle) > 180) {
newTarget -= 360;
}
double rotationSpeed = rotationPID.calculate(getMap().gyro.getRotation2d().getDegrees(), newTarget);
rotationSpeed += Math.copySign(rotationKs, rotationSpeed);
// need to ensure we move at a fast enough speed for gyro to keep up
if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) {
move(0, 0, rotationSpeed,
false);
} else {
safeState();
}
Logger.recordOutput("rotationSpeed", rotationSpeed);
Logger.recordOutput("targetVelocity", rotationPID.getSetpoint().velocity);
rotateToAngleImpl(targetAngle);
});
}

public Translation2d getSpeakerTarget() {
Optional<Pose3d> pose;
if (isBlue) {
pose = photonEstimator.getFieldTags().getTagPose(7);
Logger.recordOutput("Alliance Speaker", "Blue");
pose = kTagLayout.getTagPose(7);
} else {
pose = photonEstimator.getFieldTags().getTagPose(4);
Logger.recordOutput("Alliance Speaker", "Red/Other");
pose = kTagLayout.getTagPose(4);
}
if (pose.isEmpty()) {
return new Translation2d();
}
return new Translation2d(pose.get().getTranslation().getX(), pose.get().getTranslation().getY());
return pose.get().getTranslation().toTranslation2d();
}

public Translation2d getRobotToTarget(Translation2d target) {
return target.minus(visionEstimator.getEstimatedPosition().getTranslation());
return target.minus(estimator.getEstimatedPosition().getTranslation());
}

public Command rotateToSpeaker() {
return rotateTo(this::getSpeakerTarget);
}

public Command rotateTo(Supplier<Translation2d> target) {
return run(() -> {
var target = getRobotToTarget(getSpeakerTarget());
double targetAngle = target.getAngle().getDegrees();
double estimatorAngle = estimator.getEstimatedPosition().getRotation().getDegrees();
Logger.recordOutput("Estimator Angle", estimatorAngle);
Logger.recordOutput("adjustedTargetAngle", targetAngle);
double rotationSpeed = rotationPID.calculate(estimatorAngle,
targetAngle);
rotationSpeed += Math.copySign(rotationKs, rotationSpeed);
// need to ensure we move at a fast enough speed for gyro to keep up
if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) {
move(0, 0, rotationSpeed,
false);
} else {
safeState();
}
Logger.recordOutput("Speaker Pose", getSpeakerTarget());
var robotToTarget = getRobotToTarget(target.get());
rotateToAngleImpl(robotToTarget.getAngle().getDegrees());
Logger.recordOutput("Target Pose", robotToTarget);
});
}

public Command rotateTo(Translation2d target) {
return rotateTo(() -> target);
}

/**
* The latest estimated robot pose on the field from vision data. This may be
* empty. This should
Expand All @@ -279,7 +248,7 @@ public Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose) {
int numTags = 0;
double avgDist = 0;
for (var tgt : targets) {
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
var tagPose = kTagLayout.getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty())
continue;
numTags++;
Expand All @@ -302,7 +271,7 @@ public Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose) {

@Override
public void reset() {
// Nothing to reset here
getMap().gyro.reset();
}

@Override
Expand All @@ -317,31 +286,36 @@ public void periodic() {
super.periodic();
isBlue = DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Blue;
estimator.update(getMap().gyro.getRotation2d(), getData().getModulePositions());
visionEstimator.update(getMap().gyro.getRotation2d(), getData().getModulePositions());
var target = getRobotToTarget(getSpeakerTarget());
double targetAngle = target.getAngle().getDegrees();

// Correct pose estimate with vision measurements
var visionEst = getEstimatedGlobalPose();
visionEst.ifPresent(
est -> {
var estPose = est.estimatedPose.toPose2d();
// Change our trust in the measurement based on the tags we can see
var estStdDevs = getEstimationStdDevs(estPose);
Logger.recordOutput("Vision Pose", est.estimatedPose);
visionEstimator.addVisionMeasurement(
est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
});

Logger.recordOutput("estimatorPose", estimator.getEstimatedPosition());
Logger.recordOutput("visionEstimatorPose", visionEstimator.getEstimatedPosition());
Logger.recordOutput("poseAngle", estimator.getEstimatedPosition().getRotation());
Logger.recordOutput("robotRotationGyro", getMap().gyro.getRotation2d());
Logger.recordOutput("targetAngle", targetAngle);
visionEst.ifPresent(est -> {
var estPose = est.estimatedPose.toPose2d();
// Change our trust in the measurement based on the tags we can see
var estStdDevs = getEstimationStdDevs(estPose);
Logger.recordOutput("Vision Pose", est.estimatedPose);
estimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
});

Logger.recordOutput("Estimator Pose", estimator.getEstimatedPosition());
Logger.recordOutput("Pose Angle", estimator.getEstimatedPosition().getRotation());
Logger.recordOutput("Robot Rotation Gyro", getMap().gyro.getRotation2d());
}

public void resetGyro() {
getMap().gyro.reset();
private void rotateToAngleImpl(double targetAngle) {
double estimatorAngle = estimator.getEstimatedPosition().getRotation().getDegrees();
double rotationSpeed = rotationPID.calculate(estimatorAngle, targetAngle);
rotationSpeed += Math.copySign(rotationKs, rotationSpeed);
// need to ensure we move at a fast enough speed for gyro to keep up
if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) {
move(0, 0, rotationSpeed, false);
} else {
safeState();
}
Logger.recordOutput("Target Angle", targetAngle);
Logger.recordOutput("Estimator Angle", estimatorAngle);
Logger.recordOutput("Rotation Speed", rotationSpeed);
Logger.recordOutput("Target Velocity", rotationPID.getSetpoint().velocity);
Logger.recordOutput("Position Error", rotationPID.getPositionError());
}
}
1 change: 0 additions & 1 deletion src/main/java/frc/robot/subsystems/Led.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import com.chopshop166.chopshoplib.leds.LEDSubsystem;
import com.chopshop166.chopshoplib.leds.patterns.AlliancePattern;
import com.chopshop166.chopshoplib.leds.patterns.ColdFirePattern;
import com.chopshop166.chopshoplib.leds.patterns.FlashPattern;
import com.chopshop166.chopshoplib.leds.patterns.RainbowRoad;
import com.chopshop166.chopshoplib.leds.patterns.SpinPattern;
Expand Down

0 comments on commit 5970197

Please sign in to comment.