Skip to content

Commit

Permalink
temp
Browse files Browse the repository at this point in the history
  • Loading branch information
@eshen7
eshen7 committed Mar 22, 2024
1 parent a757fcf commit 09ae5db
Show file tree
Hide file tree
Showing 11 changed files with 73 additions and 35 deletions.
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/commands/ClimbCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,11 @@ public class ClimbCommands {
private final Set<Subsystem> requirements;

public Command goUp() {
return Commands.run(() -> climb.openLoop(0.5), climb);
return Commands.run(() -> climb.openLoop(1), climb);
}

public Command goDown() {
return Commands.run(() -> climb.openLoop(-0.8), climb);
return Commands.run(() -> climb.openLoop(-1), climb);
}

public Command kill() {
Expand Down
10 changes: 10 additions & 0 deletions src/main/java/team3647/frc2024/commands/ShooterCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,16 @@ public Command setVelocity(DoubleSupplier bill, DoubleSupplier ratio) {
shooterLeft);
}

public Command setVelocityIndep(DoubleSupplier left, DoubleSupplier right) {
return Commands.run(
() -> {
shooterRight.setVelocity(right.getAsDouble());
shooterLeft.setVelocity(left.getAsDouble());
},
shooterRight,
shooterLeft);
}

public Command kill() {
return Commands.run(
() -> {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ public void robotInit() {
LiveWindow.disableAllTelemetry();
LiveWindow.setEnabled(false);
SignalLogger.enableAutoLogging(false);

// SignalLogger.setPath("/home/lvuser/logs/");
SignalLogger.stop();

// Instantiate our RobotContainer. This will perform all our button bindings,
Expand Down
1 change: 1 addition & 0 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -382,6 +382,7 @@ public Command getAutonomousCommand() {
new VisionController(
swerve::addVisionData,
swerve::shouldAddData,
swerve::seedFieldRelative,
coController.buttonA,
coController.buttonX,
backLeft,
Expand Down
14 changes: 6 additions & 8 deletions src/main/java/team3647/frc2024/subsystems/Pivot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import java.util.function.BooleanSupplier;
import org.littletonrobotics.junction.Logger;
import team3647.lib.TalonFXSubsystem;

public class Pivot extends TalonFXSubsystem {
Expand Down Expand Up @@ -61,12 +59,12 @@ public void setEncoder(double degree) {
@Override
public void periodic() {
super.periodic();
Logger.recordOutput(
"Pivot/Pose",
new Pose3d(
new Translation3d(
Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2 + 1.75)),
new Rotation3d(0, Units.degreesToRadians(getAngle()), 0)));
// Logger.recordOutput(
// "Pivot/Pose",
// new Pose3d(
// new Translation3d(
// Units.inchesToMeters(0.3), 0, Units.inchesToMeters(17.2 + 1.75)),
// new Rotation3d(0, Units.degreesToRadians(getAngle()), 0)));
if (underStage.getAsBoolean()) {
this.maxAngle = maxAngleUnderStage;
} else {
Expand Down
8 changes: 3 additions & 5 deletions src/main/java/team3647/frc2024/subsystems/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import team3647.frc2024.commands.PivotCommands;
import team3647.frc2024.commands.ShooterCommands;
import team3647.frc2024.commands.WristCommands;
import team3647.frc2024.constants.ShooterConstants;

public class Superstructure {

Expand Down Expand Up @@ -114,8 +113,7 @@ public boolean isClimbing() {
}

public Command spinUp() {
return shooterCommands.setVelocity(
() -> getDesiredSpeed(), () -> ShooterConstants.kLeftRatio);
return shooterCommands.setVelocityIndep(() -> 28, () -> 18);
}

public Command spinUpTrap() {
Expand All @@ -137,7 +135,7 @@ public Command stowChurro() {
}

public Command spinUpAmp() {
return shooterCommands.setVelocity(() -> 3.5, () -> 1);
return shooterCommands.setVelocity(() -> 5, () -> 1);
}

public double getDesiredSpeed() {
Expand Down Expand Up @@ -173,7 +171,7 @@ public Command ejectPiece() {
}

public boolean flywheelReadY() {
return shooterLeft.velocityReached(15, 1);
return shooterRight.velocityGreater(27.5);
}

public boolean pivotReady() {
Expand Down
29 changes: 25 additions & 4 deletions src/main/java/team3647/frc2024/subsystems/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,7 +268,7 @@ public Command runSteerDynamTest(SysIdRoutine.Direction direction) {
}

public void setRobotPose(Pose2d pose) {
m_odometry.resetPosition(this.m_pigeon2.getRotation2d(), getModulePositions(), pose);
seedFieldRelative(pose);
periodicIO = new PeriodicIO();
}

Expand Down Expand Up @@ -327,8 +327,20 @@ public Pose2d getOdoPose() {
}

public void setStuff(SwerveDriveState state) {
// SignalLogger.writeDoubleArray(
// "odometry",
// new double[] {
// state.Pose.getX(), state.Pose.getY(), state.Pose.getRotation().getDegrees()
// });
periodicIO.pose = state.Pose;
periodicIO.speeds = this.m_kinematics.toChassisSpeeds(state.ModuleStates);
// SignalLogger.writeDoubleArray(
// "speeds",
// new double[] {
// periodicIO.speeds.vxMetersPerSecond,
// periodicIO.speeds.vyMetersPerSecond,
// periodicIO.speeds.omegaRadiansPerSecond
// });
}

public Rotation2d getOdoRot() {
Expand Down Expand Up @@ -366,9 +378,10 @@ public ChassisSpeeds getFieldRelativeChassisSpeeds() {
return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getOdoRot());
}

public boolean shouldAddData(Pose2d visionPose) {
double distance = visionPose.minus(getOdoPose()).getTranslation().getNorm();
double angle = visionPose.getRotation().minus(getOdoPose().getRotation()).getDegrees();
public boolean shouldAddData(VisionMeasurement measurement) {
double distance = measurement.pose.minus(getOdoPose()).getTranslation().getNorm();
double angle =
measurement.pose.getRotation().minus(getOdoPose().getRotation()).getDegrees();
return (distance < 1.5 && Math.abs(angle) < 15) || DriverStation.isAutonomous()
? true
: false;
Expand All @@ -377,6 +390,14 @@ public boolean shouldAddData(Pose2d visionPose) {
public void addVisionData(VisionMeasurement data) {
periodicIO.visionPose = data.pose;
SmartDashboard.putNumber("timestamped viison", data.timestamp);
// SignalLogger.writeDoubleArray(
// "vision pose",
// new double[] {
// data.pose.getX(),
// data.pose.getY(),
// data.pose.getRotation().getDegrees(),
// data.timestamp
// });
addVisionMeasurement(data.pose, data.timestamp, data.stdDevs);
}

Expand Down
15 changes: 5 additions & 10 deletions src/main/java/team3647/frc2024/subsystems/Wrist.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,6 @@

import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import org.littletonrobotics.junction.Logger;
import team3647.frc2024.util.InverseKinematics;
import team3647.lib.TalonFXSubsystem;

Expand Down Expand Up @@ -38,11 +33,11 @@ public void setEncoder(double degree) {
@Override
public void periodic() {
super.periodic();
Logger.recordOutput(
"Wrist/Pose",
new Pose3d(
new Translation3d(0.28, 0, 0.18),
new Rotation3d(0, -Units.degreesToRadians(getAngle()), 0)));
// Logger.recordOutput(
// "Wrist/Pose",
// new Pose3d(
// new Translation3d(0.28, 0, 0.18),
// new Rotation3d(0, -Units.degreesToRadians(getAngle()), 0)));
// Logger.recordOutput(
// "Intake/Pose",
// new Pose3d(
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ public Optional<VisionMeasurement> QueueToInputs() {
if (targetDistance > 4 && !hasPriority) {
return Optional.empty();
}
if (targetDistance > 10) {
if (targetDistance > 8) {
return Optional.empty();
}
if (Math.abs(update.get().estimatedPose.getZ()) > 0.5) {
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/team3647/frc2024/util/AutoDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,10 @@ public double getRot() {
rotController.calculate(targetRot, 0)
* MathUtil.clamp(
Math.abs(swerve.getChassisSpeeds().vyMetersPerSecond), 1, 100);
double setpoint = Math.abs(targetRot) < 0.01 ? 0 : k;
if (Math.abs(k) < 0.05) {
k *= 3;
}
double setpoint = Math.abs(targetRot) < 0.002 ? 0 : k;
return setpoint;
}

Expand Down
18 changes: 15 additions & 3 deletions src/main/java/team3647/frc2024/util/VisionController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,18 +13,22 @@
public class VisionController extends VirtualSubsystem {
private final AprilTagCamera[] cameras;
private final Consumer<VisionMeasurement> botPoseAcceptor;
private final Function<Pose2d, Boolean> shouldAddData;
private final Consumer<Pose2d> resetPose;
private final Function<VisionMeasurement, Boolean> shouldAddData;
private final ArrayList<VisionMeasurement> list = new ArrayList<>();
private final BooleanSupplier dataAddOverride;
private final BooleanSupplier turnOffVision;
private int count;

public VisionController(
Consumer<VisionMeasurement> visionAcceptor,
Function<Pose2d, Boolean> shouldAddData,
Function<VisionMeasurement, Boolean> shouldAddData,
Consumer<Pose2d> resetPose,
BooleanSupplier dataAddOverride,
BooleanSupplier turnOffVision,
AprilTagCamera... cameras) {
this.cameras = cameras;
this.resetPose = resetPose;
this.shouldAddData = shouldAddData;
this.botPoseAcceptor = visionAcceptor;
this.dataAddOverride = dataAddOverride;
Expand All @@ -46,8 +50,16 @@ public void periodic() {

var getInputs = inputs.get();

if (shouldAddData.apply(getInputs.pose) || dataAddOverride.getAsBoolean()) {
if (shouldAddData.apply(getInputs) || dataAddOverride.getAsBoolean()) {
list.add(getInputs);
count = 0;
} else {
count++;
if (count > 10) {
resetPose.accept(getInputs.pose);
Logger.recordOutput("Robot/Reset", getInputs.pose);
break;
}
}

// Logger.recordOutput("Robot/" + camera.getName(), getInputs.pose);
Expand Down

0 comments on commit 09ae5db

Please sign in to comment.